From 49130998038a60a9148547ed8f8ef76fab591518 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Wed, 16 May 2018 20:38:34 -0400 Subject: [PATCH] Gyro calibration cleanup Data type and variable name cleanup. Calculate the calibration sum using floats to prevent possibilities of future overflows. Rename the calibratingG element to cyclesRemaining to be more representative of its purpose. Change its data type to int32_t to avoid calculations with signed and unsigned variables. Fix the zero offset calculation from the calibration results to return a floating point result rather than using integer math. This may result in slight improvements in reduced gyro drift. --- src/main/sensors/gyro.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 4b4a987efa..a39f4eb3b2 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -96,9 +96,9 @@ static FAST_RAM timeUs_t accumulationLastTimeSampledUs; static bool gyroHasOverflowProtection = true; typedef struct gyroCalibration_s { - int32_t sum[XYZ_AXIS_COUNT]; + float sum[XYZ_AXIS_COUNT]; stdev_t var[XYZ_AXIS_COUNT]; - uint32_t calibratingG; + int32_t cyclesRemaining; } gyroCalibration_t; bool firstArmingCalibrationWasStarted = false; @@ -844,7 +844,7 @@ void gyroInitFilters(void) FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor) { - return gyroSensor->calibration.calibratingG == 0; + return gyroSensor->calibration.cyclesRemaining == 0; } FAST_CODE bool isGyroCalibrationComplete(void) @@ -869,22 +869,22 @@ FAST_CODE bool isGyroCalibrationComplete(void) static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration) { - return gyroCalibration->calibratingG == 1; + return gyroCalibration->cyclesRemaining == 1; } -static uint32_t gyroCalculateCalibratingCycles(void) +static int32_t gyroCalculateCalibratingCycles(void) { return (CALIBRATING_GYRO_TIME_US / gyro.targetLooptime); } static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration) { - return gyroCalibration->calibratingG == gyroCalculateCalibratingCycles(); + return gyroCalibration->cyclesRemaining == gyroCalculateCalibratingCycles(); } static void gyroSetCalibrationCycles(gyroSensor_t *gyroSensor) { - gyroSensor->calibration.calibratingG = gyroCalculateCalibratingCycles(); + gyroSensor->calibration.cyclesRemaining = gyroCalculateCalibratingCycles(); } void gyroStartCalibration(bool isFirstArmingCalibration) @@ -911,10 +911,10 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // Reset g[axis] at start of calibration if (isOnFirstGyroCalibrationCycle(&gyroSensor->calibration)) { - gyroSensor->calibration.sum[axis] = 0; + gyroSensor->calibration.sum[axis] = 0.0f; devClear(&gyroSensor->calibration.var[axis]); // gyroZero is set to zero until calibration complete - gyroSensor->gyroDev.gyroZero[axis] = 0; + gyroSensor->gyroDev.gyroZero[axis] = 0.0f; } // Sum up CALIBRATING_GYRO_TIME_US readings @@ -933,7 +933,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t } // please take care with exotic boardalignment !! - gyroSensor->gyroDev.gyroZero[axis] = (float)gyroSensor->calibration.sum[axis] / (float)gyroCalculateCalibratingCycles(); + gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles(); if (axis == Z) { gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100); } @@ -946,7 +946,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t beeper(BEEPER_GYRO_CALIBRATED); } } - --gyroSensor->calibration.calibratingG; + --gyroSensor->calibration.cyclesRemaining; }