From 61efa9b5e13fd1cf504e38a4c9d925e16a031b1f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sun, 25 Feb 2018 12:34:53 +0000 Subject: [PATCH] Precalculate external mag rotation matrix 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. --- src/main/common/maths.c | 16 +++++---- src/main/common/maths.h | 1 + src/main/drivers/compass/compass.h | 6 +++- src/main/sensors/compass.c | 55 +++++++++++++++++------------- 4 files changed, 47 insertions(+), 31 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 7c2b503799b..bf7672eaf1f 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -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 diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 2eba6bd766f..f1b2702b2a1 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -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); diff --git a/src/main/drivers/compass/compass.h b/src/main/drivers/compass/compass.h index 18db9c45138..4baad49184f 100644 --- a/src/main/drivers/compass/compass.h +++ b/src/main/drivers/compass/compass.h @@ -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; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 1f37e5dfabb..2114d620d8c 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -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: @@ -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; @@ -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; @@ -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; @@ -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; @@ -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; @@ -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; @@ -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; @@ -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; @@ -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; @@ -371,24 +388,14 @@ 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; @@ -396,7 +403,7 @@ void compassUpdate(timeUs_t currentTimeUs) } else { // On-board compass - applySensorAlignment(mag.magADC, mag.magADC, mag.dev.magAlign); + applySensorAlignment(mag.magADC, mag.magADC, mag.dev.magAlign.onBoard); applyBoardAlignment(mag.magADC); }