From e1ffd34d8f3575aae146e5a830068b4af4a1741c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 15 Feb 2017 07:07:51 +0000 Subject: [PATCH] Further work on making gyro reentrant --- src/main/sensors/gyro.c | 66 ++++++++++++++++++++--------------------- 1 file changed, 33 insertions(+), 33 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index ac374c7a14..30f1560c47 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -74,7 +74,13 @@ gyro_t gyro; STATIC_UNIT_TESTED gyroDev_t gyroDev0; static int16_t gyroTemperature0; -static uint16_t calibratingG = 0; +typedef struct gyroCalibration_s { + int32_t g[XYZ_AXIS_COUNT]; + stdev_t var[XYZ_AXIS_COUNT]; + uint16_t calibratingG; +} gyroCalibration_t; + +STATIC_UNIT_TESTED gyroCalibration_t gyroCalibration; static filterApplyFnPtr softLpfFilterApplyFn; static void *softLpfFilter[3]; @@ -416,12 +422,12 @@ void gyroInitFilters(void) bool isGyroCalibrationComplete(void) { - return calibratingG == 0; + return gyroCalibration.calibratingG == 0; } -static bool isOnFinalGyroCalibrationCycle(void) +static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration) { - return calibratingG == 1; + return gyroCalibration->calibratingG == 1; } static uint16_t gyroCalculateCalibratingCycles(void) @@ -429,56 +435,52 @@ static uint16_t gyroCalculateCalibratingCycles(void) return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES; } -static bool isOnFirstGyroCalibrationCycle(void) +static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration) { - return calibratingG == gyroCalculateCalibratingCycles(); + return gyroCalibration->calibratingG == gyroCalculateCalibratingCycles(); } void gyroSetCalibrationCycles(void) { - calibratingG = gyroCalculateCalibratingCycles(); + gyroCalibration.calibratingG = gyroCalculateCalibratingCycles(); } -STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, uint8_t gyroMovementCalibrationThreshold) +STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, gyroCalibration_t *gyroCalibration, uint8_t gyroMovementCalibrationThreshold) { - static int32_t g[3]; - static stdev_t var[3]; - for (int axis = 0; axis < 3; axis++) { - // Reset g[axis] at start of calibration - if (isOnFirstGyroCalibrationCycle()) { - g[axis] = 0; - devClear(&var[axis]); + if (isOnFirstGyroCalibrationCycle(gyroCalibration)) { + gyroCalibration->g[axis] = 0; + devClear(&gyroCalibration->var[axis]); } // Sum up CALIBRATING_GYRO_CYCLES readings - g[axis] += gyroDev->gyroADC[axis]; - devPush(&var[axis], gyroDev->gyroADC[axis]); + gyroCalibration->g[axis] += gyroDev->gyroADC[axis]; + devPush(&gyroCalibration->var[axis], gyroDev->gyroADC[axis]); // Reset global variables to prevent other code from using un-calibrated data gyroDev->gyroADC[axis] = 0; gyroDev->gyroZero[axis] = 0; - if (isOnFinalGyroCalibrationCycle()) { - const float dev = devStandardDeviation(&var[axis]); + if (isOnFinalGyroCalibrationCycle(gyroCalibration)) { + const float stddev = devStandardDeviation(&gyroCalibration->var[axis]); - DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(dev)); + DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(stddev)); // check deviation and startover in case the model was moved - if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) { + if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) { gyroSetCalibrationCycles(); return; } - gyroDev->gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); + gyroDev->gyroZero[axis] = (gyroCalibration->g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles(); } } - if (isOnFinalGyroCalibrationCycle()) { + if (isOnFinalGyroCalibrationCycle(gyroCalibration)) { schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics beeper(BEEPER_GYRO_CALIBRATED); } - calibratingG--; + gyroCalibration->calibratingG--; } @@ -493,14 +495,13 @@ static bool gyroUpdateISR(gyroDev_t* gyroDev) #endif gyroDev->dataReady = false; // move gyro data into 32-bit variables to avoid overflows in calculations - gyroDev->gyroADC[X] = gyroDev->gyroADCRaw[X]; - gyroDev->gyroADC[Y] = gyroDev->gyroADCRaw[Y]; - gyroDev->gyroADC[Z] = gyroDev->gyroADCRaw[Z]; + gyroDev->gyroADC[X] = (int32_t)gyroDev->gyroADCRaw[X] - (int32_t)gyroDev->gyroZero[X]; + gyroDev->gyroADC[Y] = (int32_t)gyroDev->gyroADCRaw[Y] - (int32_t)gyroDev->gyroZero[Y]; + gyroDev->gyroADC[Z] = (int32_t)gyroDev->gyroADCRaw[Z] - (int32_t)gyroDev->gyroZero[Z]; alignSensors(gyroDev->gyroADC, gyroDev->gyroAlign); for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - gyroDev->gyroADC[axis] -= gyroDev->gyroZero[axis]; // scale gyro output to degrees per second float gyroADCf = (float)gyroDev->gyroADC[axis] * gyroDev->scale; gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf); @@ -527,9 +528,9 @@ void gyroUpdate(void) } gyroDev0.dataReady = false; // move gyro data into 32-bit variables to avoid overflows in calculations - gyroDev0.gyroADC[X] = gyroDev0.gyroADCRaw[X]; - gyroDev0.gyroADC[Y] = gyroDev0.gyroADCRaw[Y]; - gyroDev0.gyroADC[Z] = gyroDev0.gyroADCRaw[Z]; + gyroDev0.gyroADC[X] = (int32_t)gyroDev0.gyroADCRaw[X] - (int32_t)gyroDev0.gyroZero[X]; + gyroDev0.gyroADC[Y] = (int32_t)gyroDev0.gyroADCRaw[Y] - (int32_t)gyroDev0.gyroZero[Y]; + gyroDev0.gyroADC[Z] = (int32_t)gyroDev0.gyroADCRaw[Z] - (int32_t)gyroDev0.gyroZero[Z]; alignSensors(gyroDev0.gyroADC, gyroDev0.gyroAlign); @@ -546,11 +547,10 @@ void gyroUpdate(void) debug[3] = (uint16_t)(micros() & 0xffff); #endif } else { - performGyroCalibration(&gyroDev0, gyroConfig()->gyroMovementCalibrationThreshold); + performGyroCalibration(&gyroDev0, &gyroCalibration, gyroConfig()->gyroMovementCalibrationThreshold); } for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - gyroDev0.gyroADC[axis] -= gyroDev0.gyroZero[axis]; // scale gyro output to degrees per second float gyroADCf = (float)gyroDev0.gyroADC[axis] * gyroDev0.scale;