Add magnetic heading as debug and magnetic declination for the Mahony filter

src/main/cli/settings.c

@@ -1003,16 +1003,18 @@ const clivalue_t valueTable[] = {
     { "serial_update_rate_hz",      VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 2000 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, serial_update_rate_hz) },
 
 // PG_IMU_CONFIG
-    { "imu_dcm_kp",                 VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_kp) },
-    { "imu_dcm_ki",                 VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_ki) },
-    { "small_angle",                VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 180 }, PG_IMU_CONFIG, offsetof(imuConfig_t, small_angle) },
-    { "imu_process_denom",          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 1, 4 }, PG_IMU_CONFIG, offsetof(imuConfig_t, imu_process_denom) },
+    { PARAM_NAME_IMU_DCM_KP,          VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, imu_dcm_kp) },
+    { PARAM_NAME_IMU_DCM_KI,          VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, imu_dcm_ki) },
+    { PARAM_NAME_IMU_SMALL_ANGLE,     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0,   180 }, PG_IMU_CONFIG, offsetof(imuConfig_t, small_angle) },
+    { PARAM_NAME_IMU_PROCESS_DENOM,   VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 1,     4 }, PG_IMU_CONFIG, offsetof(imuConfig_t, imu_process_denom) },
+#ifdef USE_MAG
+    { PARAM_NAME_IMU_MAG_DECLINATION, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0,  3599 }, PG_IMU_CONFIG, offsetof(imuConfig_t, mag_declination) },
+#endif
 
 // PG_ARMING_CONFIG
     { "auto_disarm_delay",          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 60 }, PG_ARMING_CONFIG, offsetof(armingConfig_t, auto_disarm_delay) },
     { PARAM_NAME_GYRO_CAL_ON_FIRST_ARM, VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ARMING_CONFIG, offsetof(armingConfig_t, gyro_cal_on_first_arm) },
 
-
 // PG_GPS_CONFIG
 #ifdef USE_GPS
     { PARAM_NAME_GPS_PROVIDER,               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_PROVIDER },   PG_GPS_CONFIG, offsetof(gpsConfig_t, provider) },

src/main/common/vector.h

@@ -121,6 +121,26 @@ static inline fpVector3_t * matrixVectorMul(fpVector3_t * result, const fpMat33_
     return result;
 }
 
+static inline fpMat33_t * yawToRotationMatrixZ(fpMat33_t * result, const float yaw)
+{
+    fpMat33_t r;
+    const float sinYaw = sin_approx(yaw);
+    const float cosYaw = cos_approx(yaw);
+
+    r.m[0][0] = cosYaw;
+    r.m[1][0] = sinYaw;
+    r.m[2][0] = 0.0f;
+    r.m[0][1] = -sinYaw;
+    r.m[1][1] = cosYaw;
+    r.m[2][1] = 0.0f;
+    r.m[0][2] = 0.0f;
+    r.m[1][2] = 0.0f;
+    r.m[2][2] = 1.0f;
+
+    *result = r;
+    return result;
+}
+
 static inline fpVector3_t * matrixTrnVectorMul(fpVector3_t * result, const fpMat33_t * mat, const fpVector3_t * a)
 {
     fpVector3_t r;

src/main/drivers/compass/compass_hmc5883l.c

@@ -125,6 +125,7 @@
 #define HMC_CONFA_POS_BIAS          0x01
 #define HMC_CONFA_NEG_BIAS          0x02
 #define HMC_CONFA_DOR_15HZ          0X10
+#define HMC_CONFA_DOR_75HZ          0x06
 #define HMC_CONFA_8_SAMLES          0X60
 #define HMC_CONFB_GAIN_2_5GA        0X60
 #define HMC_CONFB_GAIN_1_3GA        0X20
@@ -227,7 +228,7 @@ static bool hmc5883lInit(magDev_t *mag)
     extDevice_t *dev = &mag->dev;
 
     // leave test mode
-    busWriteRegister(dev, HMC58X3_REG_CONFA, HMC_CONFA_8_SAMLES | HMC_CONFA_DOR_15HZ | HMC_CONFA_NORMAL);    // Configuration Register A  -- 0 11 100 00  num samples: 8 ; output rate: 15Hz ; normal measurement mode
+    busWriteRegister(dev, HMC58X3_REG_CONFA, HMC_CONFA_8_SAMLES | HMC_CONFA_DOR_75HZ | HMC_CONFA_NORMAL);    // Configuration Register A  -- 0 11 100 00  num samples: 8 ; output rate: 15Hz ; normal measurement mode
     busWriteRegister(dev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_1_3GA);                                          // Configuration Register B  -- 001 00000    configuration gain 1.3Ga
     busWriteRegister(dev, HMC58X3_REG_MODE, HMC_MODE_CONTINOUS);                                             // Mode register             -- 000000 00    continuous Conversion Mode
 

src/main/drivers/compass/compass_qmc5883l.c

@@ -109,7 +109,7 @@ static bool qmc5883lRead(magDev_t *magDev, int16_t *magData)
             return false;
 
         case STATE_WAIT_STATUS:
-            if ((status & 0x04) == 0) {
+            if ((status & 0x01) == 0) {
                 state = STATE_READ_STATUS;
                 return false;
             }

src/main/fc/parameter_names.h

@@ -197,3 +197,11 @@
 #define PARAM_NAME_GPS_LAP_TIMER_GATE_TOLERANCE "gps_lap_timer_gate_tolerance_m"
 #endif // USE_GPS_LAP_TIMER
 #endif
+
+#define PARAM_NAME_IMU_DCM_KP "imu_dcm_kp"
+#define PARAM_NAME_IMU_DCM_KI "imu_dcm_ki"
+#define PARAM_NAME_IMU_SMALL_ANGLE "small_angle"
+#define PARAM_NAME_IMU_PROCESS_DENOM "imu_process_denom"
+#ifdef USE_MAG
+#define PARAM_NAME_IMU_MAG_DECLINATION "mag_declination"
+#endif

src/main/fc/tasks.c

@@ -383,7 +383,7 @@ task_attribute_t task_attributes[TASK_COUNT] = {
 #endif
 
 #ifdef USE_MAG
-    [TASK_COMPASS] = DEFINE_TASK("COMPASS", NULL, NULL, taskUpdateMag, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW),
+    [TASK_COMPASS] = DEFINE_TASK("COMPASS", NULL, NULL, taskUpdateMag, TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ), TASK_PRIORITY_LOW),
 #endif
 
 #ifdef USE_BARO

src/main/flight/imu.c

@@ -100,6 +100,7 @@ static float smallAngleCosZ = 0;
 static imuRuntimeConfig_t imuRuntimeConfig;
 
 float rMat[3][3];
+static fpVector2_t north_ef;
 
 #if defined(USE_ACC)
 STATIC_UNIT_TESTED bool attitudeIsEstablished = false;
@@ -115,13 +116,14 @@ quaternion offset = QUATERNION_INITIALIZE;
 // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 attitudeEulerAngles_t attitude = EULER_INITIALIZE;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
+PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 3);
 
 PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
-    .dcm_kp = 2500,                // 1.0 * 10000
-    .dcm_ki = 0,                   // 0.003 * 10000
+    .imu_dcm_kp = 2500,      // 1.0 * 10000
+    .imu_dcm_ki = 0,         // 0.003 * 10000
     .small_angle = 25,
-    .imu_process_denom = 2
+    .imu_process_denom = 2,
+    .mag_declination = 0
 );
 
 static void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd)
@@ -167,8 +169,12 @@ static float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
 
 void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value)
 {
-    imuRuntimeConfig.dcm_kp = imuConfig()->dcm_kp / 10000.0f;
-    imuRuntimeConfig.dcm_ki = imuConfig()->dcm_ki / 10000.0f;
+    imuRuntimeConfig.imuDcmKp = imuConfig()->imu_dcm_kp / 10000.0f;
+    imuRuntimeConfig.imuDcmKi = imuConfig()->imu_dcm_ki / 10000.0f;
+    // magnetic declination has negative sign (positive clockwise when seen from top)
+    const float imuMagneticDeclinationRad = DEGREES_TO_RADIANS(imuConfig()->mag_declination / 10.0f);
+    north_ef.x = cos_approx(imuMagneticDeclinationRad);
+    north_ef.y = -sin_approx(imuMagneticDeclinationRad);
 
     smallAngleCosZ = cos_approx(degreesToRadians(imuConfig()->small_angle));
 
@@ -252,21 +258,39 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float
 #ifdef USE_MAG
     // Use measured magnetic field vector
     fpVector3_t mag_bf = {{mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}};
-    float recipMagNorm = vectorNormSquared(&mag_bf);
-    if (useMag && recipMagNorm > 0.01f) {
+    float magNormSquared = vectorNormSquared(&mag_bf);
+    fpVector3_t mag_ef;
+    matrixVectorMul(&mag_ef, (const fpMat33_t*)&rMat, &mag_bf); // BF->EF true north
+
+    // Encapsulate additional operations in a block so that it is only executed when the according debug mode is used
+    // Only re-calculate magYaw when there is a new Mag data reading, to avoid spikes
+    if (debugMode == DEBUG_GPS_RESCUE_HEADING && mag.isNewMagADCFlag) {
+        fpMat33_t rMatZTrans;
+        yawToRotationMatrixZ(&rMatZTrans, -atan2_approx(rMat[1][0], rMat[0][0]));
+        fpVector3_t mag_ef_yawed;
+        matrixVectorMul(&mag_ef_yawed, &rMatZTrans, &mag_ef); // EF->EF yawed
+        // Magnetic yaw is the angle between true north and the X axis of the body frame
+        int16_t magYaw = lrintf((atan2_approx(mag_ef_yawed.y, mag_ef_yawed.x) * (1800.0f / M_PIf)));
+        if (magYaw < 0) {
+            magYaw += 3600;
+        }
+        DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 4, magYaw); // mag heading in degrees * 10
+        // reset new mag data flag to false to initiate monitoring for new Mag data.
+        // note that if the debug doesn't run, this reset will not occur, and we won't waste cycles on the comparison
+        mag.isNewMagADCFlag = false;
+    }
+
+    if (useMag && magNormSquared > 0.01f) {
         // Normalise magnetometer measurement
-        vectorNormalize(&mag_bf, &mag_bf);
+        vectorNormalize(&mag_ef, &mag_ef);
 
         // For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
         // This way magnetic field will only affect heading and wont mess roll/pitch angles
 
         // (hx; hy; 0) - measured mag field vector in EF (forcing Z-component to zero)
         // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
-        fpVector3_t mag_ef;
-        matrixVectorMul(&mag_ef, (const fpMat33_t*)&rMat, &mag_bf);  // BF->EF
         mag_ef.z = 0.0f;                // project to XY plane (optimized away)
 
-        fpVector2_t north_ef = {{ 1.0f, 0.0f }};
         // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
         // increase gain on large misalignment
         const float dot = vector2Dot((fpVector2_t*)&mag_ef, &north_ef);
@@ -297,10 +321,10 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float
     }
 
     // Compute and apply integral feedback if enabled
-    if (imuRuntimeConfig.dcm_ki > 0.0f) {
+    if (imuRuntimeConfig.imuDcmKi > 0.0f) {
         // Stop integrating if spinning beyond the certain limit
         if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
-            const float dcmKiGain = imuRuntimeConfig.dcm_ki;
+            const float dcmKiGain = imuRuntimeConfig.imuDcmKi;
             integralFBx += dcmKiGain * ex * dt;    // integral error scaled by Ki
             integralFBy += dcmKiGain * ey * dt;
             integralFBz += dcmKiGain * ez * dt;
@@ -380,7 +404,7 @@ static bool imuIsAccelerometerHealthy(float *accAverage)
     return (0.81f < accMagnitudeSq) && (accMagnitudeSq < 1.21f);
 }
 
-// Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling.
+// Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.imuDcmKp * 1.0 scaling.
 // When disarmed after initial boot, the scaling is set to 10.0 for the first 20 seconds to speed up initial convergence.
 // After disarming we want to quickly reestablish convergence to deal with the attitude estimation being incorrect due to a crash.
 //   - wait for a 250ms period of low gyro activity to ensure the craft is not moving
@@ -425,7 +449,7 @@ static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAvera
                 arState = stReset;
             }
             // low gain during quiet phase
-            return imuRuntimeConfig.dcm_kp;
+            return imuRuntimeConfig.imuDcmKp;
         case stReset:
             if (cmpTimeUs(currentTimeUs, stateTimeout) >= 0) {
                 arState = stDisarmed;
@@ -434,11 +458,11 @@ static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAvera
             return ATTITUDE_RESET_KP_GAIN;
         case stDisarmed:
             // Scale the kP to generally converge faster when disarmed.
-            return imuRuntimeConfig.dcm_kp * 10.0f;
+            return imuRuntimeConfig.imuDcmKp * 10.0f;
         }
     } else {
         arState = stArmed;
-        return imuRuntimeConfig.dcm_kp;
+        return imuRuntimeConfig.imuDcmKp;
     }
 }
 

src/main/flight/imu.h

@@ -53,17 +53,18 @@ extern attitudeEulerAngles_t attitude;
 extern float rMat[3][3];
 
 typedef struct imuConfig_s {
-    uint16_t dcm_kp;                        // DCM filter proportional gain ( x 10000)
-    uint16_t dcm_ki;                        // DCM filter integral gain ( x 10000)
+    uint16_t imu_dcm_kp;          // DCM filter proportional gain ( x 10000)
+    uint16_t imu_dcm_ki;          // DCM filter integral gain ( x 10000)
     uint8_t small_angle;
     uint8_t imu_process_denom;
+    uint16_t mag_declination;     // Magnetic declination in degrees * 10
 } imuConfig_t;
 
 PG_DECLARE(imuConfig_t, imuConfig);
 
 typedef struct imuRuntimeConfig_s {
-    float dcm_ki;
-    float dcm_kp;
+    float imuDcmKi;
+    float imuDcmKp;
 } imuRuntimeConfig_t;
 
 void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value);

src/main/sensors/compass.c

@@ -117,6 +117,7 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig)
 }
 
 static int16_t magADCRaw[XYZ_AXIS_COUNT];
+static int16_t magADCRawPrevious[XYZ_AXIS_COUNT];
 static uint8_t magInit = 0;
 
 void compassPreInit(void)
@@ -375,12 +376,17 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
     if (busBusy(&magDev.dev, NULL) || !magDev.read(&magDev, magADCRaw)) {
         // No action was taken as the read has not completed
         schedulerIgnoreTaskExecRate();
-        return 1000; // Wait 1ms between states
+        return 500; // Wait 500us between states
     }
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        if (magADCRaw[axis] != magADCRawPrevious[axis]) {
+            // this test, and the isNewMagADCFlag itself, is only needed if we calculate magYaw in imu.c
+            mag.isNewMagADCFlag = true;
+        }
         mag.magADC[axis] = magADCRaw[axis];
     }
+
     if (magDev.magAlignment == ALIGN_CUSTOM) {
         alignSensorViaMatrix(mag.magADC, &magDev.rotationMatrix);
     } else {
@@ -413,6 +419,6 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
         }
     }
 
-    return TASK_PERIOD_HZ(10);
+    return TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ);
 }
 #endif // USE_MAG

src/main/sensors/compass.h

@@ -27,6 +27,7 @@
 #include "pg/pg.h"
 #include "sensors/sensors.h"
 
+#define TASK_COMPASS_RATE_HZ 200
 
 // Type of magnetometer used/detected
 typedef enum {
@@ -42,6 +43,7 @@ typedef enum {
 } magSensor_e;
 
 typedef struct mag_s {
+    bool isNewMagADCFlag;
     float magADC[XYZ_AXIS_COUNT];
 } mag_t;