diff --git a/docs/papers/MagnetometerOffsetNullingRevisited.pdf b/docs/papers/MagnetometerOffsetNullingRevisited.pdf new file mode 100644 index 00000000000..caa96c57bb9 Binary files /dev/null and b/docs/papers/MagnetometerOffsetNullingRevisited.pdf differ diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 906cdf1271b..844f8c948ad 100644 --- a/src/main/fc/fc_msp.c +++ b/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)) { diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index f7c3d49b29f..656a3a6144c 100644 --- a/src/main/fc/rc_controls.c +++ b/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 diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 84cbea9e958..7ff3b8c1153 100644 --- a/src/main/fc/runtime_config.h +++ b/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), diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index b5f1dcd394d..048ae9071a0 100644 --- a/src/main/sensors/compass.c +++ b/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) { diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index a09cad89111..2043ce6656d 100644 --- a/src/main/sensors/compass.h +++ b/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);