diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 9b21ef198c2..b0ff3996ba6 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -116,4 +116,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "DSHOT_TELEMETRY_COUNTS", "RPM_LIMIT", "RC_STATS", + "MAG_CALIB", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 0662e342a73..22605aace32 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -116,6 +116,7 @@ typedef enum { DEBUG_DSHOT_TELEMETRY_COUNTS, DEBUG_RPM_LIMIT, DEBUG_RC_STATS, + DEBUG_MAG_CALIB, DEBUG_COUNT } debugType_e; diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 75eb9b53d33..e4ab44ceff3 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -21,12 +21,16 @@ #include #include #include +#include #include "platform.h" #if defined(USE_MAG) +#include "build/debug.h" + #include "common/axis.h" +#include "common/maths.h" #include "config/config.h" @@ -62,9 +66,22 @@ #include "compass.h" +#define LAMBDA_MIN 0.95f // minimal adaptive forgetting factor, range: [0.90, 0.99], currently tuned for 200 Hz + // (TASK_COMPASS_RATE_HZ) and update rate of compassBiasEstimatorApply(), not the mag readout + // rate. + // offline evaluations showed that 10 Hz is ok, 50 Hz is better and above 100 Hz is good. + // TBC with online tests. +#define P0 1.0e2f // value to initialize P(0) = diag([P0, P0, P0, P0]), typically in range: (1, 1000) + +#define CALIBRATION_WAIT_US (15 * 1000 * 1000) // wait for movement to start and trigger the calibration routine in us +#define GYRO_NORM_SQUARED_MIN sq(DEGREES_TO_RADIANS(450.0f)) // minimal value that has to be reached so that the calibration routine starts in (rad/sec)^2, + // a relatively high value so that the calibration routine is not triggered too early +#define CALIBRATION_TIME_US (30 * 1000 * 1000) // duration of the calibration phase in us + static timeUs_t tCal = 0; -static flightDynamicsTrims_t magZeroTempMin; -static flightDynamicsTrims_t magZeroTempMax; +static bool didMovementStart = false; + +static compassBiasEstimator_t compassBiasEstimator; magDev_t magDev; mag_t mag; @@ -120,7 +137,6 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig) static int16_t magADCRaw[XYZ_AXIS_COUNT]; static int16_t magADCRawPrevious[XYZ_AXIS_COUNT]; -static uint8_t magInit = 0; void compassPreInit(void) { @@ -339,7 +355,6 @@ bool compassInit(void) LED1_ON; magDev.init(&magDev); LED1_OFF; - magInit = 1; magDev.magAlignment = alignment; @@ -349,6 +364,8 @@ bool compassInit(void) buildRotationMatrixFromAlignment(&compassConfig()->mag_customAlignment, &magDev.rotationMatrix); + compassBiasEstimatorInit(&compassBiasEstimator, LAMBDA_MIN, P0); + return true; } @@ -359,13 +376,16 @@ bool compassIsHealthy(void) void compassStartCalibration(void) { - tCal = micros(); + // starting now, the user has CALIBRATION_WAIT_US to start moving the quad and trigger the actual calibration routine + tCal = micros() + CALIBRATION_WAIT_US; flightDynamicsTrims_t *magZero = &compassConfigMutable()->magZero; - for (int axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { magZero->raw[axis] = 0; - magZeroTempMin.raw[axis] = mag.magADC[axis]; - magZeroTempMax.raw[axis] = mag.magADC[axis]; } + didMovementStart = false; + + // reset / update the compass bias estimator for faster convergence + compassBiasEstimatorUpdate(&compassBiasEstimator, LAMBDA_MIN, P0); } bool compassIsCalibrationComplete(void) @@ -402,36 +422,161 @@ uint32_t compassUpdate(timeUs_t currentTimeUs) } flightDynamicsTrims_t *magZero = &compassConfigMutable()->magZero; - if (magInit) { // we apply offset only once mag calibration is done - mag.magADC[X] -= magZero->raw[X]; - mag.magADC[Y] -= magZero->raw[Y]; - mag.magADC[Z] -= magZero->raw[Z]; - } if (tCal != 0) { - if ((currentTimeUs - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions + if (cmpTimeUs(tCal, currentTimeUs) > 0) { LED0_TOGGLE; - for (int axis = 0; axis < 3; axis++) { - if (mag.magADC[axis] < magZeroTempMin.raw[axis]) - magZeroTempMin.raw[axis] = mag.magADC[axis]; - if (mag.magADC[axis] > magZeroTempMax.raw[axis]) - magZeroTempMax.raw[axis] = mag.magADC[axis]; + + // it is assumed that the user has started to move the quad if squared norm of rotational speed vector is greater than GYRO_NORM_SQUARED_MIN + float gyroNormSquared = 0.0f; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroNormSquared += sq(DEGREES_TO_RADIANS(gyroGetFilteredDownsampled(axis))); + } + + // check if movement has started + if (!didMovementStart && gyroNormSquared > GYRO_NORM_SQUARED_MIN) { + didMovementStart = true; + // starting now, the user has CALIBRATION_TIME_US to move the quad in a figure of eight in all directions + tCal = micros() + CALIBRATION_TIME_US; + } + + // only start calibration after the user has started to move the quad + if (didMovementStart) { + compassBiasEstimatorApply(&compassBiasEstimator, mag.magADC); } } else { tCal = 0; - for (int axis = 0; axis < 3; axis++) { - magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets + // only copy the estimated bias and save it to the config if the user has actually triggered the calibration routine + if (didMovementStart) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + magZero->raw[axis] = lrintf(compassBiasEstimator.b[axis]); + } + saveConfigAndNotify(); } + } + } + + // remove bias + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + mag.magADC[axis] -= magZero->raw[axis]; + } - saveConfigAndNotify(); + if (debugMode == DEBUG_MAG_CALIB) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + // DEBUG 0-2: magADC[X], magADC[Y], magADC[Z] + DEBUG_SET(DEBUG_MAG_CALIB, axis, lrintf(mag.magADC[axis])); + // DEBUG 4-6: estimated magnetometer bias + DEBUG_SET(DEBUG_MAG_CALIB, axis + 4, lrintf(compassBiasEstimator.b[axis])); } + // DEBUG 3: norm / length of magADC, ideally the norm stays constant independent of the orientation of the quad + DEBUG_SET(DEBUG_MAG_CALIB, 3, lrintf(sqrtf(sq(mag.magADC[X]) + sq(mag.magADC[Y]) + sq(mag.magADC[Z])))); + // map adaptive forgetting factor lambda from (lambda_min, 1.0f) -> (0, 2000) + const float mapLambdaGain = 1.0f / (1.0f - compassBiasEstimator.lambda_min + 1.0e-6f) * 2.0e3f; + // DEBUG 7: adaptive forgetting factor lambda, after the transient phase it should converge to 2000 + DEBUG_SET(DEBUG_MAG_CALIB, 7, lrintf((compassBiasEstimator.lambda - compassBiasEstimator.lambda_min) * mapLambdaGain)); } nextPeriod = TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ) - COMPASS_READ_US * busyCount; // Reset the busy count busyCount = 0; - return nextPeriod; } + +// initialize the compass bias estimator +void compassBiasEstimatorInit(compassBiasEstimator_t *cBE, const float lambda_min, const float p0) +{ + memset(cBE, 0, sizeof(*cBE)); // zero contained IEEE754 floats + // create identity matrix + for (unsigned i = 0; i < 4; i++) { + cBE->U[i][i] = 1.0f; + } + + compassBiasEstimatorUpdate(cBE, lambda_min, p0); + + cBE->lambda = lambda_min; +} + +// reset / update the compass bias estimator, this can be used after the compass bias estimator did +// already run to achieve faster convergence for the next run +void compassBiasEstimatorUpdate(compassBiasEstimator_t *cBE, const float lambda_min, const float p0) +{ + cBE->lambda_min = lambda_min; + // update diagonal entries for faster convergence + for (unsigned i = 0; i < 4; i++) { + cBE->D[i] = p0; + } +} + +// apply one estimation step of the compass bias estimator +void compassBiasEstimatorApply(compassBiasEstimator_t *cBE, float *mag) +{ + // update phi + float phi[4]; + phi[0] = sq(mag[0]) + sq(mag[1]) + sq(mag[2]); + for (unsigned i = 0; i < 3; i++) { + phi[i + 1] = mag[i]; + } + + // update e + float e = 1.0f; + for (unsigned i = 0; i < 4; i++) { + e -= phi[i] * cBE->theta[i]; + } + + // U D U^T + float f[4]; + float v[4]; + for (unsigned i = 0; i < 4; i++) { + f[i] = 0.0f; + for (unsigned j = 0; j <= i; j++) { + f[i] += cBE->U[j][i] * phi[j]; + } + v[i] = cBE->D[i] * f[i]; + } + + // first iteration + float alpha[4]; + float k[4] = {0}; + alpha[0] = cBE->lambda + v[0] * f[0]; + cBE->D[0] /= alpha[0]; + k[0] = v[0]; + + // rest of the iterations + for (unsigned i = 1; i < 4; i++) { + alpha[i] = alpha[i - 1] + v[i] * f[i]; + cBE->D[i] *= alpha[i - 1] / (alpha[i] * cBE->lambda); + for (unsigned j = 0; j < i; j++) { + float dU = -(f[i] / alpha[i - 1]) * k[j]; + k[j] += v[i] * cBE->U[j][i]; + cBE->U[j][i] += dU; + } + k[i] += v[i]; + } + + // parameter-update + for (unsigned i = 0; i < 4; i++) { + cBE->theta[i] += (k[i] / alpha[3]) * e; + } + + // bias update + for (unsigned i = 0; i < 3; i++) { + cBE->b[i] = -0.5f * cBE->theta[i + 1] / cBE->theta[0]; + } + + // compute zn + float U_v; + float phiTrans_U_v = 0.0f; + for (unsigned i = 0; i < 4; i++) { + U_v = 0.0f; + for (unsigned j = i; j < 4; j++) { + U_v += cBE->U[i][j] * v[j]; + } + phiTrans_U_v += phi[i] * U_v; + } + float zn = cBE->lambda / (cBE->lambda + phiTrans_U_v); + + // update lambda + cBE->lambda = cBE->lambda_min + (1.0f - cBE->lambda_min) * sq(zn); +} #endif // USE_MAG diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 1d30d62e770..84f80f46c9e 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -62,6 +62,14 @@ typedef struct compassConfig_s { sensorAlignment_t mag_customAlignment; } compassConfig_t; +typedef struct compassBiasEstimator_s { + float lambda_min, lambda; + float b[3]; + float theta[4]; + float U[4][4]; + float D[4]; +} compassBiasEstimator_t; + PG_DECLARE(compassConfig_t, compassConfig); bool compassIsHealthy(void); @@ -70,4 +78,6 @@ bool compassInit(void); void compassPreInit(void); void compassStartCalibration(void); bool compassIsCalibrationComplete(void); - +void compassBiasEstimatorInit(compassBiasEstimator_t *compassBiasEstimator, const float lambda_min, const float p0); +void compassBiasEstimatorUpdate(compassBiasEstimator_t *compassBiasEstimator, const float lambda_min, const float p0); +void compassBiasEstimatorApply(compassBiasEstimator_t *cBE, float *mag);