diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 06ee84cbb9..7c29d87129 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -449,7 +449,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) float gyroAverage[XYZ_AXIS_COUNT]; gyroGetAccumulationAverage(gyroAverage); float accAverage[XYZ_AXIS_COUNT]; - accGetAccumulationAverage(accAverage); + if (!accGetAccumulationAverage(accAverage)) { + useAcc = false; + } imuMahonyAHRSupdate(deltaT * 1e-6f, DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]), useAcc, accAverage[X], accAverage[Y], accAverage[Z], diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 186f228b67..38db3c8179 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -69,9 +69,8 @@ acc_t acc; // acc access functions -static float accumulator[XYZ_AXIS_COUNT]; -static timeUs_t accumulatedTimeUs; -static timeUs_t lastTimeSampledUs; +static float accumulatedMeasurements[XYZ_AXIS_COUNT]; +static int accumulatedMeasurementCount; static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. @@ -469,6 +468,8 @@ static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrim void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims) { + UNUSED(currentTimeUs); + if (!acc.dev.readFn(&acc.dev)) { return; } @@ -497,28 +498,25 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims) applyAccelerationTrims(accelerationTrims); - const timeDelta_t sampleDeltaUs = currentTimeUs - lastTimeSampledUs; - lastTimeSampledUs = currentTimeUs; - accumulatedTimeUs += sampleDeltaUs; + ++accumulatedMeasurementCount; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - accumulator[axis] += acc.accSmooth[axis] * sampleDeltaUs; + accumulatedMeasurements[axis] += acc.accSmooth[axis]; } } -bool accGetAccumulationAverage(float *accumulation) +bool accGetAccumulationAverage(float *accumulationAverage) { - if (accumulatedTimeUs > 0) { - const float accumulatedTimeS = accumulatedTimeUs * 1e-6; + if (accumulatedMeasurementCount > 0) { // If we have gyro data accumulated, calculate average rate that will yield the same rotation for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - accumulation[axis] = accumulator[axis] / accumulatedTimeS; - accumulator[axis] = 0.0f; + accumulationAverage[axis] = accumulatedMeasurements[axis] / accumulatedMeasurementCount; + accumulatedMeasurements[axis] = 0.0f; } - accumulatedTimeUs = 0; + accumulatedMeasurementCount = 0; return true; } else { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - accumulation[axis] = 0.0f; + accumulationAverage[axis] = 0.0f; } return false; } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index e90e36ca17..596edf143a 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -77,9 +77,9 @@ gyro_t gyro; static uint8_t gyroDebugMode; -static float accumulator[XYZ_AXIS_COUNT]; -static timeUs_t accumulatedTimeUs; -static timeUs_t lastTimeSampledUs; +static float accumulatedMeasurements[XYZ_AXIS_COUNT]; +static timeUs_t accumulatedMeasurementTimeUs; +static timeUs_t accumulationLastTimeSampledUs; typedef struct gyroCalibration_s { int32_t sum[XYZ_AXIS_COUNT]; @@ -649,9 +649,9 @@ static void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn); #endif - const timeDelta_t sampleDeltaUs = currentTimeUs - lastTimeSampledUs; - lastTimeSampledUs = currentTimeUs; - accumulatedTimeUs += sampleDeltaUs; + const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs; + accumulationLastTimeSampledUs = currentTimeUs; + accumulatedMeasurementTimeUs += sampleDeltaUs; if (gyroDebugMode == DEBUG_NONE) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { @@ -664,7 +664,7 @@ static void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf); gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); gyro.gyroADCf[axis] = gyroADCf; - accumulator[axis] += gyroADCf * sampleDeltaUs; + accumulatedMeasurements[axis] += gyroADCf * sampleDeltaUs; } } else { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { @@ -696,7 +696,7 @@ static void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); gyro.gyroADCf[axis] = gyroADCf; - accumulator[axis] += gyroADCf * sampleDeltaUs; + accumulatedMeasurements[axis] += gyroADCf * sampleDeltaUs; } } } @@ -706,20 +706,19 @@ void gyroUpdate(timeUs_t currentTimeUs) gyroUpdateSensor(&gyroSensor1, currentTimeUs); } -bool gyroGetAccumulationAverage(float *accumulation) +bool gyroGetAccumulationAverage(float *accumulationAverage) { - if (accumulatedTimeUs > 0) { - const float accumulatedTimeS = accumulatedTimeUs * 1e-6; + if (accumulatedMeasurementTimeUs > 0) { // If we have gyro data accumulated, calculate average rate that will yield the same rotation for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - accumulation[axis] = accumulator[axis] / accumulatedTimeS; - accumulator[axis] = 0.0f; + accumulationAverage[axis] = accumulatedMeasurements[axis] / accumulatedMeasurementTimeUs; + accumulatedMeasurements[axis] = 0.0f; } - accumulatedTimeUs = 0; + accumulatedMeasurementTimeUs = 0; return true; } else { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - accumulation[axis] = 0.0f; + accumulationAverage[axis] = 0.0f; } return false; }