Skip to content

Commit

Permalink
Precalculate external mag rotation matrix
Browse files Browse the repository at this point in the history 
Introduce rotateByMatrix(), which rotates a vector by the given
matrix. Initialize the matrix on mag initialization, so it's not
recalculated on every mag reading.
  • Loading branch information
@fiam
fiam committed Feb 25, 2018
1 parent 1a4a3e8 commit 4a55873
Show file tree
Hide file tree
Showing 4 changed files with 47 additions and 31 deletions.
16 changes: 10 additions & 6 deletions src/main/common/maths.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -244,20 +244,24 @@ void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
matrix[2][Z] = cosy * cosx;
}

// Rotate a vector *v by the euler angles defined by the 3-vector *delta.
void rotateV(struct fp_vector *v, fp_angles_t *delta)
void rotateByMatrix(struct fp_vector *v, float matrix[3][3])
{
struct fp_vector v_tmp = *v;

float matrix[3][3];

buildRotationMatrix(delta, matrix);

v->X = v_tmp.X * matrix[0][X] + v_tmp.Y * matrix[1][X] + v_tmp.Z * matrix[2][X];
v->Y = v_tmp.X * matrix[0][Y] + v_tmp.Y * matrix[1][Y] + v_tmp.Z * matrix[2][Y];
v->Z = v_tmp.X * matrix[0][Z] + v_tmp.Y * matrix[1][Z] + v_tmp.Z * matrix[2][Z];
}

// Rotate a vector *v by the euler angles defined by the 3-vector *delta.
void rotateV(struct fp_vector *v, fp_angles_t *delta)
{
float matrix[3][3];

buildRotationMatrix(delta, matrix);
rotateByMatrix(v, matrix);
}

// Quick median filter implementation
// (c) N. Devillard - 1998
// http://ndevilla.free.fr/median/median.pdf
Expand Down
1 change: 1 addition & 0 deletions src/main/common/maths.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +153,7 @@ void normalizeV(struct fp_vector *src, struct fp_vector *dest);

void rotateV(struct fp_vector *v, fp_angles_t *delta);
void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3]);
void rotateByMatrix(struct fp_vector *v, float matrix[3][3]);

int32_t wrap_18000(int32_t angle);
int32_t wrap_36000(int32_t angle);
Expand Down
6 changes: 5 additions & 1 deletion src/main/drivers/compass/compass.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,11 @@ typedef struct magDev_s {
busDevice_t * busDev;
sensorMagInitFuncPtr init; // initialize function
sensorMagReadFuncPtr read; // read 3 axis data function
sensor_align_e magAlign;
struct {
bool useExternal;
float externalRotationMatrix[3][3];
sensor_align_e onBoard;
} magAlign;
uint8_t magSensorToUse;
int16_t magADCRaw[XYZ_AXIS_COUNT];
} magDev_t;
55 changes: 31 additions & 24 deletions src/main/sensors/compass.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,8 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
magSensor_e magHardware = MAG_NONE;
requestedSensors[SENSOR_INDEX_MAG] = magHardwareToUse;

dev->magAlign = ALIGN_DEFAULT;
dev->magAlign.useExternal = false;
dev->magAlign.onBoard = ALIGN_DEFAULT;

switch (magHardwareToUse) {
case MAG_AUTODETECT:
Expand All @@ -98,7 +99,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
#ifdef USE_MAG_QMC5883
if (qmc5883Detect(dev)) {
#ifdef MAG_QMC5883L_ALIGN
dev->magAlign = MAG_QMC5883L_ALIGN;
dev->magAlign.onBoard = MAG_QMC5883L_ALIGN;
#endif
magHardware = MAG_QMC5883;
break;
Expand All @@ -114,7 +115,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
#ifdef USE_MAG_HMC5883
if (hmc5883lDetect(dev)) {
#ifdef MAG_HMC5883_ALIGN
dev->magAlign = MAG_HMC5883_ALIGN;
dev->magAlign.onBoard = MAG_HMC5883_ALIGN;
#endif
magHardware = MAG_HMC5883;
break;
Expand All @@ -130,7 +131,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
#ifdef USE_MAG_AK8975
if (ak8975Detect(dev)) {
#ifdef MAG_AK8975_ALIGN
dev->magAlign = MAG_AK8975_ALIGN;
dev->magAlign.onBoard = MAG_AK8975_ALIGN;
#endif
magHardware = MAG_AK8975;
break;
Expand All @@ -146,7 +147,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
#ifdef USE_MAG_AK8963
if (ak8963Detect(dev)) {
#ifdef MAG_AK8963_ALIGN
dev->magAlign = MAG_AK8963_ALIGN;
dev->magAlign.onBoard = MAG_AK8963_ALIGN;
#endif
magHardware = MAG_AK8963;
break;
Expand All @@ -162,7 +163,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
#ifdef USE_GPS
if (gpsMagDetect(dev)) {
#ifdef MAG_GPS_ALIGN
dev->magAlign = MAG_GPS_ALIGN;
dev->magAlign.onBoard = MAG_GPS_ALIGN;
#endif
magHardware = MAG_GPS;
break;
Expand All @@ -178,7 +179,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
#ifdef USE_MAG_MAG3110
if (mag3110detect(dev)) {
#ifdef MAG_MAG3110_ALIGN
dev->magAlign = MAG_MAG3110_ALIGN;
dev->magAlign.onBoard = MAG_MAG3110_ALIGN;
#endif
magHardware = MAG_MAG3110;
break;
Expand All @@ -194,7 +195,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
#ifdef USE_MAG_IST8310
if (ist8310Detect(dev)) {
#ifdef MAG_IST8310_ALIGN
dev->magAlign = MAG_IST8310_ALIGN;
dev->magAlign.onBoard = MAG_IST8310_ALIGN;
#endif
magHardware = MAG_IST8310;
break;
Expand All @@ -210,7 +211,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
#ifdef USE_MAG_MPU9250
if (mpu9250CompassDetect(dev)) {
#ifdef MAG_MPU9250_ALIGN
dev->magAlign = MAG_MPU9250_ALIGN;
dev->magAlign.onBoard = MAG_MPU9250_ALIGN;
#endif
magHardware = MAG_MPU9250;
break;
Expand Down Expand Up @@ -276,8 +277,24 @@ bool compassInit(void)
addBootlogEvent2(BOOT_EVENT_MAG_INIT_FAILED, BOOT_EVENT_FLAGS_ERROR);
sensorsClear(SENSOR_MAG);
}
if (compassConfig()->mag_align != ALIGN_DEFAULT) {
mag.dev.magAlign = compassConfig()->mag_align;
if (compassConfig()->rollDeciDegrees != 0 ||
compassConfig()->pitchDeciDegrees != 0 ||
compassConfig()->yawDeciDegrees != 0) {

// Externally aligned compass
mag.dev.magAlign.useExternal = true;

fp_angles_t r = {
.angles.roll = DECIDEGREES_TO_RADIANS(compassConfig()->rollDeciDegrees),
.angles.pitch = DECIDEGREES_TO_RADIANS(compassConfig()->pitchDeciDegrees),
.angles.yaw = DECIDEGREES_TO_RADIANS(compassConfig()->yawDeciDegrees),
};
buildRotationMatrix(&r, mag.dev.magAlign.externalRotationMatrix);
} else {
mag.dev.magAlign.useExternal = false;
if (compassConfig()->mag_align != ALIGN_DEFAULT) {
mag.dev.magAlign.onBoard = compassConfig()->mag_align;
}
}

return ret;
Expand Down Expand Up @@ -371,32 +388,22 @@ void compassUpdate(timeUs_t currentTimeUs)
}
}

if (compassConfig()->rollDeciDegrees != 0 ||
compassConfig()->pitchDeciDegrees != 0 ||
compassConfig()->yawDeciDegrees != 0) {

// Externally aligned compass
if (mag.dev.magAlign.useExternal) {
struct fp_vector v = {
.X = mag.magADC[X],
.Y = mag.magADC[Y],
.Z = mag.magADC[Z],
};

fp_angles_t r = {
.angles.roll = DECIDEGREES_TO_RADIANS(compassConfig()->rollDeciDegrees),
.angles.pitch = DECIDEGREES_TO_RADIANS(compassConfig()->pitchDeciDegrees),
.angles.yaw = DECIDEGREES_TO_RADIANS(compassConfig()->yawDeciDegrees),
};

rotateV(&v, &r);
rotateByMatrix(&v, mag.dev.magAlign.externalRotationMatrix);

mag.magADC[X] = v.X;
mag.magADC[Y] = v.Y;
mag.magADC[Z] = v.Z;

} else {
// On-board compass
applySensorAlignment(mag.magADC, mag.magADC, mag.dev.magAlign);
applySensorAlignment(mag.magADC, mag.magADC, mag.dev.magAlign.onBoard);
applyBoardAlignment(mag.magADC);
}

Expand Down

0 comments on commit 4a55873

Please sign in to comment.