Initial cut on full quaternion IMU conversion

src/main/common/maths.c

@@ -21,6 +21,8 @@
 
 #include "axis.h"
 #include "maths.h"
+#include "vector.h"
+#include "quaternion.h"
 
 // http://lolengine.net/blog/2011/12/21/better-function-approximations
 // Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
@@ -203,32 +205,65 @@ float scaleRangef(float x, float srcMin, float srcMax, float destMin, float dest
     return ((a / b) + destMin);
 }
 
-void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
+// Build rMat from Tait–Bryan angles (convention X1, Y2, Z3)
+void rotationMatrixFromAngles(fpMat3_t * rmat, const fp_angles_t * angles)
 {
     float cosx, sinx, cosy, siny, cosz, sinz;
     float coszcosx, sinzcosx, coszsinx, sinzsinx;
 
-    cosx = cos_approx(delta->angles.roll);
-    sinx = sin_approx(delta->angles.roll);
-    cosy = cos_approx(delta->angles.pitch);
-    siny = sin_approx(delta->angles.pitch);
-    cosz = cos_approx(delta->angles.yaw);
-    sinz = sin_approx(delta->angles.yaw);
+    cosx = cos_approx(angles->angles.roll);
+    sinx = sin_approx(angles->angles.roll);
+    cosy = cos_approx(angles->angles.pitch);
+    siny = sin_approx(angles->angles.pitch);
+    cosz = cos_approx(angles->angles.yaw);
+    sinz = sin_approx(angles->angles.yaw);
 
     coszcosx = cosz * cosx;
     sinzcosx = sinz * cosx;
     coszsinx = sinx * cosz;
     sinzsinx = sinx * sinz;
 
-    matrix[0][X] = cosz * cosy;
-    matrix[0][Y] = -cosy * sinz;
-    matrix[0][Z] = siny;
-    matrix[1][X] = sinzcosx + (coszsinx * siny);
-    matrix[1][Y] = coszcosx - (sinzsinx * siny);
-    matrix[1][Z] = -sinx * cosy;
-    matrix[2][X] = (sinzsinx) - (coszcosx * siny);
-    matrix[2][Y] = (coszsinx) + (sinzcosx * siny);
-    matrix[2][Z] = cosy * cosx;
+    rmat->m[0][X] = cosz * cosy;
+    rmat->m[0][Y] = -cosy * sinz;
+    rmat->m[0][Z] = siny;
+    rmat->m[1][X] = sinzcosx + (coszsinx * siny);
+    rmat->m[1][Y] = coszcosx - (sinzsinx * siny);
+    rmat->m[1][Z] = -sinx * cosy;
+    rmat->m[2][X] = (sinzsinx) - (coszcosx * siny);
+    rmat->m[2][Y] = (coszsinx) + (sinzcosx * siny);
+    rmat->m[2][Z] = cosy * cosx;
+}
+
+void rotationMatrixFromAxisAngle(fpMat3_t * rmat, const fpAxisAngle_t * a)
+{
+    const float sang = sin_approx(a->angle);
+    const float cang = cos_approx(a->angle);
+    const float C = 1.0f - cang;
+
+    const float xC  = a->axis.x * C;
+    const float yC  = a->axis.y * C;
+    const float zC  = a->axis.z * C;
+    const float xxC = a->axis.x * xC;
+    const float yyC = a->axis.y * yC;
+    const float zzC = a->axis.z * zC;
+    const float xyC = a->axis.x * yC;
+    const float yzC = a->axis.y * zC;
+    const float zxC = a->axis.z * xC;
+    const float xs  = a->axis.x * sang;
+    const float ys  = a->axis.y * sang;
+    const float zs  = a->axis.z * sang;
+
+    rmat->m[0][X] = xxC + cang;
+    rmat->m[0][Y] = xyC - zs;
+    rmat->m[0][Z] = zxC + ys;
+
+    rmat->m[1][X] = zxC + ys;
+    rmat->m[1][Y] = yyC + cang;
+    rmat->m[1][Z] = yzC - xs;
+
+    rmat->m[2][X] = zxC - ys;
+    rmat->m[2][Y] = yzC + xs;
+    rmat->m[2][Z] = zzC + cang;
 }
 
 // Quick median filter implementation

src/main/common/maths.h

@@ -127,8 +127,6 @@ int32_t applyDeadband(int32_t value, int32_t deadband);
 int constrain(int amt, int low, int high);
 float constrainf(float amt, float low, float high);
 
-void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3]);
-
 void devClear(stdev_t *dev);
 void devPush(stdev_t *dev, float x);
 float devVariance(stdev_t *dev);

src/main/common/quaternion.h

@@ -30,11 +30,6 @@ typedef struct {
     float q0, q1, q2, q3;
 } fpQuaternion_t;
 
-typedef struct {
-    fpVector3_t axis;
-    float angle;
-} fpAxisAngle_t;
-
 static inline fpQuaternion_t * quaternionInitUnit(fpQuaternion_t * result)
 { 
     result->q0 = 1.0f;

src/main/common/vector.h

@@ -33,7 +33,15 @@ typedef struct {
     float m[3][3];
 } fpMat3_t;
 
-static inline fpVector3_t * vectorRotate(fpVector3_t * result, const fpVector3_t * a, const fpMat3_t * rmat)
+typedef struct {
+    fpVector3_t axis;
+    float angle;
+} fpAxisAngle_t;
+
+void rotationMatrixFromAngles(fpMat3_t * rmat, const fp_angles_t * angles);
+void rotationMatrixFromAxisAngle(fpMat3_t * rmat, const fpAxisAngle_t * a);
+
+static inline fpVector3_t * rotationMatrixRotateVector(fpVector3_t * result, const fpVector3_t * a, const fpMat3_t * rmat)
 {
     fpVector3_t r;
 

src/main/flight/imu.c

@@ -83,6 +83,8 @@ STATIC_FASTRAM float smallAngleCosZ;
 
 STATIC_FASTRAM bool isAccelUpdatedAtLeastOnce;
 
+FASTRAM fpMat3_t magDeclinationRMat;                // Rotation matrix for magnetic declination correction (Earth frame)
+
 FASTRAM fpQuaternion_t orientation;
 FASTRAM attitudeEulerAngles_t attitude;             // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 STATIC_FASTRAM_UNIT_TESTED float rMat[3][3];
@@ -148,10 +150,21 @@ void imuInit(void)
     isAccelUpdatedAtLeastOnce = false;
     gpsHeadingInitialized = false;
 
+    // Create magnetic declination matrix
+    const int deg = compassConfig()->mag_declination / 100;
+    const int min = compassConfig()->mag_declination   % 100;
+    imuSetMagneticDeclination(deg + min / 60.0f);
+
     quaternionInitUnit(&orientation);
     imuComputeRotationMatrix();
 }
 
+void imuSetMagneticDeclination(float declinationDeg)
+{
+    fpAxisAngle_t axisAngle = { .axis = { .v = { 0.0f, 0.0f, 1.0f } }, .angle = DEGREES_TO_RADIANS(declinationDeg) };
+    rotationMatrixFromAxisAngle(&magDeclinationRMat, &axisAngle);
+}
+
 void imuTransformVectorBodyToEarth(fpVector3_t * v)
 {
     // From body frame to earth frame
@@ -277,6 +290,9 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
             // Ignore magnetic inclination
             vMag.z = 0.0f;
 
+            // Apply magnetic declination correction
+            rotationMatrixRotateVector(&vMag, &vMag, &magDeclinationRMat);
+
             // Normalize to unit vector
             vectorNormalize(&vMag, &vMag);
 
@@ -404,7 +420,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
     /* Compute pitch/roll angles */
     attitude.values.roll = RADIANS_TO_DECIDEGREES(atan2_approx(rMat[2][1], rMat[2][2]));
     attitude.values.pitch = RADIANS_TO_DECIDEGREES((0.5f * M_PIf) - acos_approx(-rMat[2][0]));
-    attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0])) + mag.magneticDeclination;
+    attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0]));
 
     if (attitude.values.yaw < 0)
         attitude.values.yaw += 3600;

src/main/flight/imu.h

@@ -60,6 +60,7 @@ typedef struct imuRuntimeConfig_s {
 
 void imuConfigure(void);
 
+void imuSetMagneticDeclination(float declinationDeg);
 void imuUpdateAttitude(timeUs_t currentTimeUs);
 void imuUpdateAccelerometer(void);
 float calculateCosTiltAngle(void);

src/main/navigation/navigation_pos_estimator.c

@@ -348,7 +348,7 @@ void onNewGPSData(void)
         /* Automatic magnetic declination calculation - do this once */
         static bool magDeclinationSet = false;
         if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) {
-            mag.magneticDeclination = geoCalculateMagDeclination(&newLLH) * 10.0f; // heading is in 0.1deg units
+            imuSetMagneticDeclination(geoCalculateMagDeclination(&newLLH));
             magDeclinationSet = true;
         }
 #endif

src/main/sensors/boardalignment.c

@@ -21,6 +21,7 @@
 #include <string.h>
 
 #include "common/maths.h"
+#include "common/vector.h"
 #include "common/axis.h"
 
 #include "config/parameter_group.h"
@@ -31,7 +32,7 @@
 #include "boardalignment.h"
 
 static bool standardBoardAlignment = true;     // board orientation correction
-static float boardRotation[3][3];              // matrix
+static fpMat3_t boardRotMatrix;
 
 // no template required since defaults are zero
 PG_REGISTER(boardAlignment_t, boardAlignment, PG_BOARD_ALIGNMENT, 0);
@@ -54,7 +55,7 @@ void initBoardAlignment(void)
         rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(boardAlignment()->pitchDeciDegrees);
         rotationAngles.angles.yaw   = DECIDEGREES_TO_RADIANS(boardAlignment()->yawDeciDegrees  );
 
-        buildRotationMatrix(&rotationAngles, boardRotation);
+        rotationMatrixFromAngles(&boardRotMatrix, &rotationAngles);
     }
 }
 
@@ -75,13 +76,12 @@ void applyBoardAlignment(int32_t *vec)
         return;
     }
 
-    int32_t x = vec[X];
-    int32_t y = vec[Y];
-    int32_t z = vec[Z];
+    fpVector3_t fpVec = { .v = { vec[X], vec[Y], vec[Z] } };
+    rotationMatrixRotateVector(&fpVec, &fpVec, &boardRotMatrix);
 
-    vec[X] = lrintf(boardRotation[0][X] * x + boardRotation[1][X] * y + boardRotation[2][X] * z);
-    vec[Y] = lrintf(boardRotation[0][Y] * x + boardRotation[1][Y] * y + boardRotation[2][Y] * z);
-    vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
+    vec[X] = lrintf(fpVec.x);
+    vec[Y] = lrintf(fpVec.y);
+    vec[Z] = lrintf(fpVec.z);
 }
 
 void applySensorAlignment(int32_t * dest, int32_t * src, uint8_t rotation)

src/main/sensors/compass.c

@@ -77,7 +77,6 @@ PG_RESET_TEMPLATE(compassConfig_t, compassConfig,
 
 #ifdef USE_MAG
 
-static uint8_t magInit = 0;
 static uint8_t magUpdatedAtLeastOnce = 0;
 
 bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
@@ -264,15 +263,12 @@ bool compassInit(void)
     LED1_ON;
     const bool ret = mag.dev.init(&mag.dev);
     LED1_OFF;
-    if (ret) {
-        const int deg = compassConfig()->mag_declination / 100;
-        const int min = compassConfig()->mag_declination   % 100;
-        mag.magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
-        magInit = 1;
-    } else {
+
+    if (!ret) {
         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;
     }
@@ -329,12 +325,6 @@ void compassUpdate(timeUs_t currentTimeUs)
         DISABLE_STATE(CALIBRATE_MAG);
     }
 
-    if (magInit) {              // we apply offset only once mag calibration is done
-        mag.magADC[X] -= compassConfig()->magZero.raw[X];
-        mag.magADC[Y] -= compassConfig()->magZero.raw[Y];
-        mag.magADC[Z] -= compassConfig()->magZero.raw[Z];
-    }
-
     if (calStartedAt != 0) {
         if ((currentTimeUs - calStartedAt) < (compassConfig()->magCalibrationTimeLimit * 1000000)) {
             LED0_TOGGLE;
@@ -367,6 +357,11 @@ void compassUpdate(timeUs_t currentTimeUs)
             saveConfigAndNotify();
         }
     }
+    else {
+        mag.magADC[X] -= compassConfig()->magZero.raw[X];
+        mag.magADC[Y] -= compassConfig()->magZero.raw[Y];
+        mag.magADC[Z] -= compassConfig()->magZero.raw[Z];
+    }
 
     applySensorAlignment(mag.magADC, mag.magADC, mag.dev.magAlign);
     applyBoardAlignment(mag.magADC);

src/main/sensors/compass.h

@@ -45,7 +45,6 @@ typedef enum {
 
 typedef struct mag_s {
     magDev_t dev;
-    float magneticDeclination;
     int32_t magADC[XYZ_AXIS_COUNT];
 } mag_t;
 

src/main/sensors/initialisation.c

@@ -61,8 +61,6 @@ bool sensorsAutodetect(void)
     pitotInit();
 #endif
 
-    // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
-    mag.magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
 #ifdef USE_MAG
     compassInit();
 #endif