Calibration for MAG gain

make/release.mk

@@ -1,6 +1,6 @@
 RELEASE_TARGETS = AIRHEROF3 AIRHEROF3_QUAD
 
-RELEASE_TARGETS += LUX_RACE RCEXPLORERF3 RMDO SPARKY KFC32F3_INAV FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY DALRCF405 DALRCF722DUAL
+RELEASE_TARGETS += LUX_RACE RCEXPLORERF3 RMDO SPARKY FALCORE MOTOLAB ANYFC BLUEJAYF4 COLIBRI F4BY DALRCF405 DALRCF722DUAL
 RELEASE_TARGETS += QUANTON REVO SPARKY2 PIKOBLX CLRACINGF4AIR CLRACINGF4AIRV2 PIXRACER BEEROTORF4 ANYFCF7 ANYFCF7_EXTERNAL_BARO
 RELEASE_TARGETS += ALIENFLIGHTNGF7
 

make/targets.mk

@@ -77,7 +77,7 @@ GROUP_3_TARGETS := AIRBOTF4 BLUEJAYF4 OMNIBUSF4 OMNIBUSF4PRO FIREWORKSV2 SPARKY2
 GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ALIENFLIGHTNGF7 PIXRACER YUPIF7 MATEKF405SE MATEKF411 MATEKF722 MATEKF405OSD MATEKF405_SERVOS6 NOX
 GROUP_5_TARGETS := ASGARD32F7 CLRACINGF4AIRV3 DALRCF405 DALRCF722DUAL DYSF4PROV2 F4BY FISHDRONEF4 FOXEERF405 FOXEERF722DUAL FRSKYF3 FRSKYF4 FURYF4OSD
 GROUP_6_TARGETS := MAMBAF405 OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS AIKONF4
-GROUP_7_TARGETS := KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI KFC32F3_INAV MATEKF411_RSSI MATEKF411_SFTSRL2 MATEKF722MINI MATEKF722SE MATEKF722_HEXSERVO
+GROUP_7_TARGETS := KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI MATEKF411_RSSI MATEKF411_SFTSRL2 MATEKF722MINI MATEKF722SE MATEKF722_HEXSERVO
 GROUP_8_TARGETS := MATEKF765 MATEKF722PX KAKUTEF7HDV ZEEZF7 FOXEERF722V2 FRSKYPILOT FRSKY_ROVERF7
 GROUP_OTHER_TARGETS := $(filter-out $(GROUP_1_TARGETS) $(GROUP_2_TARGETS) $(GROUP_3_TARGETS) $(GROUP_4_TARGETS) $(GROUP_5_TARGETS) $(GROUP_6_TARGETS) $(GROUP_7_TARGETS) $(GROUP_8_TARGETS), $(VALID_TARGETS))
 

src/main/fc/fc_msp.c

@@ -1283,6 +1283,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
     #else
         sbufWriteU16(dst, 0);
     #endif
+
+    #ifdef USE_MAG
+        sbufWriteU16(dst, compassConfig()->magGain[X]);
+        sbufWriteU16(dst, compassConfig()->magGain[Y]);
+        sbufWriteU16(dst, compassConfig()->magGain[Z]);
+    #else
+        sbufWriteU16(dst, 0);
+        sbufWriteU16(dst, 0);
+        sbufWriteU16(dst, 0);
+    #endif
+
         break;
 
     case MSP_POSITION_ESTIMATION_CONFIG:
@@ -2190,6 +2201,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             if (dataSize >= 20) {
                 opticalFlowConfigMutable()->opflow_scale = sbufReadU16(src) / 256.0f;
             }
+#endif
+#ifdef USE_MAG
+            if (dataSize >= 22) {
+                compassConfigMutable()->magGain[X] = sbufReadU16(src);
+                compassConfigMutable()->magGain[Y] = sbufReadU16(src);
+                compassConfigMutable()->magGain[Z] = sbufReadU16(src);
+            }
+#else
+            if (dataSize >= 22) {
+                sbufReadU16(src);
+                sbufReadU16(src);
+                sbufReadU16(src);
+            }
 #endif
         } else
             return MSP_RESULT_ERROR;

src/main/fc/settings.yaml

@@ -403,6 +403,24 @@ groups:
         field: magZero.raw[Z]
         min: INT16_MIN
         max: INT16_MAX
+      - name: maggain_x
+        description: "Magnetometer calibration X gain. If 0, no calibration or calibration failed"
+        default_value: "0"
+        field: magGain[X]
+        min: INT16_MIN
+        max: INT16_MAX
+      - name: maggain_y
+        description: "Magnetometer calibration Y gain. If 0, no calibration or calibration failed"
+        default_value: "0"
+        field: magGain[Y]
+        min: INT16_MIN
+        max: INT16_MAX
+      - name: maggain_z
+        description: "Magnetometer calibration Z gain. If 0, no calibration or calibration failed"
+        default_value: "0"
+        field: magGain[Z]
+        min: INT16_MIN
+        max: INT16_MAX
       - name: mag_calibration_time
         description: "Adjust how long time the Calibration of mag will last."
         default_value: "30"

src/main/sensors/compass.c

@@ -57,7 +57,7 @@
 
 mag_t mag;                   // mag access functions
 
-PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 3);
+PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 4);
 
 #ifdef USE_MAG
 #define MAG_HARDWARE_DEFAULT    MAG_AUTODETECT
@@ -334,9 +334,13 @@ 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};
 
     // Check magZero
-    if ((compassConfig()->magZero.raw[X] == 0) && (compassConfig()->magZero.raw[Y] == 0) && (compassConfig()->magZero.raw[Z] == 0)) {
+    if (
+        (compassConfig()->magZero.raw[X] == 0 && compassConfig()->magZero.raw[Y] == 0 && compassConfig()->magZero.raw[Z] == 0) || 
+        compassConfig()->magGain[X] == 0 || compassConfig()->magGain[Y] == 0 || compassConfig()->magGain[Z] == 0  
+    ) {
         DISABLE_STATE(COMPASS_CALIBRATED);
     }
     else {
@@ -359,6 +363,7 @@ void compassUpdate(timeUs_t currentTimeUs)
 
         for (int axis = 0; axis < 3; axis++) {
             compassConfigMutable()->magZero.raw[axis] = 0;
+            compassConfigMutable()->magGain[axis] = 0;
             magPrev[axis] = 0;
         }
 
@@ -375,9 +380,15 @@ void compassUpdate(timeUs_t currentTimeUs)
             float diffMag = 0;
             float avgMag = 0;
 
-            for (int axis = 0; axis < 3; axis++) {
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
                 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;
+                }
+
             }
 
             // sqrtf(diffMag / avgMag) is a rough approximation of tangent of angle between magADC and magPrev. tan(8 deg) = 0.14
@@ -396,14 +407,23 @@ void compassUpdate(timeUs_t currentTimeUs)
                 compassConfigMutable()->magZero.raw[axis] = lrintf(magZerof[axis]);
             }
 
+            /*
+             * Scale calibration
+             * We use max absolute value of each axis as scale calibration with constant 1024 as base
+             * 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];
+            }
+
             calStartedAt = 0;
             saveConfigAndNotify();
         }
     }
     else {
-        mag.magADC[X] -= compassConfig()->magZero.raw[X];
-        mag.magADC[Y] -= compassConfig()->magZero.raw[Y];
-        mag.magADC[Z] -= compassConfig()->magZero.raw[Z];
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            mag.magADC[axis] = (mag.magADC[axis] - compassConfig()->magZero.raw[axis]) * 1024 / compassConfig()->magGain[axis];
+        }
     }
 
     if (mag.dev.magAlign.useExternal) {

src/main/sensors/compass.h

@@ -58,6 +58,7 @@ typedef struct compassConfig_s {
     sensor_align_e mag_align;               // on-board mag alignment. Ignored if externally aligned via *DeciDegrees.
     uint8_t mag_hardware;                   // Which mag hardware to use on boards with more than one device
     flightDynamicsTrims_t magZero;
+    int16_t magGain[XYZ_AXIS_COUNT];
     uint8_t mag_to_use;
     uint8_t magCalibrationTimeLimit;        // Time for compass calibration (seconds)
     int16_t rollDeciDegrees;                // Alignment for external mag on the roll (X) axis (0.1deg)