Moved acc accumulation code out of IMU into acc sensor

src/main/flight/imu.c

@@ -103,8 +103,6 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
 /* Asynchronous update accumulators */
 STATIC_FASTRAM float imuAccumulatedRate[XYZ_AXIS_COUNT];
 STATIC_FASTRAM timeUs_t imuAccumulatedRateTimeUs;
-STATIC_FASTRAM float imuAccumulatedAcc[XYZ_AXIS_COUNT];
-STATIC_FASTRAM int   imuAccumulatedAccCount;
 #endif
 
 #ifdef ASYNC_GYRO_PROCESSING
@@ -550,25 +548,6 @@ static void imuUpdateMeasuredRotationRate(void)
 #endif
 }
 
-/* Calculate measured acceleration in body frame cm/s/s */
-static void imuUpdateMeasuredAcceleration(void)
-{
-    int axis;
-
-#ifdef ASYNC_GYRO_PROCESSING
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        imuMeasuredAccelBF.A[axis] = imuAccumulatedAcc[axis] * GRAVITY_CMSS / imuAccumulatedAccCount;
-        imuAccumulatedAcc[axis] = 0;
-    }
-    imuAccumulatedAccCount = 0;;
-#else
-    /* Convert acceleration to cm/s/s */
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        imuMeasuredAccelBF.A[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
-    }
-#endif
-}
-
 #ifdef HIL
 void imuHILUpdate(void)
 {
@@ -602,10 +581,7 @@ void imuUpdateAccelerometer(void)
 #endif
 
 #ifdef ASYNC_GYRO_PROCESSING
-    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+    accUpdateAccumulatedMeasurements();
-        imuAccumulatedAcc[axis] += acc.accADCf[axis];
-    }
-    imuAccumulatedAccCount++;
 #endif
 }
 
@@ -620,7 +596,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
 #ifdef HIL
         if (!hilActive) {
             imuUpdateMeasuredRotationRate();    // Calculate gyro rate in body frame in rad/s
-            imuUpdateMeasuredAcceleration();  // Calculate accel in body frame in cm/s/s
+            accGetMeasuredAcceleration(&imuMeasuredAccelBF);  // Calculate accel in body frame in cm/s/s
             imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
         }
         else {
@@ -629,7 +605,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
         }
 #else
             imuUpdateMeasuredRotationRate();    // Calculate gyro rate in body frame in rad/s
-            imuUpdateMeasuredAcceleration();  // Calculate accel in body frame in cm/s/s
+            accGetMeasuredAcceleration(&imuMeasuredAccelBF);  // Calculate accel in body frame in cm/s/s
             imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
 #endif
     } else {

src/main/flight/imu.h

@@ -22,9 +22,6 @@
 #include "common/time.h"
 #include "config/parameter_group.h"
 
-#define GRAVITY_CMSS    980.665f
-#define GRAVITY_MSS     9.80665f
-
 extern t_fp_vector imuMeasuredAccelBF;         // cm/s/s
 extern t_fp_vector imuMeasuredRotationBF;       // rad/s
 

src/main/sensors/acceleration.c

@@ -454,6 +454,37 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f
     accADC[Z] = (accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096;
 }
 
+#ifdef ASYNC_GYRO_PROCESSING
+STATIC_FASTRAM float accumulatedMeasurements[XYZ_AXIS_COUNT];
+STATIC_FASTRAM int   accumulatedMeasurementCount;
+
+void accUpdateAccumulatedMeasurements(void)
+{
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        accumulatedMeasurements[axis] += acc.accADCf[axis];
+    }
+    accumulatedMeasurementCount++;
+}
+#endif
+
+/*
+ * Calculate measured acceleration in body frame in g
+ */
+void accGetMeasuredAcceleration(t_fp_vector *measuredAcc)
+{
+#ifdef ASYNC_GYRO_PROCESSING
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        measuredAcc->A[axis] = accumulatedMeasurements[axis] * GRAVITY_CMSS / accumulatedMeasurementCount;
+        accumulatedMeasurements[axis] = 0;
+    }
+    accumulatedMeasurementCount = 0;;
+#else
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        measuredAcc->A[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
+    }
+#endif
+}
+
 void accUpdate(void)
 {
     if (!acc.dev.readFn(&acc.dev)) {

src/main/sensors/acceleration.h

@@ -18,10 +18,14 @@
 #pragma once
 
 #include "common/axis.h"
+#include "common/maths.h"
 #include "config/parameter_group.h"
 #include "drivers/accgyro/accgyro.h"
 #include "sensors/sensors.h"
 
+#define GRAVITY_CMSS    980.665f
+#define GRAVITY_MSS     9.80665f
+
 // Type of accelerometer used/detected
 typedef enum {
     ACC_NONE = 0,
@@ -61,6 +65,8 @@ PG_DECLARE(accelerometerConfig_t, accelerometerConfig);
 bool accInit(uint32_t accTargetLooptime);
 bool accIsCalibrationComplete(void);
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
+void accUpdateAccumulatedMeasurements(void);
+void accGetMeasuredAcceleration(t_fp_vector *measuredAcc);
 void accUpdate(void);
 void accSetCalibrationValues(void);
 void accInitFilters(void);