Merge pull request #2518 from mamalala/fix_acc_handling

Changed code for accelerometer handling

src/main/fc/boot.c

@@ -742,6 +742,22 @@ void configureScheduler(void)
 
     if (sensors(SENSOR_ACC)) {
         setTaskEnabled(TASK_ACCEL, true);
+        switch (gyroPeriodUs) {  // Switch statement kept in place to change acc rates in the future
+        case 500:
+        case 375:
+        case 250:
+        case 125:
+            accTargetLooptime = 1000;
+            break;
+        default:
+        case 1000:
+#ifdef STM32F10X
+            accTargetLooptime = 1000;
+#else
+            accTargetLooptime = 1000;
+#endif
+        }
+        rescheduleTask(TASK_ACCEL, accTargetLooptime);
     }
 
     setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));

src/main/flight/imu.c

@@ -62,7 +62,6 @@
 // http://gentlenav.googlecode.com/files/fastRotations.pdf
 #define SPIN_RATE_LIMIT 20
 
-int16_t accSmooth[XYZ_AXIS_COUNT];
 int32_t accSum[XYZ_AXIS_COUNT];
 
 uint32_t accTimeSum = 0;        // keep track for integration of acc
@@ -408,10 +407,8 @@ static bool isMagnetometerHealthy(void)
 
 static void imuCalculateEstimatedAttitude(void)
 {
-    static pt1Filter_t accLPFState[3];
     static uint32_t previousIMUUpdateTime;
     float rawYawError = 0;
-    int32_t axis;
     bool useAcc = false;
     bool useMag = false;
     bool useYaw = false;
@@ -420,15 +417,6 @@ static void imuCalculateEstimatedAttitude(void)
     uint32_t deltaT = currentTime - previousIMUUpdateTime;
     previousIMUUpdateTime = currentTime;
 
-    // Smooth and use only valid accelerometer readings
-    for (axis = 0; axis < 3; axis++) {
-        if (imuRuntimeConfig->acc_cut_hz > 0) {
-            accSmooth[axis] = pt1FilterApply4(&accLPFState[axis], accADC[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6f);
-        } else {
-            accSmooth[axis] = accADC[axis];
-        }
-    }
-
     if (imuIsAccelerometerHealthy()) {
         useAcc = true;
     }
@@ -470,9 +458,9 @@ void imuUpdateAttitude(void)
     if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
         imuCalculateEstimatedAttitude();
     } else {
-        accADC[X] = 0;
-        accADC[Y] = 0;
-        accADC[Z] = 0;
+        accSmooth[X] = 0;
+        accSmooth[Y] = 0;
+        accSmooth[Z] = 0;
     }
 }
 

src/main/flight/imu.h

@@ -21,7 +21,6 @@ extern int16_t throttleAngleCorrection;
 extern uint32_t accTimeSum;
 extern int accSumCount;
 extern float accVelScale;
-extern int16_t accSmooth[XYZ_AXIS_COUNT];
 extern int32_t accSum[XYZ_AXIS_COUNT];
 
 #define DEGREES_TO_DECIDEGREES(angle) (angle * 10)

src/main/sensors/acceleration.c

@@ -17,12 +17,14 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <math.h>
 
 #include <platform.h>
 
 #include "build/build_config.h"
 
 #include "common/axis.h"
+#include "common/filter.h"
 
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
@@ -44,6 +46,10 @@
 
 #include "sensors/acceleration.h"
 
+// TODO: acc-config is also in imu.c, make it accessible instead of using
+//       a second ptr to it here.
+static accelerometerConfig_t *accelerometerCFG;
+
 PG_REGISTER_PROFILE_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
 
 void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
@@ -64,12 +70,15 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
         .acc_unarmedcal = 1,
     );
     resetRollAndPitchTrims(&instance->accelerometerTrims);
+    accelerometerCFG = instance;
 }
 
 int32_t accADC[XYZ_AXIS_COUNT];
+int32_t accSmooth[XYZ_AXIS_COUNT];
 
 acc_t acc;                       // acc access functions
 sensor_align_e accAlign = 0;
+uint32_t accTargetLooptime;
 
 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.
 
@@ -81,6 +90,9 @@ extern bool AccInflightCalibrationActive;
 
 static flightDynamicsTrims_t *accelerationTrims;
 
+static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
+static bool accFilterInitialised = false;
+
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 {
     calibratingA = calibrationCyclesRequired;
@@ -113,10 +125,10 @@ void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
             a[axis] = 0;
 
         // Sum up CALIBRATING_ACC_CYCLES readings
-        a[axis] += accADC[axis];
+        a[axis] += accSmooth[axis];
 
         // Reset global variables to prevent other code from using un-calibrated data
-        accADC[axis] = 0;
+        accSmooth[axis] = 0;
         accelerationTrims->raw[axis] = 0;
     }
 
@@ -155,9 +167,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
             if (InflightcalibratingA == 50)
                 b[axis] = 0;
             // Sum up 50 readings
-            b[axis] += accADC[axis];
+            b[axis] += accSmooth[axis];
             // Clear global variables for next reading
-            accADC[axis] = 0;
+            accSmooth[axis] = 0;
             accelerationTrims->raw[axis] = 0;
         }
         // all values are measured
@@ -189,18 +201,9 @@ void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTri
 
 void applyAccelerationTrims(flightDynamicsTrims_t *accelerationTrims)
 {
-    accADC[X] -= accelerationTrims->raw[X];
-    accADC[Y] -= accelerationTrims->raw[Y];
-    accADC[Z] -= accelerationTrims->raw[Z];
-}
-
-static void convertRawACCADCReadingsToInternalType(int16_t *accADCRaw)
-{
-    int axis;
-
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        accADC[axis] = accADCRaw[axis];
-    }
+    accSmooth[X] -= accelerationTrims->raw[X];
+    accSmooth[Y] -= accelerationTrims->raw[Y];
+    accSmooth[Z] -= accelerationTrims->raw[Z];
 }
 
 void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
@@ -211,9 +214,28 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
         return;
     }
 
-    convertRawACCADCReadingsToInternalType(accADCRaw);
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        accSmooth[axis] = accADCRaw[axis];
+    }
+
+    if (accelerometerCFG->acc_cut_hz) {
+        if (!accFilterInitialised) {
+            if (accTargetLooptime) {  /* Initialisation needs to happen once sample rate is known */
+                for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                    biquadFilterInitLPF(&accFilter[axis], accelerometerCFG->acc_cut_hz, accTargetLooptime);
+                }
+                accFilterInitialised = true;
+            }
+        }
 
-    alignSensors(accADC, accADC, accAlign);
+        if (accFilterInitialised) {
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis]));
+            }
+        }
+    }
+
+    alignSensors(accSmooth, accSmooth, accAlign);
 
     if (!isAccelerationCalibrationComplete()) {
         performAcclerationCalibration(rollAndPitchTrims);

src/main/sensors/acceleration.h

@@ -37,6 +37,8 @@ extern sensor_align_e accAlign;
 extern acc_t acc;
 
 extern int32_t accADC[XYZ_AXIS_COUNT];
+extern int32_t accSmooth[XYZ_AXIS_COUNT];
+uint32_t accTargetLooptime;
 
 typedef struct rollAndPitchTrims_s {
     int16_t roll;

src/test/unit/flight_imu_unittest.cc

@@ -169,6 +169,7 @@ int32_t sonarAlt;
 int16_t sonarCfAltCm;
 int16_t sonarMaxAltWithTiltCm;
 int32_t accADC[XYZ_AXIS_COUNT];
+int32_t accSmooth[XYZ_AXIS_COUNT];
 int32_t gyroADC[XYZ_AXIS_COUNT];
 
 int16_t GPS_speed;

src/test/unit/msp_fc_unittest.cc

@@ -503,7 +503,7 @@ uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS];     // Carrier to Noise Ratio (Signal St
 int32_t gyroADC[XYZ_AXIS_COUNT];
 // form imu.c
 attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
-int16_t accSmooth[XYZ_AXIS_COUNT];
+int32_t accSmooth[XYZ_AXIS_COUNT];
 // from ledstrip.c
 void reevaluateLedConfig(void) {}
 bool setModeColor(ledModeIndex_e , int , int ) { return true; }