Add acc and gyro accumulators to improve attitude estimation

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) {

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)

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);
 

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)

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);

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)

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);

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; }

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; }
 }