Split acceleration code into init and runtime to save flash space

make/source.mk

@@ -128,6 +128,7 @@ COMMON_SRC = \
             rx/fport.c \
             rx/msp_override.c \
             sensors/acceleration.c \
+            sensors/acceleration_init.c \
             sensors/boardalignment.c \
             sensors/compass.c \
             sensors/gyro.c \

src/main/sensors/acceleration.c

@@ -20,8 +20,6 @@
 
 #include <stdbool.h>
 #include <stdint.h>
-#include <string.h>
-#include <math.h>
 
 #include "platform.h"
 
@@ -33,473 +31,15 @@
 #include "common/filter.h"
 #include "common/utils.h"
 
-#include "config/config_reset.h"
 #include "config/feature.h"
 
-#include "drivers/accgyro/accgyro.h"
+#include "sensors/acceleration_init.h"
-#include "drivers/accgyro/accgyro_fake.h"
-#include "drivers/accgyro/accgyro_mpu.h"
-#include "drivers/accgyro/accgyro_mpu3050.h"
-#include "drivers/accgyro/accgyro_mpu6050.h"
-#include "drivers/accgyro/accgyro_mpu6500.h"
-#include "drivers/accgyro/accgyro_spi_bmi160.h"
-#include "drivers/accgyro/accgyro_spi_bmi270.h"
-#include "drivers/accgyro/accgyro_spi_icm20649.h"
-#include "drivers/accgyro/accgyro_spi_icm20689.h"
-#include "drivers/accgyro/accgyro_spi_icm42605.h"
-#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
-#include "drivers/accgyro/accgyro_spi_mpu6000.h"
-#include "drivers/accgyro/accgyro_spi_mpu6500.h"
-#include "drivers/accgyro/accgyro_spi_mpu9250.h"
-
-#ifdef USE_ACC_ADXL345
-#include "drivers/accgyro_legacy/accgyro_adxl345.h"
-#endif
-
-#ifdef USE_ACC_BMA280
-#include "drivers/accgyro_legacy/accgyro_bma280.h"
-#endif
-
-#ifdef USE_ACC_LSM303DLHC
-#include "drivers/accgyro_legacy/accgyro_lsm303dlhc.h"
-#endif
-
-#ifdef USE_ACC_MMA8452
-#include "drivers/accgyro_legacy/accgyro_mma845x.h"
-#endif
-
-#include "drivers/bus_spi.h"
-
-#include "config/config.h"
-#include "fc/runtime_config.h"
-
-#include "io/beeper.h"
-
-#include "pg/gyrodev.h"
-#include "pg/pg.h"
-#include "pg/pg_ids.h"
-
 #include "sensors/boardalignment.h"
-#include "sensors/gyro.h"
-#include "sensors/gyro_init.h"
-#include "sensors/sensors.h"
 
 #include "acceleration.h"
 
-#define CALIBRATING_ACC_CYCLES              400
-
 FAST_DATA_ZERO_INIT acc_t acc;                       // acc access functions
 
-void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
-{
-    RESET_CONFIG_2(rollAndPitchTrims_t, rollAndPitchTrims,
-        .values.roll = 0,
-        .values.pitch = 0,
-    );
-}
-
-static void setConfigCalibrationCompleted(void)
-{
-    accelerometerConfigMutable()->accZero.values.calibrationCompleted = 1;
-}
-
-bool accHasBeenCalibrated(void)
-{
-    return accelerometerConfig()->accZero.values.calibrationCompleted;
-}
-
-void accResetRollAndPitchTrims(void)
-{
-    resetRollAndPitchTrims(&accelerometerConfigMutable()->accelerometerTrims);
-}
-
-static void resetFlightDynamicsTrims(flightDynamicsTrims_t *accZero)
-{
-    accZero->values.roll = 0;
-    accZero->values.pitch = 0;
-    accZero->values.yaw = 0;
-    accZero->values.calibrationCompleted = 0;
-}
-
-void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
-{
-    RESET_CONFIG_2(accelerometerConfig_t, instance,
-        .acc_lpf_hz = 10,
-        .acc_hardware = ACC_DEFAULT,
-        .acc_high_fsr = false,
-    );
-    resetRollAndPitchTrims(&instance->accelerometerTrims);
-    resetFlightDynamicsTrims(&instance->accZero);
-}
-
-PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 2);
-
-extern uint16_t InflightcalibratingA;
-extern bool AccInflightCalibrationMeasurementDone;
-extern bool AccInflightCalibrationSavetoEEProm;
-extern bool AccInflightCalibrationActive;
-
-static float accumulatedMeasurements[XYZ_AXIS_COUNT];
-static int accumulatedMeasurementCount;
-
-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.
-
-static flightDynamicsTrims_t *accelerationTrims;
-
-static uint16_t accLpfCutHz = 0;
-static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
-
-bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
-{
-    accelerationSensor_e accHardware = ACC_NONE;
-
-#ifdef USE_ACC_ADXL345
-    drv_adxl345_config_t acc_params;
-#endif
-
-retry:
-
-    switch (accHardwareToUse) {
-    case ACC_DEFAULT:
-        FALLTHROUGH;
-
-#ifdef USE_ACC_ADXL345
-    case ACC_ADXL345: // ADXL345
-        acc_params.useFifo = false;
-        acc_params.dataRate = 800; // unused currently
-        if (adxl345Detect(&acc_params, dev)) {
-            accHardware = ACC_ADXL345;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-#ifdef USE_ACC_LSM303DLHC
-    case ACC_LSM303DLHC:
-        if (lsm303dlhcAccDetect(dev)) {
-            accHardware = ACC_LSM303DLHC;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-#ifdef USE_ACC_MPU6050
-    case ACC_MPU6050: // MPU6050
-        if (mpu6050AccDetect(dev)) {
-            accHardware = ACC_MPU6050;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-#ifdef USE_ACC_MMA8452
-    case ACC_MMA8452: // MMA8452
-        if (mma8452Detect(dev)) {
-            accHardware = ACC_MMA8452;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-#ifdef USE_ACC_BMA280
-    case ACC_BMA280: // BMA280
-        if (bma280Detect(dev)) {
-            accHardware = ACC_BMA280;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-#ifdef USE_ACC_SPI_MPU6000
-    case ACC_MPU6000:
-        if (mpu6000SpiAccDetect(dev)) {
-            accHardware = ACC_MPU6000;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-#ifdef USE_ACC_SPI_MPU9250
-    case ACC_MPU9250:
-        if (mpu9250SpiAccDetect(dev)) {
-            accHardware = ACC_MPU9250;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-    case ACC_MPU6500:
-    case ACC_ICM20601:
-    case ACC_ICM20602:
-    case ACC_ICM20608G:
-#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
-#ifdef USE_ACC_SPI_MPU6500
-        if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) {
-#else
-        if (mpu6500AccDetect(dev)) {
-#endif
-            switch (dev->mpuDetectionResult.sensor) {
-            case MPU_9250_SPI:
-                accHardware = ACC_MPU9250;
-                break;
-            case ICM_20601_SPI:
-                accHardware = ACC_ICM20601;
-                break;
-            case ICM_20602_SPI:
-                accHardware = ACC_ICM20602;
-                break;
-            case ICM_20608_SPI:
-                accHardware = ACC_ICM20608G;
-                break;
-            default:
-                accHardware = ACC_MPU6500;
-            }
-            break;
-        }
-#endif
-        FALLTHROUGH;
-
-#ifdef USE_ACC_SPI_ICM20649
-    case ACC_ICM20649:
-        if (icm20649SpiAccDetect(dev)) {
-            accHardware = ACC_ICM20649;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-#ifdef USE_ACC_SPI_ICM20689
-    case ACC_ICM20689:
-        if (icm20689SpiAccDetect(dev)) {
-            accHardware = ACC_ICM20689;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-#ifdef USE_ACC_SPI_ICM42605
-    case ACC_ICM42605:
-        if (icm42605SpiAccDetect(dev)) {
-            accHardware = ACC_ICM42605;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-#ifdef USE_ACCGYRO_BMI160
-    case ACC_BMI160:
-        if (bmi160SpiAccDetect(dev)) {
-            accHardware = ACC_BMI160;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-#ifdef USE_ACCGYRO_BMI270
-    case ACC_BMI270:
-        if (bmi270SpiAccDetect(dev)) {
-            accHardware = ACC_BMI270;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-#ifdef USE_ACCGYRO_LSM6DSO
-    case ACC_LSM6DSO:
-        if (lsm6dsoSpiAccDetect(dev)) {
-            accHardware = ACC_LSM6DSO;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-#ifdef USE_FAKE_ACC
-    case ACC_FAKE:
-        if (fakeAccDetect(dev)) {
-            accHardware = ACC_FAKE;
-            break;
-        }
-        FALLTHROUGH;
-#endif
-
-    default:
-    case ACC_NONE: // disable ACC
-        accHardware = ACC_NONE;
-        break;
-    }
-
-    // Found anything? Check if error or ACC is really missing.
-    if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) {
-        // Nothing was found and we have a forced sensor that isn't present.
-        accHardwareToUse = ACC_DEFAULT;
-        goto retry;
-    }
-
-    if (accHardware == ACC_NONE) {
-        return false;
-    }
-
-    detectedSensors[SENSOR_INDEX_ACC] = accHardware;
-    sensorsSet(SENSOR_ACC);
-    return true;
-}
-
-void accInitFilters(void)
-{
-    // Only set the lowpass cutoff if the ACC sample rate is detected otherwise
-    // the filter initialization is not defined (sample rate = 0)
-    accLpfCutHz = (acc.sampleRateHz) ? accelerometerConfig()->acc_lpf_hz : 0;
-    if (accLpfCutHz) {
-        const uint32_t accSampleTimeUs = 1e6 / acc.sampleRateHz;
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accSampleTimeUs);
-        }
-    }
-}
-
-bool accInit(uint16_t accSampleRateHz)
-{
-    memset(&acc, 0, sizeof(acc));
-    // copy over the common gyro mpu settings
-    acc.dev.bus = *gyroSensorBus();
-    acc.dev.mpuDetectionResult = *gyroMpuDetectionResult();
-    acc.dev.acc_high_fsr = accelerometerConfig()->acc_high_fsr;
-
-    // Copy alignment from active gyro, as all production boards use acc-gyro-combi chip.
-    // Exceptions are STM32F3DISCOVERY and STM32F411DISCOVERY, and (may be) handled in future enhancement.
-
-    sensor_align_e alignment = gyroDeviceConfig(0)->alignment;
-    const sensorAlignment_t* customAlignment = &gyroDeviceConfig(0)->customAlignment;
-
-#ifdef USE_MULTI_GYRO
-    if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) {
-        alignment = gyroDeviceConfig(1)->alignment;
-
-        customAlignment = &gyroDeviceConfig(1)->customAlignment;
-    }
-#endif
-    acc.dev.accAlign = alignment;
-    buildRotationMatrixFromAlignment(customAlignment, &acc.dev.rotationMatrix);
-
-    if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
-        return false;
-    }
-    acc.dev.acc_1G = 256; // set default
-    acc.dev.initFn(&acc.dev); // driver initialisation
-    acc.dev.acc_1G_rec = 1.0f / acc.dev.acc_1G;
-
-    acc.sampleRateHz = accSampleRateHz;
-    accInitFilters();
-    return true;
-}
-
-void accStartCalibration(void)
-{
-    calibratingA = CALIBRATING_ACC_CYCLES;
-}
-
-bool accIsCalibrationComplete(void)
-{
-    return calibratingA == 0;
-}
-
-static bool isOnFinalAccelerationCalibrationCycle(void)
-{
-    return calibratingA == 1;
-}
-
-static bool isOnFirstAccelerationCalibrationCycle(void)
-{
-    return calibratingA == CALIBRATING_ACC_CYCLES;
-}
-
-static void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
-{
-    static int32_t a[3];
-
-    for (int axis = 0; axis < 3; axis++) {
-
-        // Reset a[axis] at start of calibration
-        if (isOnFirstAccelerationCalibrationCycle()) {
-            a[axis] = 0;
-        }
-
-        // Sum up CALIBRATING_ACC_CYCLES readings
-        a[axis] += acc.accADC[axis];
-
-        // Reset global variables to prevent other code from using un-calibrated data
-        acc.accADC[axis] = 0;
-        accelerationTrims->raw[axis] = 0;
-    }
-
-    if (isOnFinalAccelerationCalibrationCycle()) {
-        // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
-        accelerationTrims->raw[X] = (a[X] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
-        accelerationTrims->raw[Y] = (a[Y] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
-        accelerationTrims->raw[Z] = (a[Z] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc.dev.acc_1G;
-
-        resetRollAndPitchTrims(rollAndPitchTrims);
-        setConfigCalibrationCompleted();
-
-        saveConfigAndNotify();
-    }
-
-    calibratingA--;
-}
-
-static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
-{
-    static int32_t b[3];
-    static int16_t accZero_saved[3] = { 0, 0, 0 };
-    static rollAndPitchTrims_t angleTrim_saved = { { 0, 0 } };
-
-    // Saving old zeropoints before measurement
-    if (InflightcalibratingA == 50) {
-        accZero_saved[X] = accelerationTrims->raw[X];
-        accZero_saved[Y] = accelerationTrims->raw[Y];
-        accZero_saved[Z] = accelerationTrims->raw[Z];
-        angleTrim_saved.values.roll = rollAndPitchTrims->values.roll;
-        angleTrim_saved.values.pitch = rollAndPitchTrims->values.pitch;
-    }
-    if (InflightcalibratingA > 0) {
-        for (int axis = 0; axis < 3; axis++) {
-            // Reset a[axis] at start of calibration
-            if (InflightcalibratingA == 50)
-                b[axis] = 0;
-            // Sum up 50 readings
-            b[axis] += acc.accADC[axis];
-            // Clear global variables for next reading
-            acc.accADC[axis] = 0;
-            accelerationTrims->raw[axis] = 0;
-        }
-        // all values are measured
-        if (InflightcalibratingA == 1) {
-            AccInflightCalibrationActive = false;
-            AccInflightCalibrationMeasurementDone = true;
-            beeper(BEEPER_ACC_CALIBRATION); // indicate end of calibration
-            // recover saved values to maintain current flight behaviour until new values are transferred
-            accelerationTrims->raw[X] = accZero_saved[X];
-            accelerationTrims->raw[Y] = accZero_saved[Y];
-            accelerationTrims->raw[Z] = accZero_saved[Z];
-            rollAndPitchTrims->values.roll = angleTrim_saved.values.roll;
-            rollAndPitchTrims->values.pitch = angleTrim_saved.values.pitch;
-        }
-        InflightcalibratingA--;
-    }
-    // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
-    if (AccInflightCalibrationSavetoEEProm) {      // the aircraft is landed, disarmed and the combo has been done again
-        AccInflightCalibrationSavetoEEProm = false;
-        accelerationTrims->raw[X] = b[X] / 50;
-        accelerationTrims->raw[Y] = b[Y] / 50;
-        accelerationTrims->raw[Z] = b[Z] / 50 - acc.dev.acc_1G;    // for nunchuck 200=1G
-
-        resetRollAndPitchTrims(rollAndPitchTrims);
-        setConfigCalibrationCompleted();
-
-        saveConfigAndNotify();
-    }
-}
-
 static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrims)
 {
     acc.accADC[X] -= accelerationTrims->raw[X];
@@ -521,9 +61,9 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims)
         acc.accADC[axis] = acc.dev.ADCRaw[axis];
     }
 
-    if (accLpfCutHz) {
+    if (accelerationRuntime.accLpfCutHz) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            acc.accADC[axis] = biquadFilterApply(&accFilter[axis], acc.accADC[axis]);
+            acc.accADC[axis] = biquadFilterApply(&accelerationRuntime.accFilter[axis], acc.accADC[axis]);
         }
     }
 
@@ -541,23 +81,23 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims)
         performInflightAccelerationCalibration(rollAndPitchTrims);
     }
 
-    applyAccelerationTrims(accelerationTrims);
+    applyAccelerationTrims(accelerationRuntime.accelerationTrims);
 
-    ++accumulatedMeasurementCount;
+    ++accelerationRuntime.accumulatedMeasurementCount;
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        accumulatedMeasurements[axis] += acc.accADC[axis];
+        accelerationRuntime.accumulatedMeasurements[axis] += acc.accADC[axis];
     }
 }
 
 bool accGetAccumulationAverage(float *accumulationAverage)
 {
-    if (accumulatedMeasurementCount > 0) {
+    if (accelerationRuntime.accumulatedMeasurementCount > 0) {
         // If we have gyro data accumulated, calculate average rate that will yield the same rotation
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            accumulationAverage[axis] = accumulatedMeasurements[axis] / accumulatedMeasurementCount;
+            accumulationAverage[axis] = accelerationRuntime.accumulatedMeasurements[axis] / accelerationRuntime.accumulatedMeasurementCount;
-            accumulatedMeasurements[axis] = 0.0f;
+            accelerationRuntime.accumulatedMeasurements[axis] = 0.0f;
         }
-        accumulatedMeasurementCount = 0;
+        accelerationRuntime.accumulatedMeasurementCount = 0;
         return true;
     } else {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@@ -567,14 +107,4 @@ bool accGetAccumulationAverage(float *accumulationAverage)
     }
 }
 
-void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
-{
-    accelerationTrims = accelerationTrimsToUse;
-}
-
-void applyAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
-{
-    accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
-    accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
-}
 #endif

src/main/sensors/acceleration_init.c

@@ -0,0 +1,504 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+
+#include "platform.h"
+
+#ifdef USE_ACC
+
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/filter.h"
+#include "common/utils.h"
+
+#include "config/config_reset.h"
+#include "config/feature.h"
+
+#include "drivers/accgyro/accgyro.h"
+#include "drivers/accgyro/accgyro_fake.h"
+#include "drivers/accgyro/accgyro_mpu.h"
+#include "drivers/accgyro/accgyro_mpu3050.h"
+#include "drivers/accgyro/accgyro_mpu6050.h"
+#include "drivers/accgyro/accgyro_mpu6500.h"
+#include "drivers/accgyro/accgyro_spi_bmi160.h"
+#include "drivers/accgyro/accgyro_spi_bmi270.h"
+#include "drivers/accgyro/accgyro_spi_icm20649.h"
+#include "drivers/accgyro/accgyro_spi_icm20689.h"
+#include "drivers/accgyro/accgyro_spi_icm42605.h"
+#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
+#include "drivers/accgyro/accgyro_spi_mpu6000.h"
+#include "drivers/accgyro/accgyro_spi_mpu6500.h"
+#include "drivers/accgyro/accgyro_spi_mpu9250.h"
+
+#ifdef USE_ACC_ADXL345
+#include "drivers/accgyro_legacy/accgyro_adxl345.h"
+#endif
+
+#ifdef USE_ACC_BMA280
+#include "drivers/accgyro_legacy/accgyro_bma280.h"
+#endif
+
+#ifdef USE_ACC_LSM303DLHC
+#include "drivers/accgyro_legacy/accgyro_lsm303dlhc.h"
+#endif
+
+#ifdef USE_ACC_MMA8452
+#include "drivers/accgyro_legacy/accgyro_mma845x.h"
+#endif
+
+#include "drivers/bus_spi.h"
+
+#include "config/config.h"
+#include "fc/runtime_config.h"
+
+#include "io/beeper.h"
+
+#include "pg/gyrodev.h"
+#include "pg/pg.h"
+#include "pg/pg_ids.h"
+
+#include "sensors/boardalignment.h"
+#include "sensors/gyro.h"
+#include "sensors/gyro_init.h"
+#include "sensors/sensors.h"
+
+#include "acceleration_init.h"
+
+#define CALIBRATING_ACC_CYCLES              400
+
+FAST_DATA_ZERO_INIT accelerationRuntime_t accelerationRuntime;
+
+void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims)
+{
+    RESET_CONFIG_2(rollAndPitchTrims_t, rollAndPitchTrims,
+        .values.roll = 0,
+        .values.pitch = 0,
+    );
+}
+
+static void setConfigCalibrationCompleted(void)
+{
+    accelerometerConfigMutable()->accZero.values.calibrationCompleted = 1;
+}
+
+bool accHasBeenCalibrated(void)
+{
+    return accelerometerConfig()->accZero.values.calibrationCompleted;
+}
+
+void accResetRollAndPitchTrims(void)
+{
+    resetRollAndPitchTrims(&accelerometerConfigMutable()->accelerometerTrims);
+}
+
+static void resetFlightDynamicsTrims(flightDynamicsTrims_t *accZero)
+{
+    accZero->values.roll = 0;
+    accZero->values.pitch = 0;
+    accZero->values.yaw = 0;
+    accZero->values.calibrationCompleted = 0;
+}
+
+void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
+{
+    RESET_CONFIG_2(accelerometerConfig_t, instance,
+        .acc_lpf_hz = 10,
+        .acc_hardware = ACC_DEFAULT,
+        .acc_high_fsr = false,
+    );
+    resetRollAndPitchTrims(&instance->accelerometerTrims);
+    resetFlightDynamicsTrims(&instance->accZero);
+}
+
+PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 2);
+
+extern uint16_t InflightcalibratingA;
+extern bool AccInflightCalibrationMeasurementDone;
+extern bool AccInflightCalibrationSavetoEEProm;
+extern bool AccInflightCalibrationActive;
+
+
+bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
+{
+    accelerationSensor_e accHardware = ACC_NONE;
+
+#ifdef USE_ACC_ADXL345
+    drv_adxl345_config_t acc_params;
+#endif
+
+retry:
+
+    switch (accHardwareToUse) {
+    case ACC_DEFAULT:
+        FALLTHROUGH;
+
+#ifdef USE_ACC_ADXL345
+    case ACC_ADXL345: // ADXL345
+        acc_params.useFifo = false;
+        acc_params.dataRate = 800; // unused currently
+        if (adxl345Detect(&acc_params, dev)) {
+            accHardware = ACC_ADXL345;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+#ifdef USE_ACC_LSM303DLHC
+    case ACC_LSM303DLHC:
+        if (lsm303dlhcAccDetect(dev)) {
+            accHardware = ACC_LSM303DLHC;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+#ifdef USE_ACC_MPU6050
+    case ACC_MPU6050: // MPU6050
+        if (mpu6050AccDetect(dev)) {
+            accHardware = ACC_MPU6050;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+#ifdef USE_ACC_MMA8452
+    case ACC_MMA8452: // MMA8452
+        if (mma8452Detect(dev)) {
+            accHardware = ACC_MMA8452;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+#ifdef USE_ACC_BMA280
+    case ACC_BMA280: // BMA280
+        if (bma280Detect(dev)) {
+            accHardware = ACC_BMA280;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+#ifdef USE_ACC_SPI_MPU6000
+    case ACC_MPU6000:
+        if (mpu6000SpiAccDetect(dev)) {
+            accHardware = ACC_MPU6000;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+#ifdef USE_ACC_SPI_MPU9250
+    case ACC_MPU9250:
+        if (mpu9250SpiAccDetect(dev)) {
+            accHardware = ACC_MPU9250;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+    case ACC_MPU6500:
+    case ACC_ICM20601:
+    case ACC_ICM20602:
+    case ACC_ICM20608G:
+#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
+#ifdef USE_ACC_SPI_MPU6500
+        if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) {
+#else
+        if (mpu6500AccDetect(dev)) {
+#endif
+            switch (dev->mpuDetectionResult.sensor) {
+            case MPU_9250_SPI:
+                accHardware = ACC_MPU9250;
+                break;
+            case ICM_20601_SPI:
+                accHardware = ACC_ICM20601;
+                break;
+            case ICM_20602_SPI:
+                accHardware = ACC_ICM20602;
+                break;
+            case ICM_20608_SPI:
+                accHardware = ACC_ICM20608G;
+                break;
+            default:
+                accHardware = ACC_MPU6500;
+            }
+            break;
+        }
+#endif
+        FALLTHROUGH;
+
+#ifdef USE_ACC_SPI_ICM20649
+    case ACC_ICM20649:
+        if (icm20649SpiAccDetect(dev)) {
+            accHardware = ACC_ICM20649;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+#ifdef USE_ACC_SPI_ICM20689
+    case ACC_ICM20689:
+        if (icm20689SpiAccDetect(dev)) {
+            accHardware = ACC_ICM20689;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+#ifdef USE_ACC_SPI_ICM42605
+    case ACC_ICM42605:
+        if (icm42605SpiAccDetect(dev)) {
+            accHardware = ACC_ICM42605;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+#ifdef USE_ACCGYRO_BMI160
+    case ACC_BMI160:
+        if (bmi160SpiAccDetect(dev)) {
+            accHardware = ACC_BMI160;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+#ifdef USE_ACCGYRO_BMI270
+    case ACC_BMI270:
+        if (bmi270SpiAccDetect(dev)) {
+            accHardware = ACC_BMI270;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+#ifdef USE_ACCGYRO_LSM6DSO
+    case ACC_LSM6DSO:
+        if (lsm6dsoSpiAccDetect(dev)) {
+            accHardware = ACC_LSM6DSO;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+#ifdef USE_FAKE_ACC
+    case ACC_FAKE:
+        if (fakeAccDetect(dev)) {
+            accHardware = ACC_FAKE;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
+    default:
+    case ACC_NONE: // disable ACC
+        accHardware = ACC_NONE;
+        break;
+    }
+
+    // Found anything? Check if error or ACC is really missing.
+    if (accHardware == ACC_NONE && accHardwareToUse != ACC_DEFAULT && accHardwareToUse != ACC_NONE) {
+        // Nothing was found and we have a forced sensor that isn't present.
+        accHardwareToUse = ACC_DEFAULT;
+        goto retry;
+    }
+
+    if (accHardware == ACC_NONE) {
+        return false;
+    }
+
+    detectedSensors[SENSOR_INDEX_ACC] = accHardware;
+    sensorsSet(SENSOR_ACC);
+    return true;
+}
+
+void accInitFilters(void)
+{
+    // Only set the lowpass cutoff if the ACC sample rate is detected otherwise
+    // the filter initialization is not defined (sample rate = 0)
+    accelerationRuntime.accLpfCutHz = (acc.sampleRateHz) ? accelerometerConfig()->acc_lpf_hz : 0;
+    if (accelerationRuntime.accLpfCutHz) {
+        const uint32_t accSampleTimeUs = 1e6 / acc.sampleRateHz;
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            biquadFilterInitLPF(&accelerationRuntime.accFilter[axis], accelerationRuntime.accLpfCutHz, accSampleTimeUs);
+        }
+    }
+}
+
+bool accInit(uint16_t accSampleRateHz)
+{
+    memset(&acc, 0, sizeof(acc));
+    // copy over the common gyro mpu settings
+    acc.dev.bus = *gyroSensorBus();
+    acc.dev.mpuDetectionResult = *gyroMpuDetectionResult();
+    acc.dev.acc_high_fsr = accelerometerConfig()->acc_high_fsr;
+
+    // Copy alignment from active gyro, as all production boards use acc-gyro-combi chip.
+    // Exceptions are STM32F3DISCOVERY and STM32F411DISCOVERY, and (may be) handled in future enhancement.
+
+    sensor_align_e alignment = gyroDeviceConfig(0)->alignment;
+    const sensorAlignment_t* customAlignment = &gyroDeviceConfig(0)->customAlignment;
+
+#ifdef USE_MULTI_GYRO
+    if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) {
+        alignment = gyroDeviceConfig(1)->alignment;
+
+        customAlignment = &gyroDeviceConfig(1)->customAlignment;
+    }
+#endif
+    acc.dev.accAlign = alignment;
+    buildRotationMatrixFromAlignment(customAlignment, &acc.dev.rotationMatrix);
+
+    if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {
+        return false;
+    }
+    acc.dev.acc_1G = 256; // set default
+    acc.dev.initFn(&acc.dev); // driver initialisation
+    acc.dev.acc_1G_rec = 1.0f / acc.dev.acc_1G;
+
+    acc.sampleRateHz = accSampleRateHz;
+    accInitFilters();
+    return true;
+}
+
+void accStartCalibration(void)
+{
+    accelerationRuntime.calibratingA = CALIBRATING_ACC_CYCLES;
+}
+
+bool accIsCalibrationComplete(void)
+{
+    return accelerationRuntime.calibratingA == 0;
+}
+
+static bool isOnFinalAccelerationCalibrationCycle(void)
+{
+    return accelerationRuntime.calibratingA == 1;
+}
+
+static bool isOnFirstAccelerationCalibrationCycle(void)
+{
+    return accelerationRuntime.calibratingA == CALIBRATING_ACC_CYCLES;
+}
+
+void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
+{
+    static int32_t a[3];
+
+    for (int axis = 0; axis < 3; axis++) {
+
+        // Reset a[axis] at start of calibration
+        if (isOnFirstAccelerationCalibrationCycle()) {
+            a[axis] = 0;
+        }
+
+        // Sum up CALIBRATING_ACC_CYCLES readings
+        a[axis] += acc.accADC[axis];
+
+        // Reset global variables to prevent other code from using un-calibrated data
+        acc.accADC[axis] = 0;
+        accelerationRuntime.accelerationTrims->raw[axis] = 0;
+    }
+
+    if (isOnFinalAccelerationCalibrationCycle()) {
+        // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
+        accelerationRuntime.accelerationTrims->raw[X] = (a[X] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
+        accelerationRuntime.accelerationTrims->raw[Y] = (a[Y] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
+        accelerationRuntime.accelerationTrims->raw[Z] = (a[Z] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc.dev.acc_1G;
+
+        resetRollAndPitchTrims(rollAndPitchTrims);
+        setConfigCalibrationCompleted();
+
+        saveConfigAndNotify();
+    }
+
+    accelerationRuntime.calibratingA--;
+}
+
+void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims)
+{
+    static int32_t b[3];
+    static int16_t accZero_saved[3] = { 0, 0, 0 };
+    static rollAndPitchTrims_t angleTrim_saved = { { 0, 0 } };
+
+    // Saving old zeropoints before measurement
+    if (InflightcalibratingA == 50) {
+        accZero_saved[X] = accelerationRuntime.accelerationTrims->raw[X];
+        accZero_saved[Y] = accelerationRuntime.accelerationTrims->raw[Y];
+        accZero_saved[Z] = accelerationRuntime.accelerationTrims->raw[Z];
+        angleTrim_saved.values.roll = rollAndPitchTrims->values.roll;
+        angleTrim_saved.values.pitch = rollAndPitchTrims->values.pitch;
+    }
+    if (InflightcalibratingA > 0) {
+        for (int axis = 0; axis < 3; axis++) {
+            // Reset a[axis] at start of calibration
+            if (InflightcalibratingA == 50)
+                b[axis] = 0;
+            // Sum up 50 readings
+            b[axis] += acc.accADC[axis];
+            // Clear global variables for next reading
+            acc.accADC[axis] = 0;
+            accelerationRuntime.accelerationTrims->raw[axis] = 0;
+        }
+        // all values are measured
+        if (InflightcalibratingA == 1) {
+            AccInflightCalibrationActive = false;
+            AccInflightCalibrationMeasurementDone = true;
+            beeper(BEEPER_ACC_CALIBRATION); // indicate end of calibration
+            // recover saved values to maintain current flight behaviour until new values are transferred
+            accelerationRuntime.accelerationTrims->raw[X] = accZero_saved[X];
+            accelerationRuntime.accelerationTrims->raw[Y] = accZero_saved[Y];
+            accelerationRuntime.accelerationTrims->raw[Z] = accZero_saved[Z];
+            rollAndPitchTrims->values.roll = angleTrim_saved.values.roll;
+            rollAndPitchTrims->values.pitch = angleTrim_saved.values.pitch;
+        }
+        InflightcalibratingA--;
+    }
+    // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration
+    if (AccInflightCalibrationSavetoEEProm) {      // the aircraft is landed, disarmed and the combo has been done again
+        AccInflightCalibrationSavetoEEProm = false;
+        accelerationRuntime.accelerationTrims->raw[X] = b[X] / 50;
+        accelerationRuntime.accelerationTrims->raw[Y] = b[Y] / 50;
+        accelerationRuntime.accelerationTrims->raw[Z] = b[Z] / 50 - acc.dev.acc_1G;    // for nunchuck 200=1G
+
+        resetRollAndPitchTrims(rollAndPitchTrims);
+        setConfigCalibrationCompleted();
+
+        saveConfigAndNotify();
+    }
+}
+
+void setAccelerationTrims(flightDynamicsTrims_t *accelerationTrimsToUse)
+{
+    accelerationRuntime.accelerationTrims = accelerationTrimsToUse;
+}
+
+void applyAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
+{
+    accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
+    accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
+}
+#endif

src/main/sensors/acceleration_init.h

@@ -0,0 +1,40 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "platform.h"
+
+#include "sensors/acceleration.h"
+
+
+typedef struct accelerationRuntime_s {
+    uint16_t accLpfCutHz;
+    biquadFilter_t accFilter[XYZ_AXIS_COUNT];
+    flightDynamicsTrims_t *accelerationTrims;
+    int accumulatedMeasurementCount;
+    float accumulatedMeasurements[XYZ_AXIS_COUNT];
+    uint16_t calibratingA;      // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
+} accelerationRuntime_t;
+
+extern accelerationRuntime_t accelerationRuntime;
+
+void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims);
+void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims);