fix spikes in MagYaw debug by re-calc only on new data

betaflight · Sep 25, 2023 · 0c68430 · 0c68430
1 parent 65890cb
commit 0c68430
Show file tree

Hide file tree

Showing 3 changed files with 12 additions and 1 deletion.
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
@@ -263,7 +263,8 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float
     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
-    if (debugMode == DEBUG_GPS_RESCUE_HEADING) {
+    // 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;
@@ -274,6 +275,9 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float
             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) {

diff --git a/src/main/sensors/compass.c b/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)
@@ -379,8 +380,13 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
     }
 
     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 {

diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h
@@ -43,6 +43,7 @@ typedef enum {
 } magSensor_e;
 
 typedef struct mag_s {
+    bool isNewMagADCFlag;
     float magADC[XYZ_AXIS_COUNT];
 } mag_t;