From a045a9c672030da3ef48ec225351741154228c99 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 16 Nov 2019 23:54:37 +1300 Subject: [PATCH] Improved compass calibration. --- src/main/config/config.c | 2 + src/main/fc/core.c | 24 ++++------ src/main/fc/rc_controls.c | 6 ++- src/main/fc/runtime_config.h | 1 - src/main/msp/msp.c | 21 ++++++-- src/main/sensors/compass.c | 53 +++++++++++---------- src/main/sensors/compass.h | 3 ++ src/main/target/COLIBRI_RACE/i2c_bst.c | 9 +++- src/test/unit/arming_prevention_unittest.cc | 2 + src/test/unit/rc_controls_unittest.cc | 1 + src/test/unit/vtx_unittest.cc | 2 + 11 files changed, 79 insertions(+), 45 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 28cd468a1a..48b1f24573 100644 --- a/src/main/config/config.c +++ b/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); diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 6710f68d01..9743c4c2c5 100644 --- a/src/main/fc/core.c +++ b/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 - 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 ( + return !isGyroCalibrationComplete() #ifdef USE_ACC - !accIsCalibrationComplete() -#else - false + || (sensors(SENSOR_ACC) && !accIsCalibrationComplete()) #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 diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 6b37734f63..128972b75f 100644 --- a/src/main/fc/rc_controls.c +++ b/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)) { diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 4771204ee4..cecdddf852 100644 --- a/src/main/fc/runtime_config.h +++ b/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)) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 061c6ff86d..22582ca065 100644 --- a/src/main/msp/msp.c +++ b/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)) - ENABLE_STATE(CALIBRATE_MAG); - break; + if (!ARMING_FLAG(ARMED)) { + compassStartCalibration(); + } +#endif + break; case MSP_EEPROM_WRITE: if (ARMING_FLAG(ARMED)) { return MSP_RESULT_ERROR; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 9107a61c83..f593e896e2 100644 --- a/src/main/sensors/compass.c +++ b/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 "pg/pg_ids.h" +#include "config/config.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 "hardware_revision.h" -#endif +#include "compass.h" + +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]; diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 5411169b75..57d1b2bafc 100644 --- a/src/main/sensors/compass.h +++ b/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); + diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 8e505c5778..70c6a8e32b 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/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)) - ENABLE_STATE(CALIBRATE_MAG); + if (!ARMING_FLAG(ARMED)) { + compassStartCalibration(); + } +#endif + break; case BST_EEPROM_WRITE: if (ARMING_FLAG(ARMED)) { diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 1b8fea0840..a18b7ca265 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/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; } } diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 895e3e7086..158126b8eb 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/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) {} } diff --git a/src/test/unit/vtx_unittest.cc b/src/test/unit/vtx_unittest.cc index 33c9a8112d..f050baa4ae 100644 --- a/src/test/unit/vtx_unittest.cc +++ b/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; } }