Merge pull request cleanflight#1012 from martinbudden/bf_acc_init

Improved acc initialisation

src/main/config/config_master.h

@@ -78,7 +78,7 @@ typedef struct master_t {
 
     rollAndPitchTrims_t accelerometerTrims; // accelerometer trim
 
-    float acc_lpf_hz;                       // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
+    uint16_t acc_lpf_hz;                       // cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz
     accDeadband_t accDeadband;
     barometerConfig_t barometerConfig;
     uint8_t acc_unarmedcal;                 // turn automatic acc compensation on/off

src/main/io/serial_cli.c

@@ -820,7 +820,7 @@ const clivalue_t valueTable[] = {
 #endif
 
     { "acc_hardware",               VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.acc_hardware, .config.lookup = { TABLE_ACC_HARDWARE } },
-    { "acc_lpf_hz",                 VAR_FLOAT  | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0,  400 } },
+    { "acc_lpf_hz",                 VAR_UINT16 | MASTER_VALUE, &masterConfig.acc_lpf_hz, .config.minmax = { 0,  400 } },
     { "accxy_deadband",             VAR_UINT8  | MASTER_VALUE, &masterConfig.accDeadband.xy, .config.minmax = { 0,  100 } },
     { "accz_deadband",              VAR_UINT8  | MASTER_VALUE, &masterConfig.accDeadband.z, .config.minmax = { 0,  100 } },
     { "acc_unarmedcal",             VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &masterConfig.acc_unarmedcal, .config.lookup = { TABLE_OFF_ON } },

src/main/main.c

@@ -683,22 +683,7 @@ void main_init(void)
 
     if (sensors(SENSOR_ACC)) {
         setTaskEnabled(TASK_ACCEL, true);
-        switch (gyro.targetLooptime) {  // 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);
+        rescheduleTask(TASK_ACCEL, accSamplingInterval);
     }
 
     setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));

src/main/sensors/acceleration.c

@@ -41,7 +41,7 @@ int32_t accSmooth[XYZ_AXIS_COUNT];
 
 acc_t acc;                       // acc access functions
 sensor_align_e accAlign = 0;
-uint32_t accTargetLooptime;
+uint32_t accSamplingInterval;
 
 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.
 
@@ -52,9 +52,33 @@ extern bool AccInflightCalibrationActive;
 
 static flightDynamicsTrims_t *accelerationTrims;
 
-static float accLpfCutHz = 0;
+static uint16_t accLpfCutHz = 0;
 static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
-static bool accFilterInitialised = false;
+
+void accInit(uint32_t gyroSamplingInverval)
+{
+    // set the acc sampling interval according to the gyro sampling interval
+    switch (gyroSamplingInverval) {  // Switch statement kept in place to change acc sampling interval in the future
+    case 500:
+    case 375:
+    case 250:
+    case 125:
+        accSamplingInterval = 1000;
+        break;
+    case 1000:
+    default:
+#ifdef STM32F10X
+        accSamplingInterval = 1000;
+#else
+        accSamplingInterval = 1000;
+#endif
+    }
+    if (accLpfCutHz) {
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSamplingInterval);
+        }
+    }
+}
 
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 {
@@ -188,19 +212,8 @@ void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
     }
 
     if (accLpfCutHz) {
-        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], accLpfCutHz, accTargetLooptime);
-                }
-                accFilterInitialised = true;
-            }
-        }
-
-        if (accFilterInitialised) {
-            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis]));
-            }
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis]));
         }
     }
 
@@ -222,7 +235,12 @@ void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
     accelerationTrims = accelerationTrimsToUse;
 }
 
-void setAccelerationFilter(float initialAccLpfCutHz)
+void setAccelerationFilter(uint16_t initialAccLpfCutHz)
 {
     accLpfCutHz = initialAccLpfCutHz;
+    if (accSamplingInterval) {
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSamplingInterval);
+        }
+    }
 }

src/main/sensors/acceleration.h

@@ -34,7 +34,7 @@ typedef enum {
 
 extern sensor_align_e accAlign;
 extern acc_t acc;
-extern uint32_t accTargetLooptime;
+extern uint32_t accSamplingInterval;
 
 extern int32_t accSmooth[XYZ_AXIS_COUNT];
 
@@ -48,9 +48,12 @@ typedef union rollAndPitchTrims_u {
     rollAndPitchTrims_t_def values;
 } rollAndPitchTrims_t;
 
+void accInit(uint32_t gyroTargetLooptime);
 bool isAccelerationCalibrationComplete(void);
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims);
 void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims);
-void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse);
-void setAccelerationFilter(float initialAccLpfCutHz);
+union flightDynamicsTrims_u;
+void setAccelerationTrims(union flightDynamicsTrims_u *accelerationTrimsToUse);
+void setAccelerationFilter(uint16_t initialAccLpfCutHz);
+

src/main/sensors/gyro.c

@@ -103,7 +103,7 @@ void gyroSetCalibrationCycles(void)
     calibratingG = gyroCalculateCalibratingCycles();
 }
 
-static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
+static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
 {
     static int32_t g[3];
     static stdev_t var[3];
@@ -166,7 +166,7 @@ void gyroUpdate(void)
     alignSensors(gyroADC, gyroADC, gyroAlign);
 
     if (!isGyroCalibrationComplete()) {
-        performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
+        performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
     }
 
     applyGyroZero();

src/main/sensors/initialisation.c

@@ -277,7 +277,7 @@ bool detectGyro(void)
     return true;
 }
 
-static void detectAcc(accelerationSensor_e accHardwareToUse)
+static bool detectAcc(accelerationSensor_e accHardwareToUse)
 {
     accelerationSensor_e accHardware;
 
@@ -407,24 +407,22 @@ static void detectAcc(accelerationSensor_e accHardwareToUse)
 
 
     if (accHardware == ACC_NONE) {
-        return;
+        return false;
     }
 
     detectedSensors[SENSOR_INDEX_ACC] = accHardware;
     sensorsSet(SENSOR_ACC);
+    return true;
 }
 
-static void detectBaro(baroSensor_e baroHardwareToUse)
+#ifdef BARO
+static bool detectBaro(baroSensor_e baroHardwareToUse)
 {
-#ifndef BARO
-    UNUSED(baroHardwareToUse);
-#else
     // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
 
     baroSensor_e baroHardware = baroHardwareToUse;
 
 #ifdef USE_BARO_BMP085
-
     const bmp085Config_t *bmp085Config = NULL;
 
 #if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
@@ -476,15 +474,17 @@ static void detectBaro(baroSensor_e baroHardwareToUse)
     }
 
     if (baroHardware == BARO_NONE) {
-        return;
+        return false;
     }
 
     detectedSensors[SENSOR_INDEX_BARO] = baroHardware;
     sensorsSet(SENSOR_BARO);
-#endif
+    return true;
 }
+#endif
 
-static void detectMag(magSensor_e magHardwareToUse)
+#ifdef MAG
+static bool detectMag(magSensor_e magHardwareToUse)
 {
     magSensor_e magHardware;
 
@@ -571,12 +571,14 @@ static void detectMag(magSensor_e magHardwareToUse)
     }
 
     if (magHardware == MAG_NONE) {
-        return;
+        return false;
     }
 
     detectedSensors[SENSOR_INDEX_MAG] = magHardware;
     sensorsSet(SENSOR_MAG);
+    return true;
 }
+#endif
 
 void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
 {
@@ -613,32 +615,41 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
     if (!detectGyro()) {
         return false;
     }
-    detectAcc(accHardwareToUse);
-    detectBaro(baroHardwareToUse);
 
-    // Now time to init things, acc first
-    if (sensors(SENSOR_ACC)) {
-        acc.acc_1G = 256; // set default
-        acc.init(&acc);
-    }
+    // Now time to init things
     // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
     gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator);    // Set gyro sample rate before initialisation
-    gyro.init(gyroLpf);
-    gyroInit();
+    gyro.init(gyroLpf); // driver initialisation
+    gyroInit(); // sensor initialisation
 
-    detectMag(magHardwareToUse);
+    if (detectAcc(accHardwareToUse)) {
+        acc.acc_1G = 256; // set default
+        acc.init(&acc); // driver initialisation
+        accInit(gyro.targetLooptime); // sensor initialisation
+    }
 
-    reconfigureAlignment(sensorAlignmentConfig);
 
+    magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
+#ifdef MAG
     // FIXME extract to a method to reduce dependencies, maybe move to sensors_compass.c
-    if (sensors(SENSOR_MAG)) {
+    if (detectMag(magHardwareToUse)) {
         // calculate magnetic declination
         const int16_t deg = magDeclinationFromConfig / 100;
         const int16_t min = magDeclinationFromConfig % 100;
         magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
-    } else {
-        magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used.
     }
+#else
+    UNUSED(magHardwareToUse);
+    UNUSED(magDeclinationFromConfig);
+#endif
+
+#ifdef BARO
+    detectBaro(baroHardwareToUse);
+#else
+    UNUSED(baroHardwareToUse);
+#endif
+
+    reconfigureAlignment(sensorAlignmentConfig);
 
     return true;
 }