Add acc and gyro accumulators to improve attitude estimation

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