Automatic compass mounting-orientation detection during calibration

src/main/CMakeLists.txt

@@ -468,6 +468,8 @@ main_sources(COMMON_SRC
     sensors/boardalignment.h
     sensors/compass.c
     sensors/compass.h
+    sensors/compass_orientation.c
+    sensors/compass_orientation.h
     sensors/diagnostics.c
     sensors/diagnostics.h
     sensors/gyro.c

src/main/build/debug.h

@@ -80,6 +80,7 @@ typedef enum {
     DEBUG_LULU,
     DEBUG_SBUS2,
     DEBUG_OSD_REFRESH,
+    DEBUG_MAG_CALIB,
     DEBUG_COUNT // also update debugModeNames in cli.c
 } debugType_e;
 

src/main/fc/cli.c

@@ -228,7 +228,8 @@ static const char *debugModeNames[DEBUG_COUNT] = {
     "GPS",
     "LULU",
     "SBUS2",
-    "OSD_REFRESH"
+    "OSD_REFRESH",
+    "MAG_CALIB"
 };
 
 /* Sensor names (used in lookup tables for *_hardware settings and in status

src/main/flight/imu.c

@@ -781,7 +781,11 @@ void imuUpdateTailSitter(void)
 static void imuCalculateEstimatedAttitude(float dT)
 {
 #if defined(USE_MAG)
-    const bool canUseMAG = sensors(SENSOR_MAG) && compassIsHealthy();
+    // Raw mag samples are not offset/gain corrected while a calibration spin is in
+    // progress (see compass.c) - fusing them would let mag hard-iron bias leak into
+    // the yaw estimate, which is also the attitude reference the calibration spin's
+    // own orientation-detection step depends on being bias-free.
+    const bool canUseMAG = sensors(SENSOR_MAG) && compassIsHealthy() && !compassIsCalibrating();
 #else
     const bool canUseMAG = false;
 #endif

src/main/sensors/compass.c

@@ -56,8 +56,11 @@
 #include "io/gps.h"
 #include "io/beeper.h"
 
+#include "flight/imu.h"
+
 #include "sensors/boardalignment.h"
 #include "sensors/compass.h"
+#include "sensors/compass_orientation.h"
 #include "sensors/gyro.h"
 #include "sensors/sensors.h"
 
@@ -82,6 +85,7 @@ PG_RESET_TEMPLATE(compassConfig_t, compassConfig,
 );
 
 static bool magUpdatedAtLeastOnce = false;
+static timeUs_t calStartedAt = 0;
 
 bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
 {
@@ -303,26 +307,12 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
     return true;
 }
 
-bool compassInit(void)
+// Rebuilds mag.dev.magAlign (useExternal / externalRotation / onBoard) from the
+// current compassConfig(). Called at init, and again whenever alignment settings
+// change at runtime (e.g. auto-detected orientation) so the cached rotation takes
+// effect immediately instead of only after the next reboot.
+static void compassRefreshAlignment(void)
 {
-#ifdef USE_DUAL_MAG
-    mag.dev.magSensorToUse = compassConfig()->mag_to_use;
-#else
-    mag.dev.magSensorToUse = 0;
-#endif
-
-    if (!compassDetect(&mag.dev, compassConfig()->mag_hardware)) {
-        return false;
-    }
-    // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
-    LED1_ON;
-    const bool ret = mag.dev.init(&mag.dev);
-    LED1_OFF;
-
-    if (!ret) {
-        sensorsClear(SENSOR_MAG);
-    }
-
     if (compassConfig()->rollDeciDegrees != 0 ||
         compassConfig()->pitchDeciDegrees != 0 ||
         compassConfig()->yawDeciDegrees != 0) {
@@ -344,6 +334,29 @@ bool compassInit(void)
             mag.dev.magAlign.onBoard = CW270_DEG_FLIP;  // The most popular default is 270FLIP for external mags
         }
     }
+}
+
+bool compassInit(void)
+{
+#ifdef USE_DUAL_MAG
+    mag.dev.magSensorToUse = compassConfig()->mag_to_use;
+#else
+    mag.dev.magSensorToUse = 0;
+#endif
+
+    if (!compassDetect(&mag.dev, compassConfig()->mag_hardware)) {
+        return false;
+    }
+    // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
+    LED1_ON;
+    const bool ret = mag.dev.init(&mag.dev);
+    LED1_OFF;
+
+    if (!ret) {
+        sensorsClear(SENSOR_MAG);
+    }
+
+    compassRefreshAlignment();
 
     return ret;
 }
@@ -368,6 +381,37 @@ bool compassIsCalibrationComplete(void)
     }
 }
 
+bool compassIsCalibrating(void)
+{
+    return calStartedAt != 0;
+}
+
+static void compassPushOrientationSample(void)
+{
+    const int16_t rawMagSample[3] = {
+        (int16_t)mag.magADC[X], (int16_t)mag.magADC[Y], (int16_t)mag.magADC[Z]
+    };
+    compassOrientationBufferPush(rawMagSample, &orientation);
+}
+
+static void compassApplyDetectedOrientation(int16_t rollDD, int16_t pitchDD, int16_t yawDD)
+{
+    // rollDeciDegrees/pitchDeciDegrees/yawDeciDegrees == 0,0,0 means
+    // "unconfigured, fall back to mag_align" (see align_mag_roll in
+    // settings.yaml) - a detected identity rotation would be silently
+    // discarded if written there. mag_align's ALIGN_DEFAULT(0) vs
+    // CW0_DEG(1) encoding can represent "explicitly no rotation"
+    // unambiguously, so route that one case through it instead.
+    if (rollDD == 0 && pitchDD == 0 && yawDD == 0) {
+        compassConfigMutable()->mag_align = CW0_DEG;
+    } else {
+        compassConfigMutable()->rollDeciDegrees = rollDD;
+        compassConfigMutable()->pitchDeciDegrees = pitchDD;
+        compassConfigMutable()->yawDeciDegrees = yawDD;
+    }
+    compassRefreshAlignment();
+}
+
 void compassUpdate(timeUs_t currentTimeUs)
 {
 #ifdef USE_SIMULATOR
@@ -377,9 +421,9 @@ void compassUpdate(timeUs_t currentTimeUs)
 	}
 #endif
     static sensorCalibrationState_t calState;
-    static timeUs_t calStartedAt = 0;
     static int16_t magPrev[XYZ_AXIS_COUNT];
     static int magAxisDeviation[XYZ_AXIS_COUNT];
+    static bool autoDetectOrientationThisSpin;
 
 #if defined(SITL_BUILD)
     ENABLE_STATE(COMPASS_CALIBRATED);
@@ -421,6 +465,11 @@ void compassUpdate(timeUs_t currentTimeUs)
 
         sensorCalibrationResetState(&calState);
         DISABLE_STATE(CALIBRATE_MAG);
+
+        autoDetectOrientationThisSpin = !STATE(COMPASS_CALIBRATED) && !STATE(ACCELEROMETER_CALIBRATED);
+        if (autoDetectOrientationThisSpin) {
+            compassOrientationBufferReset();
+        }
     }
 
     if (calStartedAt != 0) {
@@ -445,6 +494,10 @@ void compassUpdate(timeUs_t currentTimeUs)
             if ((avgMag > 0.01f) && ((diffMag / avgMag) > (0.14f * 0.14f))) {
                 sensorCalibrationPushSampleForOffsetCalculation(&calState, mag.magADC);
 
+                if (autoDetectOrientationThisSpin) {
+                    compassPushOrientationSample();
+                }
+
                 for (int axis = 0; axis < 3; axis++) {
                     magPrev[axis] = mag.magADC[axis];
                 }
@@ -466,6 +519,19 @@ void compassUpdate(timeUs_t currentTimeUs)
                 compassConfigMutable()->magGain[axis] = ABS(magAxisDeviation[axis] - compassConfig()->magZero.raw[axis]);
             }
 
+            if (autoDetectOrientationThisSpin) {
+                int16_t detectedRollDD, detectedPitchDD, detectedYawDD;
+                float confidence = 0.0f;
+                // magZerof (float, pre-rounding) rather than the already-rounded
+                // compassConfig()->magZero.raw is deliberate - the more precise
+                // value is available here, use it.
+                if (compassOrientationDetect(magZerof, compassConfig()->magGain, COMPASS_ORIENTATION_CONFIDENCE_MIN,
+                                              &detectedRollDD, &detectedPitchDD, &detectedYawDD, &confidence)) {
+                    compassApplyDetectedOrientation(detectedRollDD, detectedPitchDD, detectedYawDD);
+                }
+                DEBUG_SET(DEBUG_MAG_CALIB, 0, lrintf(confidence * 100.0f));
+            }
+
             calStartedAt = 0;
             saveConfigAndNotify();
         }

src/main/sensors/compass.h

@@ -81,5 +81,6 @@ void compassUpdate(timeUs_t currentTimeUs);
 bool compassIsReady(void);
 bool compassIsHealthy(void);
 bool compassIsCalibrationComplete(void);
+bool compassIsCalibrating(void);
 
 #endif