From c40bce02c26f745a8cd5b0124d9098977f009f6e Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Mon, 1 Oct 2018 23:39:51 +1300 Subject: [PATCH] Merge pull request #6844 from etracer65/dual_gyro_fixes Dual gyro fixes - fix non-reentrant code in gyro sensor update and hardcoded gyroSensor1 logic --- src/main/sensors/gyro.c | 108 +++++++++++++++++++++++++++--------- src/main/sensors/gyro_filter_impl.h | 7 +-- 2 files changed, 83 insertions(+), 32 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c48bf996d..fb11af1a0 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -89,15 +89,23 @@ FAST_RAM_ZERO_INIT gyro_t gyro; static FAST_RAM_ZERO_INIT uint8_t gyroDebugMode; static uint8_t gyroToUse = 0; +static FAST_RAM_ZERO_INIT bool overflowDetected; #ifdef USE_GYRO_OVERFLOW_CHECK static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask; #endif + +#ifdef USE_YAW_SPIN_RECOVERY +static FAST_RAM_ZERO_INIT bool yawSpinDetected; +#endif + static FAST_RAM_ZERO_INIT float accumulatedMeasurements[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float gyroPrevious[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT timeUs_t accumulatedMeasurementTimeUs; static FAST_RAM_ZERO_INIT timeUs_t accumulationLastTimeSampledUs; +static FAST_RAM_ZERO_INIT int16_t gyroSensorTemperature; + static bool gyroHasOverflowProtection = true; typedef struct gyroCalibration_s { @@ -925,9 +933,9 @@ FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis) static FAST_CODE_NOINLINE void handleOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) { const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyroSensor->gyroDev.scale; - if ((abs(gyro.gyroADCf[X]) < gyroOverflowResetRate) - && (abs(gyro.gyroADCf[Y]) < gyroOverflowResetRate) - && (abs(gyro.gyroADCf[Z]) < gyroOverflowResetRate)) { + if ((abs(gyroSensor->gyroDev.gyroADCf[X]) < gyroOverflowResetRate) + && (abs(gyroSensor->gyroDev.gyroADCf[Y]) < gyroOverflowResetRate) + && (abs(gyroSensor->gyroDev.gyroADCf[Z]) < gyroOverflowResetRate)) { // if we have 50ms of consecutive OK gyro vales, then assume yaw readings are OK again and reset overflowDetected // reset requires good OK values on all axes if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) { @@ -952,13 +960,13 @@ static FAST_CODE void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t curren // check for overflow in the axes set in overflowAxisMask gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE; const float gyroOverflowTriggerRate = GYRO_OVERFLOW_TRIGGER_THRESHOLD * gyroSensor->gyroDev.scale; - if (abs(gyro.gyroADCf[X]) > gyroOverflowTriggerRate) { + if (abs(gyroSensor->gyroDev.gyroADCf[X]) > gyroOverflowTriggerRate) { overflowCheck |= GYRO_OVERFLOW_X; } - if (abs(gyro.gyroADCf[Y]) > gyroOverflowTriggerRate) { + if (abs(gyroSensor->gyroDev.gyroADCf[Y]) > gyroOverflowTriggerRate) { overflowCheck |= GYRO_OVERFLOW_Y; } - if (abs(gyro.gyroADCf[Z]) > gyroOverflowTriggerRate) { + if (abs(gyroSensor->gyroDev.gyroADCf[Z]) > gyroOverflowTriggerRate) { overflowCheck |= GYRO_OVERFLOW_Z; } if (overflowCheck & overflowAxisMask) { @@ -977,7 +985,7 @@ static FAST_CODE void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t curren static FAST_CODE_NOINLINE void handleYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) { const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f; - if (abs(gyro.gyroADCf[Z]) < yawSpinResetRate) { + if (abs(gyroSensor->gyroDev.gyroADCf[Z]) < yawSpinResetRate) { // testing whether 20ms of consecutive OK gyro yaw values is enough if (cmpTimeUs(currentTimeUs, gyroSensor->yawSpinTimeUs) > 20000) { gyroSensor->yawSpinDetected = false; @@ -1003,7 +1011,7 @@ static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t current } else { #ifndef SIMULATOR_BUILD // check for spin on yaw axis only - if (abs(gyro.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) { + if (abs(gyroSensor->gyroDev.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) { gyroSensor->yawSpinDetected = true; gyroSensor->yawSpinTimeUs = currentTimeUs; } @@ -1051,10 +1059,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens return; } - const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs; - accumulationLastTimeSampledUs = currentTimeUs; - accumulatedMeasurementTimeUs += sampleDeltaUs; - #ifdef USE_GYRO_OVERFLOW_CHECK if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) { checkForOverflow(gyroSensor, currentTimeUs); @@ -1068,9 +1072,9 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens #endif if (gyroDebugMode == DEBUG_NONE) { - filterGyro(gyroSensor, sampleDeltaUs); + filterGyro(gyroSensor); } else { - filterGyroDebug(gyroSensor, sampleDeltaUs); + filterGyroDebug(gyroSensor); } #ifdef USE_GYRO_DATA_ANALYSE @@ -1078,11 +1082,18 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn); } #endif + +#if (!defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY)) + UNUSED(currentTimeUs); +#endif } FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) { -#ifdef USE_DUAL_GYRO + const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs; + accumulationLastTimeSampledUs = currentTimeUs; + accumulatedMeasurementTimeUs += sampleDeltaUs; + switch (gyroToUse) { case GYRO_CONFIG_USE_GYRO_1: gyroUpdateSensor(&gyroSensor1, currentTimeUs); @@ -1090,6 +1101,12 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X]; gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y]; gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z]; +#ifdef USE_GYRO_OVERFLOW_CHECK + overflowDetected = gyroSensor1.overflowDetected; +#endif +#ifdef USE_YAW_SPIN_RECOVERY + yawSpinDetected = gyroSensor1.yawSpinDetected; +#endif } DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]); @@ -1104,6 +1121,12 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) gyro.gyroADCf[X] = gyroSensor2.gyroDev.gyroADCf[X]; gyro.gyroADCf[Y] = gyroSensor2.gyroDev.gyroADCf[Y]; gyro.gyroADCf[Z] = gyroSensor2.gyroDev.gyroADCf[Z]; +#ifdef USE_GYRO_OVERFLOW_CHECK + overflowDetected = gyroSensor2.overflowDetected; +#endif +#ifdef USE_YAW_SPIN_RECOVERY + yawSpinDetected = gyroSensor2.yawSpinDetected; +#endif } DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]); @@ -1119,7 +1142,14 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) gyro.gyroADCf[X] = (gyroSensor1.gyroDev.gyroADCf[X] + gyroSensor2.gyroDev.gyroADCf[X]) / 2.0f; gyro.gyroADCf[Y] = (gyroSensor1.gyroDev.gyroADCf[Y] + gyroSensor2.gyroDev.gyroADCf[Y]) / 2.0f; gyro.gyroADCf[Z] = (gyroSensor1.gyroDev.gyroADCf[Z] + gyroSensor2.gyroDev.gyroADCf[Z]) / 2.0f; +#ifdef USE_GYRO_OVERFLOW_CHECK + overflowDetected = gyroSensor1.overflowDetected || gyroSensor2.overflowDetected; +#endif +#ifdef USE_YAW_SPIN_RECOVERY + yawSpinDetected = gyroSensor1.yawSpinDetected || gyroSensor2.yawSpinDetected; +#endif } + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]); DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]); DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X])); @@ -1135,12 +1165,14 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf(gyroSensor1.gyroDev.gyroADCf[Z] - gyroSensor2.gyroDev.gyroADCf[Z])); break; } -#else - gyroUpdateSensor(&gyroSensor1, currentTimeUs); - gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X]; - gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y]; - gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z]; -#endif + + if (!overflowDetected) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + // integrate using trapezium rule to avoid bias + accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * sampleDeltaUs; + gyroPrevious[axis] = gyro.gyroADCf[axis]; + } + } } bool gyroGetAccumulationAverage(float *accumulationAverage) @@ -1161,16 +1193,36 @@ bool gyroGetAccumulationAverage(float *accumulationAverage) } } +int16_t gyroReadSensorTemperature(gyroSensor_t gyroSensor) +{ + if (gyroSensor.gyroDev.temperatureFn) { + gyroSensor.gyroDev.temperatureFn(&gyroSensor.gyroDev, &gyroSensor.gyroDev.temperature); + } + return gyroSensor.gyroDev.temperature; +} + void gyroReadTemperature(void) { - if (gyroSensor1.gyroDev.temperatureFn) { - gyroSensor1.gyroDev.temperatureFn(&gyroSensor1.gyroDev, &gyroSensor1.gyroDev.temperature); + switch (gyroToUse) { + case GYRO_CONFIG_USE_GYRO_1: + gyroSensorTemperature = gyroReadSensorTemperature(gyroSensor1); + break; + +#ifdef USE_MULTI_GYRO + case GYRO_CONFIG_USE_GYRO_2: + gyroSensorTemperature = gyroReadSensorTemperature(gyroSensor2); + break; + + case GYRO_CONFIG_USE_GYRO_BOTH: + gyroSensorTemperature = MAX(gyroReadSensorTemperature(gyroSensor1), gyroReadSensorTemperature(gyroSensor2)); + break; +#endif // USE_MULTI_GYRO } } int16_t gyroGetTemperature(void) { - return gyroSensor1.gyroDev.temperature; + return gyroSensorTemperature; } int16_t gyroRateDps(int axis) @@ -1188,13 +1240,17 @@ int16_t gyroRateDps(int axis) bool gyroOverflowDetected(void) { - return gyroSensor1.overflowDetected; +#ifdef USE_GYRO_OVERFLOW_CHECK + return overflowDetected; +#else + return false; +#endif // USE_GYRO_OVERFLOW_CHECK } #ifdef USE_YAW_SPIN_RECOVERY bool gyroYawSpinDetected(void) { - return gyroSensor1.yawSpinDetected; + return yawSpinDetected; } #endif // USE_YAW_SPIN_RECOVERY diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index cd7430a5c..c32d13a09 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -1,4 +1,4 @@ -static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDelta_t sampleDeltaUs) +static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]); @@ -36,10 +36,5 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf)); gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf; - if (!gyroSensor->overflowDetected) { - // integrate using trapezium rule to avoid bias - accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs; - gyroPrevious[axis] = gyroADCf; - } } } -- 2.11.4.GIT