Compass improvements

src/main/fc/fc_msp.c

@@ -2023,6 +2023,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
  compassConfigMutable()->magZero.raw[X] = sbufReadU16(src);
  compassConfigMutable()->magZero.raw[Y] = sbufReadU16(src);
  compassConfigMutable()->magZero.raw[Z] = sbufReadU16(src);
+
+ // Set calibration flag if we have valid offsets in the config
+ compassUpdateCalibrationState();
 #else
  sbufReadU16(src);
  sbufReadU16(src);
@@ -2062,12 +2065,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
  return MSP_RESULT_ERROR;
  break;
 
+#ifdef USE_MAG
  case MSP_MAG_CALIBRATION:
  if (!ARMING_FLAG(ARMED))
- ENABLE_STATE(CALIBRATE_MAG);
+ compassStartCalibration();
  else
  return MSP_RESULT_ERROR;
  break;
+#endif
 
  case MSP_EEPROM_WRITE:
  if (!ARMING_FLAG(ARMED)) {

src/main/fc/rc_controls.c

@@ -58,6 +58,7 @@
 #include "sensors/barometer.h"
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
+#include "sensors/compass.h"
 #include "sensors/gyro.h"
 #include "sensors/acceleration.h"
 
@@ -323,10 +324,12 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
 
 
  // Calibrating Mag
+#ifdef USE_MAG
  if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
- ENABLE_STATE(CALIBRATE_MAG);
+ compassStartCalibration();
  return;
  }
+#endif
 
 
  // Accelerometer Trim

src/main/fc/runtime_config.h

@@ -91,7 +91,7 @@ extern uint32_t flightModeFlags;
 typedef enum {
  GPS_FIX_HOME = (1 << 0),
  GPS_FIX = (1 << 1),
- CALIBRATE_MAG = (1 << 2),
+ //CALIBRATE_MAG = (1 << 2),
  SMALL_ANGLE = (1 << 3),
  FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code
  ANTI_WINDUP = (1 << 5),

src/main/sensors/compass.c

@@ -76,8 +76,12 @@ PG_RESET_TEMPLATE(compassConfig_t, compassConfig,
 );
 
 #ifdef USE_MAG
-
-static uint8_t magUpdatedAtLeastOnce = 0;
+static bool magUpdatedAtLeastOnce = false;
+static bool isCalibrating = false;
+static timeUs_t calibrationStartedAt;
+static sensorCalibrationState_t calState; // Holds equasion state for static calibration
+static int32_t magCalPrev[XYZ_AXIS_COUNT]; // Holds either previous raw value when calculating static calibration offsets or previous de-biased value for dynamic calibration
+static float dynMagZerof[3] = {0.0f, 0.0f, 0.0f}; // Dynamic calibration values
 
 bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
 {
@@ -267,6 +271,9 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
 
 bool compassInit(void)
 {
+ // Set calibration flag if we have valid offsets in the config
+ compassUpdateCalibrationState();
+
 #ifdef USE_DUAL_MAG
  mag.dev.magSensorToUse = compassConfig()->mag_to_use;
 #else
@@ -284,6 +291,7 @@ bool compassInit(void)
  if (!ret) {
  addBootlogEvent2(BOOT_EVENT_MAG_INIT_FAILED, BOOT_EVENT_FLAGS_ERROR);
  sensorsClear(SENSOR_MAG);
+ return false;
  }
 
  if (compassConfig()->rollDeciDegrees != 0 ||
@@ -319,81 +327,140 @@ bool compassIsReady(void)
  return magUpdatedAtLeastOnce;
 }
 
-void compassUpdate(timeUs_t currentTimeUs)
+void compassUpdateCalibrationState(void)
 {
- static sensorCalibrationState_t calState;
- static timeUs_t calStartedAt = 0;
- static int16_t magPrev[XYZ_AXIS_COUNT];
-
  // Check magZero
- if ((compassConfig()->magZero.raw[X] == 0) && (compassConfig()->magZero.raw[Y] == 0) && (compassConfig()->magZero.raw[Z] == 0)) {
- DISABLE_STATE(COMPASS_CALIBRATED);
- }
- else {
+ if ((compassConfig()->magZero.raw[X] != 0) || (compassConfig()->magZero.raw[Y] != 0) || (compassConfig()->magZero.raw[Z] != 0)) {
  ENABLE_STATE(COMPASS_CALIBRATED);
+ return;
  }
 
- if (!mag.dev.read(&mag.dev)) {
- mag.magADC[X] = 0;
- mag.magADC[Y] = 0;
- mag.magADC[Z] = 0;
- return;
+ DISABLE_STATE(COMPASS_CALIBRATED);
+}
+
+void compassStartCalibration(void)
+{
+ isCalibrating = true;
+ calibrationStartedAt = micros();
+
+ for (int axis = 0; axis < 3; axis++) {
+ compassConfigMutable()->magZero.raw[axis] = 0;
+ magCalPrev[axis] = 0;
  }
 
- for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
- mag.magADC[axis] = mag.dev.magADCRaw[axis]; // int32_t copy to work with
+ beeper(BEEPER_ACTION_SUCCESS);
+
+ sensorCalibrationResetState(&calState);
+}
+
+static bool compassIsNewSampleSeparated(void)
+{
+ float diffMag = 0;
+ float avgMag = 0;
+
+ for (int axis = 0; axis < 3; axis++) {
+ diffMag += sq(mag.magADC[axis] - magCalPrev[axis]);
+ avgMag += sq(mag.magADC[axis] + magCalPrev[axis]) / 4.0f;
+ }
+
+ // sqrtf(diffMag / avgMag) is a rough approximation of tangent of angle between magADC and magPrev. tan(8 deg) = 0.14
+ if ((avgMag > 0.01f) && ((diffMag / avgMag) > (0.14f * 0.14f))) {
+ return true;
  }
 
- if (STATE(CALIBRATE_MAG)) {
- calStartedAt = currentTimeUs;
+ return false;
+}
+
+static void compassUpdatePreviousValue(void)
+{
+ for (int axis = 0; axis < 3; axis++) {
+ magCalPrev[axis] = mag.magADC[axis];
+ }
+}
+
+static void compassProcessStaticCalibration(timeUs_t currentTimeUs)
+{
+ if ((currentTimeUs - calibrationStartedAt) < (compassConfig()->magCalibrationTimeLimit * 1000000)) {
+ LED0_TOGGLE;
+
+ // Accumulate samples if they are far enough from each other
+ if (compassIsNewSampleSeparated()) {
+ // Accumulate sample
+ sensorCalibrationPushSampleForOffsetCalculation(&calState, mag.magADC);
+
+ // Update previous value
+ compassUpdatePreviousValue();
+ }
+ } else {
+ float magZerof[3];
+ sensorCalibrationSolveForOffset(&calState, magZerof);
 
  for (int axis = 0; axis < 3; axis++) {
- compassConfigMutable()->magZero.raw[axis] = 0;
- magPrev[axis] = 0;
+ compassConfigMutable()->magZero.raw[axis] = lrintf(magZerof[axis]);
  }
 
- beeper(BEEPER_ACTION_SUCCESS);
+ compassUpdateCalibrationState();
+ isCalibrating = false;
 
- sensorCalibrationResetState(&calState);
- DISABLE_STATE(CALIBRATE_MAG);
+ saveConfigAndNotify();
  }
+}
 
- if (calStartedAt != 0) {
- if ((currentTimeUs - calStartedAt) < (compassConfig()->magCalibrationTimeLimit * 1000000)) {
- LED0_TOGGLE;
-
- float diffMag = 0;
- float avgMag = 0;
+#define MAG_CALIBRATION_GAIN 0.1f
+#define COMPASS_OFS_LIMIT 2000
 
+static void compassProcessDynamicCalibration(void)
+{
+ // Apply dynamic compass offsets
+ mag.magADC[X] -= lrintf(dynMagZerof[X]);
+ mag.magADC[Y] -= lrintf(dynMagZerof[Y]);
+ mag.magADC[Z] -= lrintf(dynMagZerof[Z]);
+
+ // Check if we have enough separation to apply dynamic offset calibration
+ if (compassIsNewSampleSeparated()) {
+ // Slowly recalculate offsets
+ const float magNormCurr = sqrtf(sq(mag.magADC[0]) + sq(mag.magADC[1]) + sq(mag.magADC[2]));
+ const float magNormPrev = sqrtf(sq(magCalPrev[0]) + sq(magCalPrev[1]) + sq(magCalPrev[2]));
+ const float magNormDiff = sqrtf(sq(mag.magADC[0] - magCalPrev[0]) + sq(mag.magADC[1] - magCalPrev[1]) + sq(mag.magADC[2] - magCalPrev[2]));
+
+ // Sanity check to avoid dividing by small value
+ if (magNormDiff > 0.1f) {
  for (int axis = 0; axis < 3; 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;
+ dynMagZerof[axis] += MAG_CALIBRATION_GAIN * (mag.magADC[axis] - magCalPrev[axis]) * (magNormCurr - magNormPrev) / magNormDiff;
+ dynMagZerof[axis] = constrainf(dynMagZerof[axis], -COMPASS_OFS_LIMIT, COMPASS_OFS_LIMIT);
  }
+ }
 
- // sqrtf(diffMag / avgMag) is a rough approximation of tangent of angle between magADC and magPrev. tan(8 deg) = 0.14
- if ((avgMag > 0.01f) && ((diffMag / avgMag) > (0.14f * 0.14f))) {
- sensorCalibrationPushSampleForOffsetCalculation(&calState, mag.magADC);
+ // Update previous value
+ compassUpdatePreviousValue();
+ }
+}
 
- for (int axis = 0; axis < 3; axis++) {
- magPrev[axis] = mag.magADC[axis];
- }
- }
- } else {
- float magZerof[3];
- sensorCalibrationSolveForOffset(&calState, magZerof);
+void compassUpdate(timeUs_t currentTimeUs)
+{
+ if (!mag.dev.read(&mag.dev)) {
+ mag.magADC[X] = 0;
+ mag.magADC[Y] = 0;
+ mag.magADC[Z] = 0;
+ return;
+ }
 
-  for (int axis = 0; axis < 3; axis++) {
-  compassConfigMutable()->magZero.raw[axis] = lrintf(magZerof[axis]);
-  }
+ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+ mag.magADC[axis] = mag.dev.magADCRaw[axis]; // int32_t copy to work with
+ }
 
-  calStartedAt = 0;
-  saveConfigAndNotify();
- }
+ // Process calibration and de-biasing
+ if (isCalibrating) {
+ compassProcessStaticCalibration(currentTimeUs);
  }
  else {
+ // De-bias compass values
  mag.magADC[X] -= compassConfig()->magZero.raw[X];
  mag.magADC[Y] -= compassConfig()->magZero.raw[Y];
  mag.magADC[Z] -= compassConfig()->magZero.raw[Z];
+
+ // Process and apply dynamic calibration
+ compassProcessDynamicCalibration();
  }
 
  if (mag.dev.magAlign.useExternal) {

src/main/sensors/compass.h

@@ -71,3 +71,5 @@ bool compassInit(void);
 void compassUpdate(timeUs_t currentTimeUs);
 bool compassIsReady(void);
 bool compassIsHealthy(void);
+void compassStartCalibration(void);
+void compassUpdateCalibrationState(void);