diff --git a/make/source.mk b/make/source.mk index 28a93c5b6a..d168e7f306 100644 --- a/make/source.mk +++ b/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 \ diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 8faeab5ccc..6e056be7a9 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -20,8 +20,6 @@ #include #include -#include -#include #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 "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/acceleration_init.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; - accumulatedMeasurements[axis] = 0.0f; + accumulationAverage[axis] = accelerationRuntime.accumulatedMeasurements[axis] / accelerationRuntime.accumulatedMeasurementCount; + 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 diff --git a/src/main/sensors/acceleration_init.c b/src/main/sensors/acceleration_init.c new file mode 100644 index 0000000000..dceefdc54c --- /dev/null +++ b/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 . + */ + +#include +#include +#include +#include + +#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 diff --git a/src/main/sensors/acceleration_init.h b/src/main/sensors/acceleration_init.h new file mode 100644 index 0000000000..4d7e4e91c6 --- /dev/null +++ b/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 . + */ + +#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);