From 22c672fa7d487cf7ec395ef1b1f1e2c1ece1fbff Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 26 Nov 2017 20:20:48 +0000 Subject: [PATCH] Add acc and gyro accumulators to improve attitude estimation --- src/main/fc/fc_core.c | 2 +- src/main/fc/fc_tasks.c | 4 +-- src/main/flight/imu.c | 9 ++++-- src/main/sensors/acceleration.c | 32 ++++++++++++++++++- src/main/sensors/acceleration.h | 4 ++- src/main/sensors/gyro.c | 34 +++++++++++++++++++-- src/main/sensors/gyro.h | 4 ++- src/test/unit/arming_prevention_unittest.cc | 2 +- src/test/unit/flight_imu_unittest.cc | 2 ++ 9 files changed, 79 insertions(+), 14 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 13e13e63fc..2a045948eb 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -726,7 +726,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) // 1 - pidController() // 2 - subTaskMotorUpdate() // 3 - subTaskMainSubprocesses() - gyroUpdate(); + gyroUpdate(currentTimeUs); DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs); if (pidUpdateCountdown) { diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index dd6d48b4cb..22d3af5fa2 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -130,9 +130,7 @@ void taskBatteryAlerts(timeUs_t currentTimeUs) #ifndef USE_OSD_SLAVE static void taskUpdateAccelerometer(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); - - accUpdate(&accelerometerConfigMutable()->accelerometerTrims); + accUpdate(currentTimeUs, &accelerometerConfigMutable()->accelerometerTrims); } static void taskUpdateRxMain(timeUs_t currentTimeUs) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index f0381acc35..06ee84cbb9 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -446,10 +446,13 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) // printf("[imu]deltaT = %u, imuDeltaT = %u, currentTimeUs = %u, micros64_real = %lu\n", deltaT, imuDeltaT, currentTimeUs, micros64_real()); deltaT = imuDeltaT; #endif - + float gyroAverage[XYZ_AXIS_COUNT]; + gyroGetAccumulationAverage(gyroAverage); + float accAverage[XYZ_AXIS_COUNT]; + accGetAccumulationAverage(accAverage); imuMahonyAHRSupdate(deltaT * 1e-6f, - DEGREES_TO_RADIANS(gyro.gyroADCf[X]), DEGREES_TO_RADIANS(gyro.gyroADCf[Y]), DEGREES_TO_RADIANS(gyro.gyroADCf[Z]), - useAcc, acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z], + DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]), + useAcc, accAverage[X], accAverage[Y], accAverage[Z], useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z], useYaw, rawYawError); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 804ac6b465..186f228b67 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -69,6 +69,10 @@ acc_t acc; // acc access functions +static float accumulator[XYZ_AXIS_COUNT]; +static timeUs_t accumulatedTimeUs; +static timeUs_t lastTimeSampledUs; + 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. extern uint16_t InflightcalibratingA; @@ -463,7 +467,7 @@ static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrim acc.accSmooth[Z] -= accelerationTrims->raw[Z]; } -void accUpdate(rollAndPitchTrims_t *rollAndPitchTrims) +void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims) { if (!acc.dev.readFn(&acc.dev)) { return; @@ -492,6 +496,32 @@ void accUpdate(rollAndPitchTrims_t *rollAndPitchTrims) } applyAccelerationTrims(accelerationTrims); + + const timeDelta_t sampleDeltaUs = currentTimeUs - lastTimeSampledUs; + lastTimeSampledUs = currentTimeUs; + accumulatedTimeUs += sampleDeltaUs; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + accumulator[axis] += acc.accSmooth[axis] * sampleDeltaUs; + } +} + +bool accGetAccumulationAverage(float *accumulation) +{ + if (accumulatedTimeUs > 0) { + const float accumulatedTimeS = accumulatedTimeUs * 1e-6; + // 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; + } + accumulatedTimeUs = 0; + return true; + } else { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + accumulation[axis] = 0.0f; + } + return false; + } } void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse) diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index e275391f36..b848aa5404 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -17,6 +17,7 @@ #pragma once +#include "common/time.h" #include "config/parameter_group.h" #include "drivers/accgyro/accgyro.h" #include "sensors/sensors.h" @@ -77,7 +78,8 @@ bool accInit(uint32_t gyroTargetLooptime); bool isAccelerationCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims); -void accUpdate(rollAndPitchTrims_t *rollAndPitchTrims); +void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims); +bool accGetAccumulationAverage(float *accumulation); union flightDynamicsTrims_u; void setAccelerationTrims(union flightDynamicsTrims_u *accelerationTrimsToUse); void accInitFilters(void); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 1b98a41316..e90e36ca17 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -77,6 +77,9 @@ gyro_t gyro; static uint8_t gyroDebugMode; +static float accumulator[XYZ_AXIS_COUNT]; +static timeUs_t accumulatedTimeUs; +static timeUs_t lastTimeSampledUs; typedef struct gyroCalibration_s { int32_t sum[XYZ_AXIS_COUNT]; @@ -613,7 +616,7 @@ int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis) } #endif -void gyroUpdateSensor(gyroSensor_t *gyroSensor) +static void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { return; @@ -646,6 +649,10 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor) gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn); #endif + const timeDelta_t sampleDeltaUs = currentTimeUs - lastTimeSampledUs; + lastTimeSampledUs = currentTimeUs; + accumulatedTimeUs += sampleDeltaUs; + if (gyroDebugMode == DEBUG_NONE) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch @@ -657,6 +664,7 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor) gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf); gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); gyro.gyroADCf[axis] = gyroADCf; + accumulator[axis] += gyroADCf * sampleDeltaUs; } } else { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { @@ -688,13 +696,33 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor) gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); gyro.gyroADCf[axis] = gyroADCf; + accumulator[axis] += gyroADCf * sampleDeltaUs; } } } -void gyroUpdate(void) +void gyroUpdate(timeUs_t currentTimeUs) { - gyroUpdateSensor(&gyroSensor1); + gyroUpdateSensor(&gyroSensor1, currentTimeUs); +} + +bool gyroGetAccumulationAverage(float *accumulation) +{ + if (accumulatedTimeUs > 0) { + const float accumulatedTimeS = accumulatedTimeUs * 1e-6; + // 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; + } + accumulatedTimeUs = 0; + return true; + } else { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + accumulation[axis] = 0.0f; + } + return false; + } } void gyroReadTemperature(void) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 801fe6bc9d..803953a39a 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -18,6 +18,7 @@ #pragma once #include "common/axis.h" +#include "common/time.h" #include "config/parameter_group.h" #include "drivers/bus.h" #include "drivers/sensor.h" @@ -69,7 +70,8 @@ PG_DECLARE(gyroConfig_t, gyroConfig); bool gyroInit(void); void gyroInitFilters(void); -void gyroUpdate(void); +void gyroUpdate(timeUs_t currentTimeUs); +bool gyroGetAccumulationAverage(float *accumulation); const busDevice_t *gyroSensorBus(void); struct mpuConfiguration_s; const struct mpuConfiguration_s *gyroMpuConfiguration(void); diff --git a/src/test/unit/arming_prevention_unittest.cc b/src/test/unit/arming_prevention_unittest.cc index 26f8f8a113..0f5e2d3f0a 100644 --- a/src/test/unit/arming_prevention_unittest.cc +++ b/src/test/unit/arming_prevention_unittest.cc @@ -635,7 +635,7 @@ extern "C" { void writeServos(void) {}; void calculateRxChannelsAndUpdateFailsafe(timeUs_t) {} bool isMixerUsingServos(void) { return false; } - void gyroUpdate(void) {} + void gyroUpdate(timeUs_t) {} timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; } void updateRSSI(timeUs_t) {} bool failsafeIsMonitoring(void) { return false; } diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index 84b66c19a1..80b11af562 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -239,4 +239,6 @@ uint32_t micros(void) { return 0; } bool isBaroCalibrationComplete(void) { return true; } void performBaroCalibrationCycle(void) {} int32_t baroCalculateAltitude(void) { return 0; } +bool gyroGetAccumulationAverage(float *) { return false; } +bool accGetAccumulationAverage(float *) { return false; } }