Merge pull request #6636 from iNavFlight/dzikuvx-fix-mag-gain-calibration

Fix mag gain computation for negative mag zero

src/main/sensors/compass.c

@@ -350,7 +350,7 @@ void compassUpdate(timeUs_t currentTimeUs)
     static sensorCalibrationState_t calState;
     static timeUs_t calStartedAt = 0;
     static int16_t magPrev[XYZ_AXIS_COUNT];
-    static int magGain[XYZ_AXIS_COUNT] = {-4096, -4096, -4096};
+    static int magAxisDeviation[XYZ_AXIS_COUNT];
 
     // Check magZero
     if (
@@ -381,6 +381,7 @@ void compassUpdate(timeUs_t currentTimeUs)
             compassConfigMutable()->magZero.raw[axis] = 0;
             compassConfigMutable()->magGain[axis] = 1024;
             magPrev[axis] = 0;
+            magAxisDeviation[axis] = 0;  // Gain is based on the biggest absolute deviation from the mag zero point. Gain computation starts at 0
         }
 
         beeper(BEEPER_ACTION_SUCCESS);
@@ -400,9 +401,9 @@ void compassUpdate(timeUs_t currentTimeUs)
                 diffMag += (mag.magADC[axis] - magPrev[axis]) * (mag.magADC[axis] - magPrev[axis]);
                 avgMag += (mag.magADC[axis] + magPrev[axis]) * (mag.magADC[axis] + magPrev[axis]) / 4.0f;
 
-                const int32_t sample = ABS(mag.magADC[axis]);
-                if (sample > magGain[axis]) {
-                    magGain[axis] = sample;
+                // Find the biggest sample deviation together with sample' sign
+                if (ABS(mag.magADC[axis]) > ABS(magAxisDeviation[axis])) {
+                    magAxisDeviation[axis] = mag.magADC[axis];
                 }
 
             }
@@ -429,7 +430,7 @@ void compassUpdate(timeUs_t currentTimeUs)
              * It is dirty, but worth checking if this will solve the problem of changing mag vector when UAV is tilted
              */
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                compassConfigMutable()->magGain[axis] = magGain[axis] - compassConfig()->magZero.raw[axis];
+                compassConfigMutable()->magGain[axis] = ABS(magAxisDeviation[axis] - compassConfig()->magZero.raw[axis]);
             }
 
             calStartedAt = 0;