Improved compass calibration.

src/main/config/config.c

@@ -268,7 +268,9 @@ static void validateAndFixConfig(void)
 
     validateAndFixGyroConfig();
 
+#if defined(USE_MAG)
     buildAlignmentFromStandardAlignment(&compassConfigMutable()->mag_customAlignment, compassConfig()->mag_alignment);
+#endif
     buildAlignmentFromStandardAlignment(&gyroDeviceConfigMutable(0)->customAlignment, gyroDeviceConfig(0)->alignment);
 #if defined(USE_MULTI_GYRO)
     buildAlignmentFromStandardAlignment(&gyroDeviceConfigMutable(1)->customAlignment, gyroDeviceConfig(1)->alignment);

src/main/fc/core.c

@@ -98,8 +98,8 @@
 #include "sensors/barometer.h"
 #include "sensors/battery.h"
 #include "sensors/boardalignment.h"
+#include "sensors/compass.h"
 #include "sensors/gyro.h"
-#include "sensors/sensors.h"
 
 #include "telemetry/telemetry.h"
 
@@ -177,21 +177,17 @@ PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig,
 
 static bool isCalibrating(void)
 {
-#ifdef USE_BARO
+    return !isGyroCalibrationComplete()
-    if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
-        return true;
-    }
-#endif
-
-    // Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
-
-    return (
 #ifdef USE_ACC
-        !accIsCalibrationComplete()
+        || (sensors(SENSOR_ACC) && !accIsCalibrationComplete())
-#else
-        false
 #endif
-            && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
+#ifdef USE_BARO
+        || (sensors(SENSOR_BARO) && !isBaroCalibrationComplete())
+#endif
+#ifdef USE_MAG
+        || (sensors(SENSOR_MAG) && !isCompassCalibrationComplete())
+#endif
+        ;
 }
 
 #ifdef USE_LAUNCH_CONTROL

src/main/fc/rc_controls.c

@@ -62,6 +62,7 @@
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
 #include "sensors/battery.h"
+#include "sensors/compass.h"
 #include "sensors/gyro.h"
 
 #include "rc_controls.h"
@@ -283,11 +284,14 @@ void processRcStickPositions()
     }
 #endif
 
+#if defined(USE_MAG)
     if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
         // Calibrating Mag
-        ENABLE_STATE(CALIBRATE_MAG);
+        compassStartCalibration();
+
         return;
     }
+#endif
 
 
     if (FLIGHT_MODE(ANGLE_MODE|HORIZON_MODE)) {

src/main/fc/runtime_config.h

@@ -113,7 +113,6 @@ extern uint16_t flightModeFlags;
 typedef enum {
     GPS_FIX_HOME   = (1 << 0),
     GPS_FIX        = (1 << 1),
-    CALIBRATE_MAG  = (1 << 2),
 } stateFlags_t;
 
 #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

src/main/msp/msp.c

@@ -963,7 +963,11 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
                 sbufWriteU16(dst, gyroRateDps(i));
             }
             for (int i = 0; i < 3; i++) {
+#if defined(USE_MAG)
                 sbufWriteU16(dst, lrintf(mag.magADC[i]));
+#else
+                sbufWriteU16(dst, 0);
+#endif
             }
         }
         break;
@@ -1515,7 +1519,11 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
 #endif
         sbufWriteU8(dst, gyroAlignment);
         sbufWriteU8(dst, gyroAlignment);  // Starting with 4.0 gyro and acc alignment are the same
+#if defined(USE_MAG)
         sbufWriteU8(dst, compassConfig()->mag_alignment);
+#else
+        sbufWriteU8(dst, 0);
+#endif
 
         // API 1.41 - Add multi-gyro indicator, selected gyro, and support for separate gyro 1 & 2 alignment
         sbufWriteU8(dst, getGyroDetectionFlags());
@@ -2278,7 +2286,11 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP,
         // maintain backwards compatibility for API < 1.41
         const uint8_t gyroAlignment = sbufReadU8(src);
         sbufReadU8(src);  // discard deprecated acc_align
+#if defined(USE_MAG)
         compassConfigMutable()->mag_alignment = sbufReadU8(src);
+#else
+        sbufReadU8(src);
+#endif
 
         if (sbufBytesRemaining(src) >= 3) {
             // API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2
@@ -2534,11 +2546,14 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, uint8_t cmdMSP,
         break;
 #endif
 
+#if defined(USE_MAG)
     case MSP_MAG_CALIBRATION:
-        if (!ARMING_FLAG(ARMED))
+        if (!ARMING_FLAG(ARMED)) {
-            ENABLE_STATE(CALIBRATE_MAG);
+            compassStartCalibration();
-        break;
+        }
+#endif
 
+        break;
     case MSP_EEPROM_WRITE:
         if (ARMING_FLAG(ARMED)) {
             return MSP_RESULT_ERROR;

src/main/sensors/compass.c

@@ -24,15 +24,15 @@
 
 #include "platform.h"
 
+#if defined(USE_MAG)
+
 #include "common/axis.h"
 
-#include "pg/pg.h"
+#include "config/config.h"
-#include "pg/pg_ids.h"
 
+#include "drivers/bus.h"
 #include "drivers/bus_i2c.h"
 #include "drivers/bus_spi.h"
-#include "drivers/bus.h"
-#include "drivers/accgyro/accgyro_mpu.h"
 #include "drivers/compass/compass.h"
 #include "drivers/compass/compass_ak8975.h"
 #include "drivers/compass/compass_ak8963.h"
@@ -42,18 +42,22 @@
 #include "drivers/compass/compass_lis3mdl.h"
 #include "drivers/io.h"
 #include "drivers/light_led.h"
+#include "drivers/time.h"
 
-#include "config/config.h"
 #include "fc/runtime_config.h"
 
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
 #include "sensors/boardalignment.h"
-#include "sensors/compass.h"
 #include "sensors/gyro.h"
 #include "sensors/sensors.h"
 
-#ifdef USE_HARDWARE_REVISION_DETECTION
+#include "compass.h"
-#include "hardware_revision.h"
+
-#endif
+static timeUs_t tCal = 0;
+static flightDynamicsTrims_t magZeroTempMin;
+static flightDynamicsTrims_t magZeroTempMax;
 
 magDev_t magDev;
 mag_t mag;                   // mag access functions
@@ -102,8 +106,6 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig)
     compassConfig->interruptTag = IO_TAG(MAG_INT_EXTI);
 }
 
-#if defined(USE_MAG)
-
 static int16_t magADCRaw[XYZ_AXIS_COUNT];
 static uint8_t magInit = 0;
 
@@ -319,11 +321,24 @@ bool compassIsHealthy(void)
     return (mag.magADC[X] != 0) && (mag.magADC[Y] != 0) && (mag.magADC[Z] != 0);
 }
 
+void compassStartCalibration(void)
+{
+    tCal = micros();
+    flightDynamicsTrims_t *magZero = &compassConfigMutable()->magZero;
+    for (int axis = 0; axis < 3; axis++) {
+        magZero->raw[axis] = 0;
+        magZeroTempMin.raw[axis] = mag.magADC[axis];
+        magZeroTempMax.raw[axis] = mag.magADC[axis];
+    }
+}
+
+bool isCompassCalibrationComplete(void)
+{
+    return tCal == 0;
+}
+
 void compassUpdate(timeUs_t currentTimeUs)
 {
-    static timeUs_t tCal = 0;
-    static flightDynamicsTrims_t magZeroTempMin;
-    static flightDynamicsTrims_t magZeroTempMax;
 
     magDev.read(&magDev, magADCRaw);
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@@ -336,16 +351,6 @@ void compassUpdate(timeUs_t currentTimeUs)
     }
 
     flightDynamicsTrims_t *magZero = &compassConfigMutable()->magZero;
-    if (STATE(CALIBRATE_MAG)) {
-        tCal = currentTimeUs;
-        for (int axis = 0; axis < 3; axis++) {
-            magZero->raw[axis] = 0;
-            magZeroTempMin.raw[axis] = mag.magADC[axis];
-            magZeroTempMax.raw[axis] = mag.magADC[axis];
-        }
-        DISABLE_STATE(CALIBRATE_MAG);
-    }
-
     if (magInit) {              // we apply offset only once mag calibration is done
         mag.magADC[X] -= magZero->raw[X];
         mag.magADC[Y] -= magZero->raw[Y];

src/main/sensors/compass.h

@@ -67,3 +67,6 @@ bool compassIsHealthy(void);
 void compassUpdate(timeUs_t currentTime);
 bool compassInit(void);
 void compassPreInit(void);
+void compassStartCalibration(void);
+bool isCompassCalibrationComplete(void);
+

src/main/target/COLIBRI_RACE/i2c_bst.c

@@ -539,9 +539,14 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
                accStartCalibration();
            break;
 #endif
+
+#if defined(USE_MAG)
         case BST_MAG_CALIBRATION:
-           if (!ARMING_FLAG(ARMED))
+           if (!ARMING_FLAG(ARMED)) {
-               ENABLE_STATE(CALIBRATE_MAG);
+               compassStartCalibration();
+           }
+#endif
+
            break;
         case BST_EEPROM_WRITE:
             if (ARMING_FLAG(ARMED)) {

src/test/unit/arming_prevention_unittest.cc

@@ -1092,5 +1092,7 @@ extern "C" {
     void pidSetItermReset(bool) {}
     void applyAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
     bool isFixedWing(void) { return false; }
+    void compassStartCalibration(void) {}
+    bool isCompassCalibrationComplete(void) { return true; }
     bool isUpright(void) { return mockIsUpright; }
 }

src/test/unit/rc_controls_unittest.cc

@@ -654,4 +654,5 @@ bool isTryingToArm(void) { return false; }
 void resetTryingToArm(void) {}
 void setLedProfile(uint8_t profile) { UNUSED(profile); }
 uint8_t getLedProfile(void) { return 0; }
+void compassStartCalibration(void) {}
 }

src/test/unit/vtx_unittest.cc

@@ -179,5 +179,7 @@ extern "C" {
     void pidSetItermReset(bool) {}
     void applyAccelerometerTrimsDelta(rollAndPitchTrims_t*) {}
     bool isFixedWing(void) { return false; }
+    void compassStartCalibration(void) {}
+    bool isCompassCalibrationComplete(void) { return true; }
     bool isUpright(void) { return true; }
 }