diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 38b56b22ae0..725f3e00f80 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -364,6 +364,15 @@ bool gyroInit(void) return true; } +/* Zero calibration works on raw gyro readings, so the movement threshold has to be + * expressed in that sensor's LSB. Each gyro is asked for its own scale: a dual-IMU + * board may pair two unrelated parts, and the primary's sensitivity would be the + * wrong reference for the secondary. */ +static float gyroMovementThreshold(uint8_t index) +{ + return CALIBRATING_GYRO_MORON_THRESHOLD_DPS / gyroDev[index].scale; +} + void gyroStartCalibration(void) { if (!gyro.initialized) { @@ -377,7 +386,7 @@ void gyroStartCalibration(void) #endif gyroCalibrationComplete = false; - zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, CALIBRATING_GYRO_MORON_THRESHOLD, false); + zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, gyroMovementThreshold(0), false); } bool gyroIsCalibrationComplete(void) diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index ac588581a7e..bcbbd426c52 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -43,7 +43,10 @@ typedef union flightDynamicsTrims_u { #define CALIBRATING_PITOT_TIME_MS 4000 #define CALIBRATING_GYRO_TIME_MS 2000 #define CALIBRATING_ACC_TIME_MS 500 -#define CALIBRATING_GYRO_MORON_THRESHOLD 32 +// Gyro zero calibration movement threshold, in dps. Expressed as a physical rotation rate so +// that it does not depend on the gyro sensitivity. Equals the legacy threshold of 32 LSB at +// the default scale of 16.4 LSB/dps +#define CALIBRATING_GYRO_MORON_THRESHOLD_DPS (32.0f / 16.4f) // These bits have to be aligned with sensorIndex_e typedef enum {