From 38928085f26d7e0d1d5825100b69b2fe68bff004 Mon Sep 17 00:00:00 2001 From: Mark Haslinghuis Date: Tue, 31 Dec 2024 20:06:39 +0100 Subject: [PATCH] Adds missing ACCGYRO defines and remove legacy drivers (#14087) --- mk/source.mk | 20 - src/main/cli/settings.c | 43 +- src/main/drivers/accgyro/accgyro.h | 3 +- src/main/drivers/accgyro/accgyro_mpu.c | 2 - src/main/drivers/accgyro/accgyro_mpu3050.c | 114 ----- src/main/drivers/accgyro/accgyro_mpu3050.h | 32 -- src/main/drivers/accgyro/accgyro_mpu6500.c | 7 + .../drivers/accgyro/accgyro_spi_icm20689.c | 4 + .../drivers/accgyro/accgyro_spi_mpu6500.c | 4 + .../drivers/accgyro/accgyro_spi_mpu9250.c | 4 + .../drivers/accgyro/legacy/accgyro_adxl345.c | 139 ------ .../drivers/accgyro/legacy/accgyro_adxl345.h | 28 -- .../drivers/accgyro/legacy/accgyro_bma280.c | 76 --- .../drivers/accgyro/legacy/accgyro_bma280.h | 23 - .../drivers/accgyro/legacy/accgyro_l3g4200d.c | 119 ----- .../drivers/accgyro/legacy/accgyro_l3g4200d.h | 23 - .../accgyro/legacy/accgyro_lsm303dlhc.c | 171 ------- .../accgyro/legacy/accgyro_lsm303dlhc.h | 434 ------------------ .../drivers/accgyro/legacy/accgyro_mma845x.c | 144 ------ .../drivers/accgyro/legacy/accgyro_mma845x.h | 23 - src/main/sensors/acceleration.c | 2 +- src/main/sensors/acceleration.h | 5 +- src/main/sensors/acceleration_init.c | 82 +--- src/main/sensors/acceleration_init.h | 2 +- src/main/sensors/gyro_init.c | 48 +- src/main/target/common_post.h | 45 +- 26 files changed, 114 insertions(+), 1483 deletions(-) delete mode 100644 src/main/drivers/accgyro/accgyro_mpu3050.c delete mode 100644 src/main/drivers/accgyro/accgyro_mpu3050.h delete mode 100644 src/main/drivers/accgyro/legacy/accgyro_adxl345.c delete mode 100644 src/main/drivers/accgyro/legacy/accgyro_adxl345.h delete mode 100644 src/main/drivers/accgyro/legacy/accgyro_bma280.c delete mode 100644 src/main/drivers/accgyro/legacy/accgyro_bma280.h delete mode 100644 src/main/drivers/accgyro/legacy/accgyro_l3g4200d.c delete mode 100644 src/main/drivers/accgyro/legacy/accgyro_l3g4200d.h delete mode 100644 src/main/drivers/accgyro/legacy/accgyro_lsm303dlhc.c delete mode 100644 src/main/drivers/accgyro/legacy/accgyro_lsm303dlhc.h delete mode 100644 src/main/drivers/accgyro/legacy/accgyro_mma845x.c delete mode 100644 src/main/drivers/accgyro/legacy/accgyro_mma845x.h diff --git a/mk/source.mk b/mk/source.mk index e4c60d55cb4..a9a8ed0e492 100644 --- a/mk/source.mk +++ b/mk/source.mk @@ -290,7 +290,6 @@ ifneq ($(SIMULATOR_BUILD),yes) COMMON_SRC += \ drivers/bus_spi.c \ drivers/serial_uart.c \ - drivers/accgyro/accgyro_mpu3050.c \ drivers/accgyro/accgyro_mpu6050.c \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_mpu.c \ @@ -332,24 +331,6 @@ COMMON_SRC += \ drivers/vtx_rtc6705.c \ drivers/vtx_rtc6705_soft_spi.c -ifneq ($(GYRO_DEFINE),) - -LEGACY_GYRO_DEFINES := USE_GYRO_L3GD20 -ifneq ($(findstring $(GYRO_DEFINE),$(LEGACY_GYRO_DEFINES)),) - -LEGACY_SRC := \ - drivers/accgyro/legacy/accgyro_adxl345.c \ - drivers/accgyro/legacy/accgyro_bma280.c \ - drivers/accgyro/legacy/accgyro_l3g4200d.c \ - drivers/accgyro/legacy/accgyro_lsm303dlhc.c \ - drivers/accgyro/legacy/accgyro_mma845x.c - -COMMON_SRC += $(LEGACY_SRC) -SPEED_OPTIMISED_SRC += $(LEGACY_SRC) - -endif -endif - RX_SRC = \ drivers/rx/expresslrs_driver.c \ drivers/rx/rx_a7105.c \ @@ -425,7 +406,6 @@ SPEED_OPTIMISED_SRC += \ drivers/bus_spi.c \ drivers/serial_uart.c \ drivers/accgyro/accgyro_mpu.c \ - drivers/accgyro/accgyro_mpu3050.c \ drivers/accgyro/accgyro_spi_bmi160.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_spi_lsm6dso.c diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 1bfe829b794..9a9a7490933 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -143,16 +143,47 @@ // Sensor names (used in lookup tables for *_hardware settings and in status command output) // sync with accelerationSensor_e const char * const lookupTableAccHardware[] = { - "AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC", - "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P", - "BMI160", "BMI270", "LSM6DSO", "LSM6DSV16X", "VIRTUAL" + "AUTO", + "NONE", + "MPU6050", + "MPU6000", + "MPU6500", + "MPU9250", + "ICM20601", + "ICM20602", + "ICM20608G", + "ICM20649", + "ICM20689", + "ICM42605", + "ICM42688P", + "BMI160", + "BMI270", + "LSM6DSO", + "LSM6DSV16X", + "VIRTUAL" }; // sync with gyroHardware_e const char * const lookupTableGyroHardware[] = { - "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", - "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P", - "BMI160", "BMI270", "LSM6DSO", "LSM6DSV16X", "VIRTUAL" + "AUTO", + "NONE", + "MPU6050", + "L3GD20", + "MPU6000", + "MPU6500", + "MPU9250", + "ICM20601", + "ICM20602", + "ICM20608G", + "ICM20649", + "ICM20689", + "ICM42605", + "ICM42688P", + "BMI160", + "BMI270", + "LSM6DSO", + "LSM6DSV16X", + "VIRTUAL" }; #if defined(USE_SENSOR_NAMES) || defined(USE_BARO) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index e4d69260316..0c63a8258f4 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -41,12 +41,11 @@ #define GYRO_SCALE_2000DPS (2000.0f / (1 << 15)) // 16.384 dps/lsb scalefactor for 2000dps sensors #define GYRO_SCALE_4000DPS (4000.0f / (1 << 15)) // 8.192 dps/lsb scalefactor for 4000dps sensors +// Gyro hardware types were updated in PR #14087 (removed GYRO_L3G4200D, GYRO_MPU3050) typedef enum { GYRO_NONE = 0, GYRO_DEFAULT, GYRO_MPU6050, - GYRO_L3G4200D, - GYRO_MPU3050, GYRO_L3GD20, GYRO_MPU6000, GYRO_MPU6500, diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 4febfe23826..d1ebe2857aa 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -43,7 +43,6 @@ #include "drivers/time.h" #include "drivers/accgyro/accgyro.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" @@ -452,7 +451,6 @@ bool mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config) if (ack) { busDeviceRegister(&gyro->dev); - // If an MPU3050 is connected sig will contain 0. uint8_t inquiryResult; ack = busReadRegisterBuffer(&gyro->dev, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1); inquiryResult &= MPU_INQUIRY_MASK; diff --git a/src/main/drivers/accgyro/accgyro_mpu3050.c b/src/main/drivers/accgyro/accgyro_mpu3050.c deleted file mode 100644 index a52b1a7984a..00000000000 --- a/src/main/drivers/accgyro/accgyro_mpu3050.c +++ /dev/null @@ -1,114 +0,0 @@ -/* - * 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 . - */ - -// NOTE: This gyro is considered obsolete and may be removed in the future. - -#include -#include - -#include "platform.h" - -#ifdef USE_GYRO_MPU3050 - -#include "common/maths.h" -#include "common/utils.h" - -#include "drivers/bus_i2c.h" -#include "drivers/exti.h" -#include "drivers/sensor.h" -#include "drivers/system.h" -#include "drivers/time.h" - -#include "accgyro.h" -#include "accgyro_mpu.h" -#include "accgyro_mpu3050.h" - -// MPU3050, Standard address 0x68 -#define MPU3050_ADDRESS 0x68 - -// Bits -#define MPU3050_FS_SEL_2000DPS 0x18 -#define MPU3050_DLPF_10HZ 0x05 -#define MPU3050_DLPF_20HZ 0x04 -#define MPU3050_DLPF_42HZ 0x03 -#define MPU3050_DLPF_98HZ 0x02 -#define MPU3050_DLPF_188HZ 0x01 -#define MPU3050_DLPF_256HZ 0x00 - -#define MPU3050_USER_RESET 0x01 -#define MPU3050_CLK_SEL_PLL_GX 0x01 - -static void mpu3050Init(gyroDev_t *gyro) -{ - delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe - - const bool ack = busWriteRegister(&gyro->bus, MPU3050_SMPLRT_DIV, 0); - if (!ack) { - failureMode(FAILURE_ACC_INIT); - } - - busWriteRegister(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | MPU3050_DLPF_256HZ); - busWriteRegister(&gyro->bus, MPU3050_INT_CFG, 0); - busWriteRegister(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET); - busWriteRegister(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); -} - -static bool mpu3050GyroRead(gyroDev_t *gyro) -{ - uint8_t data[6]; - - const bool ack = busReadRegisterBuffer(&gyro->bus, MPU3050_GYRO_OUT, data, 6); - if (!ack) { - return false; - } - - gyro->gyroADCRaw[X] = (int16_t)((data[0] << 8) | data[1]); - gyro->gyroADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]); - gyro->gyroADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]); - - return true; -} - -static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData) -{ - uint8_t buf[2]; - if (!busReadRegisterBuffer(&gyro->bus, MPU3050_TEMP_OUT, buf, 2)) { - return false; - } - - *tempData = 35 + ((int32_t)(buf[0] << 8 | buf[1]) + 13200) / 280; - - return true; -} - -bool mpu3050Detect(gyroDev_t *gyro) -{ - if (gyro->mpuDetectionResult.sensor != MPU_3050) { - return false; - } - gyro->initFn = mpu3050Init; - gyro->readFn = mpu3050GyroRead; - gyro->temperatureFn = mpu3050ReadTemperature; - - gyro->scale = GYRO_SCALE_2000DPS; - - return true; -} -#endif diff --git a/src/main/drivers/accgyro/accgyro_mpu3050.h b/src/main/drivers/accgyro/accgyro_mpu3050.h deleted file mode 100644 index 3de31fae844..00000000000 --- a/src/main/drivers/accgyro/accgyro_mpu3050.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * 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 - -// Registers -#define MPU3050_SMPLRT_DIV 0x15 -#define MPU3050_DLPF_FS_SYNC 0x16 -#define MPU3050_INT_CFG 0x17 -#define MPU3050_TEMP_OUT 0x1B -#define MPU3050_GYRO_OUT 0x1D -#define MPU3050_USER_CTRL 0x3D -#define MPU3050_PWR_MGM 0x3E - -bool mpu3050Detect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_mpu6500.c b/src/main/drivers/accgyro/accgyro_mpu6500.c index e4a0c86dfc5..ae607d303a5 100644 --- a/src/main/drivers/accgyro/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_mpu6500.c @@ -24,6 +24,11 @@ #include "platform.h" +#if defined(USE_ACC_SPI_MPU6500) \ + || defined(USE_GYRO_SPI_MPU6500) \ + || defined(USE_ACC_MPU6500) \ + || defined(USE_GYRO_MPU6500) + #include "common/axis.h" #include "common/maths.h" @@ -108,3 +113,5 @@ bool mpu6500GyroDetect(gyroDev_t *gyro) return true; } + +#endif // USE_ACC_SPI_MPU6500 || USE_GYRO_SPI_MPU6500 || USE_ACC_MPU6500 || USE_GYRO_MPU6500 diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20689.c b/src/main/drivers/accgyro/accgyro_spi_icm20689.c index ada4dadf78c..3a15d0f0573 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm20689.c @@ -24,6 +24,8 @@ #include "platform.h" +#if defined(USE_ACC_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM20689) + #include "common/axis.h" #include "common/maths.h" @@ -187,3 +189,5 @@ bool icm20689SpiGyroDetect(gyroDev_t *gyro) return true; } + +#endif // USE_ACC_SPI_ICM20689 || USE_GYRO_SPI_ICM20689 diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index 9691c77818d..7a2e02408b0 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c @@ -23,6 +23,8 @@ #include "platform.h" +#if defined(USE_ACC_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6500) + #include "common/axis.h" #include "common/maths.h" @@ -146,3 +148,5 @@ bool mpu6500SpiGyroDetect(gyroDev_t *gyro) return true; } + +#endif // USE_ACC_SPI_MPU6500 || USE_GYRO_SPI_MPU6500 diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c index 633a614d293..eca0f03b6d3 100644 --- a/src/main/drivers/accgyro/accgyro_spi_mpu9250.c +++ b/src/main/drivers/accgyro/accgyro_spi_mpu9250.c @@ -30,6 +30,8 @@ #include "platform.h" +#ifdef USE_ACC_SPI_MPU9250 + #include "common/axis.h" #include "common/maths.h" @@ -173,3 +175,5 @@ bool mpu9250SpiGyroDetect(gyroDev_t *gyro) return true; } + +#endif // USE_ACC_SPI_MPU9250 diff --git a/src/main/drivers/accgyro/legacy/accgyro_adxl345.c b/src/main/drivers/accgyro/legacy/accgyro_adxl345.c deleted file mode 100644 index d64cd9fccb1..00000000000 --- a/src/main/drivers/accgyro/legacy/accgyro_adxl345.c +++ /dev/null @@ -1,139 +0,0 @@ -/* - * 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 "platform.h" - -#ifdef USE_ACC_ADXL345 - -#include "drivers/bus_i2c.h" - -#include "drivers/sensor.h" -#include "drivers/accgyro/accgyro.h" -#include "accgyro_adxl345.h" - -// ADXL345, Alternative address mode 0x53 -#define ADXL345_ADDRESS 0x53 - -// Registers -#define ADXL345_BW_RATE 0x2C -#define ADXL345_POWER_CTL 0x2D -#define ADXL345_INT_ENABLE 0x2E -#define ADXL345_DATA_FORMAT 0x31 -#define ADXL345_DATA_OUT 0x32 -#define ADXL345_FIFO_CTL 0x38 - -// BW_RATE values -#define ADXL345_RATE_50 0x09 -#define ADXL345_RATE_100 0x0A -#define ADXL345_RATE_200 0x0B -#define ADXL345_RATE_400 0x0C -#define ADXL345_RATE_800 0x0D -#define ADXL345_RATE_1600 0x0E -#define ADXL345_RATE_3200 0x0F - -// various register values -#define ADXL345_POWER_MEAS 0x08 -#define ADXL345_FULL_RANGE 0x08 -#define ADXL345_RANGE_2G 0x00 -#define ADXL345_RANGE_4G 0x01 -#define ADXL345_RANGE_8G 0x02 -#define ADXL345_RANGE_16G 0x03 -#define ADXL345_FIFO_STREAM 0x80 - -static bool useFifo = false; - -static void adxl345Init(accDev_t *acc) -{ - if (useFifo) { - uint8_t fifoDepth = 16; - i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS); - i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G); - i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_400); - i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_FIFO_CTL, (fifoDepth & 0x1F) | ADXL345_FIFO_STREAM); - } else { - i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_POWER_CTL, ADXL345_POWER_MEAS); - i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G); - i2cWrite(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100); - } - acc->acc_1G = 256; // 3.3V operation -} - -uint8_t acc_samples = 0; - -static bool adxl345Read(accDev_t *acc) -{ - uint8_t buf[8]; - - if (useFifo) { - int32_t x = 0; - int32_t y = 0; - int32_t z = 0; - uint8_t i = 0; - uint8_t samples_remaining; - - do { - i++; - - if (!i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf)) { - return false; - } - - x += (int16_t)(buf[0] + (buf[1] << 8)); - y += (int16_t)(buf[2] + (buf[3] << 8)); - z += (int16_t)(buf[4] + (buf[5] << 8)); - samples_remaining = buf[7] & 0x7F; - } while ((i < 32) && (samples_remaining > 0)); - acc->ADCRaw[0] = x / i; - acc->ADCRaw[1] = y / i; - acc->ADCRaw[2] = z / i; - acc_samples = i; - } else { - - if (!i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf)) { - return false; - } - - acc->ADCRaw[0] = buf[0] + (buf[1] << 8); - acc->ADCRaw[1] = buf[2] + (buf[3] << 8); - acc->ADCRaw[2] = buf[4] + (buf[5] << 8); - } - - return true; -} - -bool adxl345Detect(drv_adxl345_config_t *init, accDev_t *acc) -{ - uint8_t sig = 0; - bool ack = i2cRead(MPU_I2C_INSTANCE, ADXL345_ADDRESS, 0x00, 1, &sig); - - if (!ack || sig != 0xE5) - return false; - - // use ADXL345's fifo to filter data or not - useFifo = init->useFifo; - - acc->initFn = adxl345Init; - acc->readFn = adxl345Read; - return true; -} -#endif diff --git a/src/main/drivers/accgyro/legacy/accgyro_adxl345.h b/src/main/drivers/accgyro/legacy/accgyro_adxl345.h deleted file mode 100644 index 27ad0e5b04d..00000000000 --- a/src/main/drivers/accgyro/legacy/accgyro_adxl345.h +++ /dev/null @@ -1,28 +0,0 @@ -/* - * 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 - -typedef struct drv_adxl345_config_s { - uint16_t dataRate; - bool useFifo; -} drv_adxl345_config_t; - -bool adxl345Detect(drv_adxl345_config_t *init, accDev_t *acc); diff --git a/src/main/drivers/accgyro/legacy/accgyro_bma280.c b/src/main/drivers/accgyro/legacy/accgyro_bma280.c deleted file mode 100644 index 5eab61d8ecf..00000000000 --- a/src/main/drivers/accgyro/legacy/accgyro_bma280.c +++ /dev/null @@ -1,76 +0,0 @@ -/* - * 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 "platform.h" - -#ifdef USE_ACC_BMA280 - -#include "drivers/bus_i2c.h" - -#include "drivers/sensor.h" -#include "drivers/accgyro/accgyro.h" -#include "accgyro_bma280.h" - -// BMA280, default I2C address mode 0x18 -#define BMA280_ADDRESS 0x18 -#define BMA280_ACC_X_LSB 0x02 -#define BMA280_PMU_BW 0x10 -#define BMA280_PMU_RANGE 0x0F - -static void bma280Init(accDev_t *acc) -{ - i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range - i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW - - acc->acc_1G = 512 * 8; -} - -static bool bma280Read(accDev_t *acc) -{ - uint8_t buf[6]; - - if (!i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf)) { - return false; - } - - // Data format is lsb<5:0> | msb<13:6> - acc->ADCRaw[0] = (int16_t)((buf[0] >> 2) + (buf[1] << 8)); - acc->ADCRaw[1] = (int16_t)((buf[2] >> 2) + (buf[3] << 8)); - acc->ADCRaw[2] = (int16_t)((buf[4] >> 2) + (buf[5] << 8)); - - return true; -} - -bool bma280Detect(accDev_t *acc) -{ - uint8_t sig = 0; - bool ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig); - - if (!ack || sig != 0xFB) - return false; - - acc->initFn = bma280Init; - acc->readFn = bma280Read; - return true; -} -#endif diff --git a/src/main/drivers/accgyro/legacy/accgyro_bma280.h b/src/main/drivers/accgyro/legacy/accgyro_bma280.h deleted file mode 100644 index 93fac71f40b..00000000000 --- a/src/main/drivers/accgyro/legacy/accgyro_bma280.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * 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 - -bool bma280Detect(accDev_t *acc); diff --git a/src/main/drivers/accgyro/legacy/accgyro_l3g4200d.c b/src/main/drivers/accgyro/legacy/accgyro_l3g4200d.c deleted file mode 100644 index 6dbcf4b96c4..00000000000 --- a/src/main/drivers/accgyro/legacy/accgyro_l3g4200d.c +++ /dev/null @@ -1,119 +0,0 @@ -/* - * 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 . - */ - -// NOTE: This gyro is considered obsolete and may be removed in the future. - -#include -#include - -#include "platform.h" - -#ifdef USE_GYRO_L3G4200D - -#include "drivers/accgyro/accgyro.h" -#include "accgyro_l3g4200d.h" - -#include "common/maths.h" -#include "common/axis.h" - -#include "drivers/bus_i2c.h" -#include "drivers/time.h" -#include "drivers/sensor.h" -#include "drivers/system.h" - -// L3G4200D, Standard address 0x68 -#define L3G4200D_ADDRESS 0x68 -#define L3G4200D_ID 0xD3 -#define L3G4200D_AUTOINCR 0x80 - -// Registers -#define L3G4200D_WHO_AM_I 0x0F -#define L3G4200D_CTRL_REG1 0x20 -#define L3G4200D_CTRL_REG2 0x21 -#define L3G4200D_CTRL_REG3 0x22 -#define L3G4200D_CTRL_REG4 0x23 -#define L3G4200D_CTRL_REG5 0x24 -#define L3G4200D_REFERENCE 0x25 -#define L3G4200D_STATUS_REG 0x27 -#define L3G4200D_GYRO_OUT 0x28 - -// Bits -#define L3G4200D_POWER_ON 0x0F -#define L3G4200D_FS_SEL_2000DPS 0xF0 -#define L3G4200D_DLPF_32HZ 0x00 -#define L3G4200D_DLPF_54HZ 0x40 -#define L3G4200D_DLPF_78HZ 0x80 -#define L3G4200D_DLPF_93HZ 0xC0 - -static void l3g4200dInit(gyroDev_t *gyro) -{ - bool ack; - - // Removed lowpass filter selection and just default to 32Hz regardless of gyro->hardware_lpf - // The previous selection was broken anyway as the old gyro->lpf values ranged from 0-7 and - // the switch statement would have always taken the default and used L3G4200D_DLPF_32HZ - - delay(100); - - ack = i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS); - if (!ack) - failureMode(FAILURE_ACC_INIT); - - delay(5); - i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | L3G4200D_DLPF_32HZ); - - UNUSED(gyro); -} - -// Read 3 gyro values into user-provided buffer. No overrun checking is done. -static bool l3g4200dRead(gyroDev_t *gyro) -{ - uint8_t buf[6]; - - if (!i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) { - return false; - } - - gyro->gyroADCRaw[X] = (int16_t)((buf[0] << 8) | buf[1]); - gyro->gyroADCRaw[Y] = (int16_t)((buf[2] << 8) | buf[3]); - gyro->gyroADCRaw[Z] = (int16_t)((buf[4] << 8) | buf[5]); - - return true; -} - -bool l3g4200dDetect(gyroDev_t *gyro) -{ - uint8_t deviceid; - - delay(25); - - i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_WHO_AM_I, 1, &deviceid); - if (deviceid != L3G4200D_ID) - return false; - - gyro->initFn = l3g4200dInit; - gyro->readFn = l3g4200dRead; - - // 14.2857dps/lsb scalefactor - gyro->scale = 1.0f / 14.2857f; - - return true; -} -#endif diff --git a/src/main/drivers/accgyro/legacy/accgyro_l3g4200d.h b/src/main/drivers/accgyro/legacy/accgyro_l3g4200d.h deleted file mode 100644 index 258cc2eeb37..00000000000 --- a/src/main/drivers/accgyro/legacy/accgyro_l3g4200d.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * 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 - -bool l3g4200dDetect(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/legacy/accgyro_lsm303dlhc.c b/src/main/drivers/accgyro/legacy/accgyro_lsm303dlhc.c deleted file mode 100644 index 21a88312445..00000000000 --- a/src/main/drivers/accgyro/legacy/accgyro_lsm303dlhc.c +++ /dev/null @@ -1,171 +0,0 @@ -/* - * 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 "platform.h" - -#ifdef USE_ACC_LSM303DLHC - -#include "build/debug.h" - -#include "common/maths.h" -#include "common/axis.h" - -#include "drivers/time.h" -#include "drivers/io.h" -#include "drivers/bus_i2c.h" - -#include "drivers/sensor.h" -#include "drivers/accgyro/accgyro.h" -#include "accgyro_lsm303dlhc.h" - -// Addresses (7 bit address format) - -#define LSM303DLHC_ACCEL_ADDRESS 0x19 -#define LSM303DLHC_MAG_ADDRESS 0x1E - -/** - * Address Auto Increment - See LSM303DLHC datasheet, Section 5.1.1 I2C operation. - * http://www.st.com/web/en/resource/technical/document/datasheet/DM00027543.pdf - * - * "The I2C embedded inside the LSM303DLHC behaves like a slave device and the following protocol must be adhered to. - * After the START condition (ST) a slave address is sent, once a slave acknowledge (SAK) has been returned, an 8-bit - * sub-address (SUB) is transmitted; the 7 LSBs represent the actual register address while the MSB enables address - * autoincrement. - * - * If the MSB of the SUB field is ‘1’, the SUB (register address) is automatically increased to allow multiple data - * Read/Write. - * - * To minimize the communication between the master and magnetic digital interface of LSM303DLHC, the address pointer - * updates automatically without master intervention. This automatic address pointer update has two additional - * features. First, when address 12 or higher is accessed, the pointer updates to address 00, and secondly, when - * address 08 is reached, the pointer rolls back to address 03. Logically, the address pointer operation functions - * as shown below. - * 1) If (address pointer = 08) then the address pointer = 03 - * Or else, if (address pointer >= 12) then the address pointer = 0 - * Or else, (address pointer) = (address pointer) + 1 - * - * The address pointer value itself cannot be read via the I2C bus" - */ -#define AUTO_INCREMENT_ENABLE 0x80 - -// Registers - -#define CTRL_REG1_A 0x20 -#define CTRL_REG4_A 0x23 -#define CTRL_REG5_A 0x24 -#define OUT_X_L_A 0x28 -#define CRA_REG_M 0x00 -#define CRB_REG_M 0x01 -#define MR_REG_M 0x02 -#define OUT_X_H_M 0x03 - -/////////////////////////////////////// - -#define ODR_1344_HZ 0x90 -#define AXES_ENABLE 0x07 - -#define FULLSCALE_2G 0x00 -#define FULLSCALE_4G 0x10 -#define FULLSCALE_8G 0x20 -#define FULLSCALE_16G 0x30 - -#define BOOT 0x80 - -/////////////////////////////////////// - -#define ODR_75_HZ 0x18 -#define ODR_15_HZ 0x10 - -#define FS_1P3_GA 0x20 -#define FS_1P9_GA 0x40 -#define FS_2P5_GA 0x60 -#define FS_4P0_GA 0x80 -#define FS_4P7_GA 0xA0 -#define FS_5P6_GA 0xC0 -#define FS_8P1_GA 0xE0 - -#define CONTINUOUS_CONVERSION 0x00 - -uint8_t accelCalibrating = false; - -float accelOneG = 9.8065; - -int32_t accelSum100Hz[3] = { 0, 0, 0 }; - -int32_t accelSum500Hz[3] = { 0, 0, 0 }; - -int32_t accelSummedSamples100Hz[3]; - -int32_t accelSummedSamples500Hz[3]; - -void lsm303dlhcAccInit(accDev_t *acc) -{ - i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT); - - delay(100); - - i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE); - - delay(10); - - i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG4_A, FULLSCALE_4G); - - delay(100); - - acc->acc_1G = 512 * 8; -} - -// Read 3 gyro values into user-provided buffer. No overrun checking is done. -static bool lsm303dlhcAccRead(accDev_t *acc) -{ - uint8_t buf[6]; - - bool ack = i2cRead(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, AUTO_INCREMENT_ENABLE | OUT_X_L_A, 6, buf); - - if (!ack) { - return false; - } - - // the values range from -8192 to +8191 - acc->ADCRaw[X] = (int16_t)((buf[1] << 8) | buf[0]) / 2; - acc->ADCRaw[Y] = (int16_t)((buf[3] << 8) | buf[2]) / 2; - acc->ADCRaw[Z] = (int16_t)((buf[5] << 8) | buf[4]) / 2; - - return true; -} - -bool lsm303dlhcAccDetect(accDev_t *acc) -{ - bool ack; - uint8_t status; - - ack = i2cRead(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, LSM303DLHC_STATUS_REG_A, 1, &status); - if (!ack) { - return false; - } - - acc->initFn = lsm303dlhcAccInit; - acc->readFn = lsm303dlhcAccRead; - return true; -} -#endif diff --git a/src/main/drivers/accgyro/legacy/accgyro_lsm303dlhc.h b/src/main/drivers/accgyro/legacy/accgyro_lsm303dlhc.h deleted file mode 100644 index f8024ddcf1e..00000000000 --- a/src/main/drivers/accgyro/legacy/accgyro_lsm303dlhc.h +++ /dev/null @@ -1,434 +0,0 @@ -/* - * 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 - -/** - * @brief LSM303DLHC Status - */ - -/* LSM303DLHC ACC struct */ -typedef struct { - uint8_t Power_Mode; /* Power-down/Normal Mode */ - uint8_t AccOutput_DataRate; /* OUT data rate */ - uint8_t Axes_Enable; /* Axes enable */ - uint8_t High_Resolution; /* High Resolution enabling/disabling */ - uint8_t BlockData_Update; /* Block Data Update */ - uint8_t Endianness; /* Endian Data selection */ - uint8_t AccFull_Scale; /* Full Scale selection */ -} LSM303DLHCAcc_InitTypeDef; - -/* LSM303DLHC Acc High Pass Filter struct */ -typedef struct { - uint8_t HighPassFilter_Mode_Selection; /* Internal filter mode */ - uint8_t HighPassFilter_CutOff_Frequency; /* High pass filter cut-off frequency */ - uint8_t HighPassFilter_AOI1; /* HPF_enabling/disabling for AOI function on interrupt 1 */ - uint8_t HighPassFilter_AOI2; /* HPF_enabling/disabling for AOI function on interrupt 2 */ -} LSM303DLHCAcc_FilterConfigTypeDef; - -/* LSM303DLHC Mag struct */ -typedef struct { - uint8_t Temperature_Sensor; /* Temperature sensor enable/disable */ - uint8_t MagOutput_DataRate; /* OUT data rate */ - uint8_t Working_Mode; /* operating mode */ - uint8_t MagFull_Scale; /* Full Scale selection */ -} LSM303DLHCMag_InitTypeDef; -/** - * @} - */ - -/** @defgroup STM32F3_DISCOVERY_LSM303DLHC_Exported_Constants - * @{ - */ -#define LSM303DLHC_OK ((uint32_t) 0) -#define LSM303DLHC_FAIL ((uint32_t) 0) - -/* Uncomment the following line to use the default LSM303DLHC_TIMEOUT_UserCallback() - function implemented in stm32f3_discovery_lgd20.c file. - LSM303DLHC_TIMEOUT_UserCallback() function is called whenever a timeout condition - occure during communication (waiting transmit data register empty flag(TXE) - or waiting receive data register is not empty flag (RXNE)). */ -// #define USE_DEFAULT_TIMEOUT_CALLBACK - -/* Maximum Timeout values for flags waiting loops. These timeouts are not based - on accurate values, they just guarantee that the application will not remain - stuck if the I2C communication is corrupted. - You may modify these timeout values depending on CPU frequency and application - conditions (interrupts routines ...). */ -#define LSM303DLHC_FLAG_TIMEOUT ((uint32_t)0x1000) -#define LSM303DLHC_LONG_TIMEOUT ((uint32_t)(10 * LSM303DLHC_FLAG_TIMEOUT)) - -/******************************************************************************/ -/*************************** START REGISTER MAPPING **************************/ -/******************************************************************************/ -/* Acceleration Registers */ -#define LSM303DLHC_CTRL_REG1_A 0x20 /* Control register 1 acceleration */ -#define LSM303DLHC_CTRL_REG2_A 0x21 /* Control register 2 acceleration */ -#define LSM303DLHC_CTRL_REG3_A 0x22 /* Control register 3 acceleration */ -#define LSM303DLHC_CTRL_REG4_A 0x23 /* Control register 4 acceleration */ -#define LSM303DLHC_CTRL_REG5_A 0x24 /* Control register 5 acceleration */ -#define LSM303DLHC_CTRL_REG6_A 0x25 /* Control register 6 acceleration */ -#define LSM303DLHC_REFERENCE_A 0x26 /* Reference register acceleration */ -#define LSM303DLHC_STATUS_REG_A 0x27 /* Status register acceleration */ -#define LSM303DLHC_OUT_X_L_A 0x28 /* Output Register X acceleration */ -#define LSM303DLHC_OUT_X_H_A 0x29 /* Output Register X acceleration */ -#define LSM303DLHC_OUT_Y_L_A 0x2A /* Output Register Y acceleration */ -#define LSM303DLHC_OUT_Y_H_A 0x2B /* Output Register Y acceleration */ -#define LSM303DLHC_OUT_Z_L_A 0x2C /* Output Register Z acceleration */ -#define LSM303DLHC_OUT_Z_H_A 0x2D /* Output Register Z acceleration */ -#define LSM303DLHC_FIFO_CTRL_REG_A 0x2E /* Fifo control Register acceleration */ -#define LSM303DLHC_FIFO_SRC_REG_A 0x2F /* Fifo src Register acceleration */ - -#define LSM303DLHC_INT1_CFG_A 0x30 /* Interrupt 1 configuration Register acceleration */ -#define LSM303DLHC_INT1_SOURCE_A 0x31 /* Interrupt 1 source Register acceleration */ -#define LSM303DLHC_INT1_THS_A 0x32 /* Interrupt 1 Threshold register acceleration */ -#define LSM303DLHC_INT1_DURATION_A 0x33 /* Interrupt 1 DURATION register acceleration */ - -#define LSM303DLHC_INT2_CFG_A 0x34 /* Interrupt 2 configuration Register acceleration */ -#define LSM303DLHC_INT2_SOURCE_A 0x35 /* Interrupt 2 source Register acceleration */ -#define LSM303DLHC_INT2_THS_A 0x36 /* Interrupt 2 Threshold register acceleration */ -#define LSM303DLHC_INT2_DURATION_A 0x37 /* Interrupt 2 DURATION register acceleration */ - -#define LSM303DLHC_CLICK_CFG_A 0x38 /* Click configuration Register acceleration */ -#define LSM303DLHC_CLICK_SOURCE_A 0x39 /* Click 2 source Register acceleration */ -#define LSM303DLHC_CLICK_THS_A 0x3A /* Click 2 Threshold register acceleration */ - -#define LSM303DLHC_TIME_LIMIT_A 0x3B /* Time Limit Register acceleration */ -#define LSM303DLHC_TIME_LATENCY_A 0x3C /* Time Latency Register acceleration */ -#define LSM303DLHC_TIME_WINDOW_A 0x3D /* Time window register acceleration */ - -/* Magnetic field Registers */ -#define LSM303DLHC_CRA_REG_M 0x00 /* Control register A magnetic field */ -#define LSM303DLHC_CRB_REG_M 0x01 /* Control register B magnetic field */ -#define LSM303DLHC_MR_REG_M 0x02 /* Control register MR magnetic field */ -#define LSM303DLHC_OUT_X_H_M 0x03 /* Output Register X magnetic field */ -#define LSM303DLHC_OUT_X_L_M 0x04 /* Output Register X magnetic field */ -#define LSM303DLHC_OUT_Z_H_M 0x05 /* Output Register Z magnetic field */ -#define LSM303DLHC_OUT_Z_L_M 0x06 /* Output Register Z magnetic field */ -#define LSM303DLHC_OUT_Y_H_M 0x07 /* Output Register Y magnetic field */ -#define LSM303DLHC_OUT_Y_L_M 0x08 /* Output Register Y magnetic field */ - -#define LSM303DLHC_SR_REG_M 0x09 /* Status Register magnetic field */ -#define LSM303DLHC_IRA_REG_M 0x0A /* IRA Register magnetic field */ -#define LSM303DLHC_IRB_REG_M 0x0B /* IRB Register magnetic field */ -#define LSM303DLHC_IRC_REG_M 0x0C /* IRC Register magnetic field */ - -#define LSM303DLHC_TEMP_OUT_H_M 0x31 /* Temperature Register magnetic field */ -#define LSM303DLHC_TEMP_OUT_L_M 0x32 /* Temperature Register magnetic field */ -/******************************************************************************/ -/**************************** END REGISTER MAPPING ***************************/ -/******************************************************************************/ - -#define ACC_I2C_ADDRESS 0x32 -#define MAG_I2C_ADDRESS 0x3C - -/** @defgroup Acc_Power_Mode_selection - * @{ - */ -#define LSM303DLHC_NORMAL_MODE ((uint8_t)0x00) -#define LSM303DLHC_LOWPOWER_MODE ((uint8_t)0x08) -/** - * @} - */ - -/** @defgroup Acc_OutPut_DataRate_Selection - * @{ - */ -#define LSM303DLHC_ODR_1_HZ ((uint8_t)0x10) /*!< Output Data Rate = 1 Hz */ -#define LSM303DLHC_ODR_10_HZ ((uint8_t)0x20) /*!< Output Data Rate = 10 Hz */ -#define LSM303DLHC_ODR_25_HZ ((uint8_t)0x30) /*!< Output Data Rate = 25 Hz */ -#define LSM303DLHC_ODR_50_HZ ((uint8_t)0x40) /*!< Output Data Rate = 50 Hz */ -#define LSM303DLHC_ODR_100_HZ ((uint8_t)0x50) /*!< Output Data Rate = 100 Hz */ -#define LSM303DLHC_ODR_200_HZ ((uint8_t)0x60) /*!< Output Data Rate = 200 Hz */ -#define LSM303DLHC_ODR_400_HZ ((uint8_t)0x70) /*!< Output Data Rate = 400 Hz */ -#define LSM303DLHC_ODR_1620_HZ_LP ((uint8_t)0x80) /*!< Output Data Rate = 1620 Hz only in Low Power Mode */ -#define LSM303DLHC_ODR_1344_HZ ((uint8_t)0x90) /*!< Output Data Rate = 1344 Hz in Normal mode and 5376 Hz in Low Power Mode */ - -/** - * @} - */ - -/** @defgroup Acc_Axes_Selection - * @{ - */ -#define LSM303DLHC_X_ENABLE ((uint8_t)0x01) -#define LSM303DLHC_Y_ENABLE ((uint8_t)0x02) -#define LSM303DLHC_Z_ENABLE ((uint8_t)0x04) -#define LSM303DLHC_AXES_ENABLE ((uint8_t)0x07) -#define LSM303DLHC_AXES_DISABLE ((uint8_t)0x00) -/** - * @} - */ - -/** @defgroup Acc_High_Resolution - * @{ - */ -#define LSM303DLHC_HR_ENABLE ((uint8_t)0x08) -#define LSM303DLHC_HR_DISABLE ((uint8_t)0x00) -/** - * @} - */ - -/** @defgroup Acc_Full_Scale_Selection - * @{ - */ -#define LSM303DLHC_FULLSCALE_2G ((uint8_t)0x00) /*!< +/-2 g */ -#define LSM303DLHC_FULLSCALE_4G ((uint8_t)0x10) /*!< +/-4 g */ -#define LSM303DLHC_FULLSCALE_8G ((uint8_t)0x20) /*!< +/-8 g */ -#define LSM303DLHC_FULLSCALE_16G ((uint8_t)0x30) /*!< +/-16 g */ -/** - * @} - */ - -/** @defgroup Acc_Block_Data_Update - * @{ - */ -#define LSM303DLHC_BlockUpdate_Continous ((uint8_t)0x00) /*!< Continuos Update */ -#define LSM303DLHC_BlockUpdate_Single ((uint8_t)0x80) /*!< Single Update: output registers not updated until MSB and LSB reading */ -/** - * @} - */ - -/** @defgroup Acc_Endian_Data_selection - * @{ - */ -#define LSM303DLHC_BLE_LSB ((uint8_t)0x00) /*!< Little Endian: data LSB @ lower address */ -#define LSM303DLHC_BLE_MSB ((uint8_t)0x40) /*!< Big Endian: data MSB @ lower address */ -/** - * @} - */ - -/** @defgroup Acc_Boot_Mode_selection - * @{ - */ -#define LSM303DLHC_BOOT_NORMALMODE ((uint8_t)0x00) -#define LSM303DLHC_BOOT_REBOOTMEMORY ((uint8_t)0x80) -/** - * @} - */ - -/** @defgroup Acc_High_Pass_Filter_Mode - * @{ - */ -#define LSM303DLHC_HPM_NORMAL_MODE_RES ((uint8_t)0x00) -#define LSM303DLHC_HPM_REF_SIGNAL ((uint8_t)0x40) -#define LSM303DLHC_HPM_NORMAL_MODE ((uint8_t)0x80) -#define LSM303DLHC_HPM_AUTORESET_INT ((uint8_t)0xC0) -/** - * @} - */ - -/** @defgroup Acc_High_Pass_CUT OFF_Frequency - * @{ - */ -#define LSM303DLHC_HPFCF_8 ((uint8_t)0x00) -#define LSM303DLHC_HPFCF_16 ((uint8_t)0x10) -#define LSM303DLHC_HPFCF_32 ((uint8_t)0x20) -#define LSM303DLHC_HPFCF_64 ((uint8_t)0x30) -/** - * @} - */ - -/** @defgroup Acc_High_Pass_Filter_status - * @{ - */ -#define LSM303DLHC_HIGHPASSFILTER_DISABLE ((uint8_t)0x00) -#define LSM303DLHC_HIGHPASSFILTER_ENABLE ((uint8_t)0x08) -/** - * @} - */ - -/** @defgroup Acc_High_Pass_Filter_Click_status - * @{ - */ -#define LSM303DLHC_HPF_CLICK_DISABLE ((uint8_t)0x00) -#define LSM303DLHC_HPF_CLICK_ENABLE ((uint8_t)0x04) -/** - * @} - */ - -/** @defgroup Acc_High_Pass_Filter_AOI1_status - * @{ - */ -#define LSM303DLHC_HPF_AOI1_DISABLE ((uint8_t)0x00) -#define LSM303DLHC_HPF_AOI1_ENABLE ((uint8_t)0x01) -/** - * @} - */ - -/** @defgroup Acc_High_Pass_Filter_AOI2_status - * @{ - */ -#define LSM303DLHC_HPF_AOI2_DISABLE ((uint8_t)0x00) -#define LSM303DLHC_HPF_AOI2_ENABLE ((uint8_t)0x02) -/** - * @} - */ - -/** @defgroup Acc_LSM303DLHC_Interrupt1_Configuration_definition - * @{ - */ -#define LSM303DLHC_IT1_CLICK ((uint8_t)0x80) -#define LSM303DLHC_IT1_AOI1 ((uint8_t)0x40) -#define LSM303DLHC_IT1_AOI2 ((uint8_t)0x20) -#define LSM303DLHC_IT1_DRY1 ((uint8_t)0x10) -#define LSM303DLHC_IT1_DRY2 ((uint8_t)0x08) -#define LSM303DLHC_IT1_WTM ((uint8_t)0x04) -#define LSM303DLHC_IT1_OVERRUN ((uint8_t)0x02) -/** - * @} - */ - -/** @defgroup Acc_LSM303DLHC_Interrupt2_Configuration_definition - * @{ - */ -#define LSM303DLHC_IT2_CLICK ((uint8_t)0x80) -#define LSM303DLHC_IT2_INT1 ((uint8_t)0x40) -#define LSM303DLHC_IT2_INT2 ((uint8_t)0x20) -#define LSM303DLHC_IT2_BOOT ((uint8_t)0x10) -#define LSM303DLHC_IT2_ACT ((uint8_t)0x08) -#define LSM303DLHC_IT2_HLACTIVE ((uint8_t)0x02) -/** - * @} - */ - -/** @defgroup Acc_INT_Combination_Status - * @{ - */ -#define LSM303DLHC_OR_COMBINATION ((uint8_t)0x00) /*!< OR combination of enabled IRQs */ -#define LSM303DLHC_AND_COMBINATION ((uint8_t)0x80) /*!< AND combination of enabled IRQs */ -#define LSM303DLHC_MOV_RECOGNITION ((uint8_t)0x40) /*!< 6D movement recognition */ -#define LSM303DLHC_POS_RECOGNITION ((uint8_t)0xC0) /*!< 6D position recognition */ -/** - * @} - */ - -/** @defgroup Acc_INT_Axes - * @{ - */ -#define LSM303DLHC_Z_HIGH ((uint8_t)0x20) /*!< Z High enabled IRQs */ -#define LSM303DLHC_Z_LOW ((uint8_t)0x10) /*!< Z low enabled IRQs */ -#define LSM303DLHC_Y_HIGH ((uint8_t)0x08) /*!< Y High enabled IRQs */ -#define LSM303DLHC_Y_LOW ((uint8_t)0x04) /*!< Y low enabled IRQs */ -#define LSM303DLHC_X_HIGH ((uint8_t)0x02) /*!< X High enabled IRQs */ -#define LSM303DLHC_X_LOW ((uint8_t)0x01) /*!< X low enabled IRQs */ -/** - * @} - */ - -/** @defgroup Acc_INT_Click - * @{ - */ -#define LSM303DLHC_Z_DOUBLE_CLICK ((uint8_t)0x20) /*!< Z double click IRQs */ -#define LSM303DLHC_Z_SINGLE_CLICK ((uint8_t)0x10) /*!< Z single click IRQs */ -#define LSM303DLHC_Y_DOUBLE_CLICK ((uint8_t)0x08) /*!< Y double click IRQs */ -#define LSM303DLHC_Y_SINGLE_CLICK ((uint8_t)0x04) /*!< Y single click IRQs */ -#define LSM303DLHC_X_DOUBLE_CLICK ((uint8_t)0x02) /*!< X double click IRQs */ -#define LSM303DLHC_X_SINGLE_CLICK ((uint8_t)0x01) /*!< X single click IRQs */ -/** - * @} - */ - -/** @defgroup Acc_INT1_Interrupt_status - * @{ - */ -#define LSM303DLHC_INT1INTERRUPT_DISABLE ((uint8_t)0x00) -#define LSM303DLHC_INT1INTERRUPT_ENABLE ((uint8_t)0x80) -/** - * @} - */ - -/** @defgroup Acc_INT1_Interrupt_ActiveEdge - * @{ - */ -#define LSM303DLHC_INT1INTERRUPT_LOW_EDGE ((uint8_t)0x20) -#define LSM303DLHC_INT1INTERRUPT_HIGH_EDGE ((uint8_t)0x00) -/** - * @} - */ - -/** @defgroup Mag_Data_Rate - * @{ - */ -#define LSM303DLHC_ODR_0_75_HZ ((uint8_t) 0x00) /*!< Output Data Rate = 0.75 Hz */ -#define LSM303DLHC_ODR_1_5_HZ ((uint8_t) 0x04) /*!< Output Data Rate = 1.5 Hz */ -#define LSM303DLHC_ODR_3_0_HZ ((uint8_t) 0x08) /*!< Output Data Rate = 3 Hz */ -#define LSM303DLHC_ODR_7_5_HZ ((uint8_t) 0x0C) /*!< Output Data Rate = 7.5 Hz */ -#define LSM303DLHC_ODR_15_HZ ((uint8_t) 0x10) /*!< Output Data Rate = 15 Hz */ -#define LSM303DLHC_ODR_30_HZ ((uint8_t) 0x14) /*!< Output Data Rate = 30 Hz */ -#define LSM303DLHC_ODR_75_HZ ((uint8_t) 0x18) /*!< Output Data Rate = 75 Hz */ -#define LSM303DLHC_ODR_220_HZ ((uint8_t) 0x1C) /*!< Output Data Rate = 220 Hz */ -/** - * @} - */ - -/** @defgroup Mag_Full_Scale - * @{ - */ -#define LSM303DLHC_FS_1_3_GA ((uint8_t) 0x20) /*!< Full scale = +/-1.3 Gauss */ -#define LSM303DLHC_FS_1_9_GA ((uint8_t) 0x40) /*!< Full scale = +/-1.9 Gauss */ -#define LSM303DLHC_FS_2_5_GA ((uint8_t) 0x60) /*!< Full scale = +/-2.5 Gauss */ -#define LSM303DLHC_FS_4_0_GA ((uint8_t) 0x80) /*!< Full scale = +/-4.0 Gauss */ -#define LSM303DLHC_FS_4_7_GA ((uint8_t) 0xA0) /*!< Full scale = +/-4.7 Gauss */ -#define LSM303DLHC_FS_5_6_GA ((uint8_t) 0xC0) /*!< Full scale = +/-5.6 Gauss */ -#define LSM303DLHC_FS_8_1_GA ((uint8_t) 0xE0) /*!< Full scale = +/-8.1 Gauss */ -/** - * @} - */ - -/** - * @defgroup Magnetometer_Sensitivity - * @{ - */ -#define LSM303DLHC_M_SENSITIVITY_XY_1_3Ga 1100 /*!< magnetometer X Y axes sensitivity for 1.3 Ga full scale [LSB/Ga] */ -#define LSM303DLHC_M_SENSITIVITY_XY_1_9Ga 855 /*!< magnetometer X Y axes sensitivity for 1.9 Ga full scale [LSB/Ga] */ -#define LSM303DLHC_M_SENSITIVITY_XY_2_5Ga 670 /*!< magnetometer X Y axes sensitivity for 2.5 Ga full scale [LSB/Ga] */ -#define LSM303DLHC_M_SENSITIVITY_XY_4Ga 450 /*!< magnetometer X Y axes sensitivity for 4 Ga full scale [LSB/Ga] */ -#define LSM303DLHC_M_SENSITIVITY_XY_4_7Ga 400 /*!< magnetometer X Y axes sensitivity for 4.7 Ga full scale [LSB/Ga] */ -#define LSM303DLHC_M_SENSITIVITY_XY_5_6Ga 330 /*!< magnetometer X Y axes sensitivity for 5.6 Ga full scale [LSB/Ga] */ -#define LSM303DLHC_M_SENSITIVITY_XY_8_1Ga 230 /*!< magnetometer X Y axes sensitivity for 8.1 Ga full scale [LSB/Ga] */ -#define LSM303DLHC_M_SENSITIVITY_Z_1_3Ga 980 /*!< magnetometer Z axis sensitivity for 1.3 Ga full scale [LSB/Ga] */ -#define LSM303DLHC_M_SENSITIVITY_Z_1_9Ga 760 /*!< magnetometer Z axis sensitivity for 1.9 Ga full scale [LSB/Ga] */ -#define LSM303DLHC_M_SENSITIVITY_Z_2_5Ga 600 /*!< magnetometer Z axis sensitivity for 2.5 Ga full scale [LSB/Ga] */ -#define LSM303DLHC_M_SENSITIVITY_Z_4Ga 400 /*!< magnetometer Z axis sensitivity for 4 Ga full scale [LSB/Ga] */ -#define LSM303DLHC_M_SENSITIVITY_Z_4_7Ga 355 /*!< magnetometer Z axis sensitivity for 4.7 Ga full scale [LSB/Ga] */ -#define LSM303DLHC_M_SENSITIVITY_Z_5_6Ga 295 /*!< magnetometer Z axis sensitivity for 5.6 Ga full scale [LSB/Ga] */ -#define LSM303DLHC_M_SENSITIVITY_Z_8_1Ga 205 /*!< magnetometer Z axis sensitivity for 8.1 Ga full scale [LSB/Ga] */ -/** - * @} - */ - -/** @defgroup Mag_Working_Mode - * @{ - */ -#define LSM303DLHC_CONTINUOS_CONVERSION ((uint8_t) 0x00) /*!< Continuous-Conversion Mode */ -#define LSM303DLHC_SINGLE_CONVERSION ((uint8_t) 0x01) /*!< Single-Conversion Mode */ -#define LSM303DLHC_SLEEP ((uint8_t) 0x02) /*!< Sleep Mode */ -/** - * @} - */ - -/** @defgroup Mag_Temperature_Sensor - * @{ - */ -#define LSM303DLHC_TEMPSENSOR_ENABLE ((uint8_t) 0x80) /*!< Temp sensor Enable */ -#define LSM303DLHC_TEMPSENSOR_DISABLE ((uint8_t) 0x00) /*!< Temp sensor Disable */ - -bool lsm303dlhcAccDetect(accDev_t *acc); diff --git a/src/main/drivers/accgyro/legacy/accgyro_mma845x.c b/src/main/drivers/accgyro/legacy/accgyro_mma845x.c deleted file mode 100644 index 1b51e7adab9..00000000000 --- a/src/main/drivers/accgyro/legacy/accgyro_mma845x.c +++ /dev/null @@ -1,144 +0,0 @@ -/* - * 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 "platform.h" - -#ifdef USE_ACC_MMA8452 - -#include "drivers/io.h" -#include "drivers/bus_i2c.h" - -#include "drivers/sensor.h" -#include "drivers/accgyro/accgyro.h" -#include "accgyro_mma845x.h" - -#ifndef MMA8452_I2C_INSTANCE -#define MMA8452_I2C_INSTANCE I2CDEV_1 -#endif - -// MMA8452QT, Standard address 0x1C -// ACC_INT2 routed to PA5 - -#define MMA8452_ADDRESS 0x1C - -#define MMA8452_DEVICE_SIGNATURE 0x2A -#define MMA8451_DEVICE_SIGNATURE 0x1A - -#define MMA8452_STATUS 0x00 -#define MMA8452_OUT_X_MSB 0x01 -#define MMA8452_WHO_AM_I 0x0D -#define MMA8452_XYZ_DATA_CFG 0x0E -#define MMA8452_HP_FILTER_CUTOFF 0x0F -#define MMA8452_CTRL_REG1 0x2A -#define MMA8452_CTRL_REG2 0x2B -#define MMA8452_CTRL_REG3 0x2C -#define MMA8452_CTRL_REG4 0x2D -#define MMA8452_CTRL_REG5 0x2E - -#define MMA8452_FS_RANGE_8G 0x02 -#define MMA8452_FS_RANGE_4G 0x01 -#define MMA8452_FS_RANGE_2G 0x00 - -#define MMA8452_HPF_CUTOFF_LV1 0x00 -#define MMA8452_HPF_CUTOFF_LV2 0x01 -#define MMA8452_HPF_CUTOFF_LV3 0x02 -#define MMA8452_HPF_CUTOFF_LV4 0x03 - -#define MMA8452_CTRL_REG2_B7_ST 0x80 -#define MMA8452_CTRL_REG2_B6_RST 0x40 -#define MMA8452_CTRL_REG2_B4_SMODS1 0x10 -#define MMA8452_CTRL_REG2_B3_SMODS0 0x08 -#define MMA8452_CTRL_REG2_B2_SLPE 0x04 -#define MMA8452_CTRL_REG2_B1_MODS1 0x02 -#define MMA8452_CTRL_REG2_B0_MODS0 0x01 - -#define MMA8452_CTRL_REG2_MODS_LP 0x03 -#define MMA8452_CTRL_REG2_MODS_HR 0x02 -#define MMA8452_CTRL_REG2_MODS_LNLP 0x01 -#define MMA8452_CTRL_REG2_MODS_NOR 0x00 - -#define MMA8452_CTRL_REG3_IPOL 0x02 -#define MMA8452_CTRL_REG4_INT_EN_DRDY 0x01 - -#define MMA8452_CTRL_REG1_LNOISE 0x04 -#define MMA8452_CTRL_REG1_ACTIVE 0x01 - -static uint8_t device_id; - -static inline void mma8451ConfigureInterrupt(void) -{ -#ifdef MMA8451_INT_PIN - IOInit(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), OWNER_GYRO_EXTI, 0); - // TODO - maybe pullup / pulldown ? - IOConfigGPIO(IOGetByTag(IO_TAG(MMA8451_INT_PIN)), IOCFG_IN_FLOATING); -#endif - - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH) - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver) - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2 -} - -static void mma8452Init(accDev_t *acc) -{ - - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_XYZ_DATA_CFG, MMA8452_FS_RANGE_8G); - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_HP_FILTER_CUTOFF, MMA8452_HPF_CUTOFF_LV4); - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG2, MMA8452_CTRL_REG2_MODS_HR | MMA8452_CTRL_REG2_MODS_HR << 3); // High resolution measurement in both sleep and active modes - - mma8451ConfigureInterrupt(); - - i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, MMA8452_CTRL_REG1_LNOISE | MMA8452_CTRL_REG1_ACTIVE); // Turn on measurements, low noise at max scale mode, Data Rate 800Hz. LNoise mode makes range +-4G. - - acc->acc_1G = 256; -} - -static bool mma8452Read(accDev_t *acc) -{ - uint8_t buf[6]; - - if (!i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_OUT_X_MSB, 6, buf)) { - return false; - } - - acc->ADCRaw[0] = ((int16_t)((buf[0] << 8) | buf[1]) >> 2) / 4; - acc->ADCRaw[1] = ((int16_t)((buf[2] << 8) | buf[3]) >> 2) / 4; - acc->ADCRaw[2] = ((int16_t)((buf[4] << 8) | buf[5]) >> 2) / 4; - - return true; -} - -bool mma8452Detect(accDev_t *acc) -{ - uint8_t sig = 0; - bool ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig); - - if (!ack || (sig != MMA8452_DEVICE_SIGNATURE && sig != MMA8451_DEVICE_SIGNATURE)) - return false; - - acc->initFn = mma8452Init; - acc->readFn = mma8452Read; - device_id = sig; - return true; -} -#endif diff --git a/src/main/drivers/accgyro/legacy/accgyro_mma845x.h b/src/main/drivers/accgyro/legacy/accgyro_mma845x.h deleted file mode 100644 index 445bc08ee26..00000000000 --- a/src/main/drivers/accgyro/legacy/accgyro_mma845x.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * 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 - -bool mma8452Detect(accDev_t *acc); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index fd81054a28f..208dc798eed 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -57,7 +57,7 @@ static inline void calibrateAccelerometer(void) { if (!accIsCalibrationComplete()) { // acc.accADC is held at 0 until calibration is completed - performAcclerationCalibration(&accelerometerConfigMutable()->accelerometerTrims); + performAccelerometerCalibration(&accelerometerConfigMutable()->accelerometerTrims); } if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) { diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index df9168ebaec..66a139d6640 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -31,14 +31,11 @@ #include "sensors/sensors.h" // Type of accelerometer used/detected +// Acc hardware types were updated in PR #14087 (removed ACC_ADXL345, ACC_MMA8452, ACC_BMA280, ACC_LSM303DLHC) typedef enum { ACC_DEFAULT, ACC_NONE, - ACC_ADXL345, ACC_MPU6050, - ACC_MMA8452, - ACC_BMA280, - ACC_LSM303DLHC, ACC_MPU6000, ACC_MPU6500, ACC_MPU9250, diff --git a/src/main/sensors/acceleration_init.c b/src/main/sensors/acceleration_init.c index 62ce0933957..127ea4a80cf 100644 --- a/src/main/sensors/acceleration_init.c +++ b/src/main/sensors/acceleration_init.c @@ -35,37 +35,25 @@ #include "config/feature.h" #include "drivers/accgyro/accgyro.h" -#include "drivers/accgyro/accgyro_virtual.h" #include "drivers/accgyro/accgyro_mpu.h" -#include "drivers/accgyro/accgyro_mpu3050.h" +#include "drivers/accgyro/accgyro_virtual.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_icm426xx.h" + #include "drivers/accgyro/accgyro_spi_lsm6dso.h" +#include "drivers/accgyro/accgyro_spi_lsm6dsv16x.h" + #include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" #include "drivers/accgyro/accgyro_spi_mpu9250.h" -#include "drivers/accgyro/accgyro_spi_lsm6dsv16x.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 "config/config.h" @@ -85,18 +73,6 @@ #include "acceleration_init.h" -#if !defined(USE_ACC_ADXL345) && !defined(USE_ACC_BMA280) && !defined(USE_ACC_LSM303DLHC) \ - && !defined(USE_ACC_MMA8452) && !defined(USE_ACC_LSM303DLHC) \ - && !defined(USE_ACC_MPU6000) && !defined(USE_ACC_MPU6050) && !defined(USE_ACC_MPU6500) \ - && !defined(USE_ACC_SPI_MPU6000) && !defined(USE_ACC_SPI_MPU6500) && !defined(USE_ACC_SPI_MPU9250) \ - && !defined(USE_ACC_SPI_ICM20602) && !defined(USE_ACC_SPI_ICM20649) && !defined(USE_ACC_SPI_ICM20689) \ - && !defined(USE_ACCGYRO_BMI160) && !defined(USE_ACCGYRO_BMI270) \ - && !defined(USE_ACC_SPI_ICM42605) && !defined(USE_ACC_SPI_ICM42688P) \ - && !defined(USE_ACCGYRO_LSM6DSO) && !defined(USE_ACCGYRO_LSM6DSV16X) \ - && !defined(USE_VIRTUAL_ACC) -#error At least one USE_ACC device definition required -#endif - #define CALIBRATING_ACC_CYCLES 400 FAST_DATA_ZERO_INIT accelerationRuntime_t accelerationRuntime; @@ -158,36 +134,12 @@ 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)) { @@ -197,24 +149,6 @@ bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) 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)) { @@ -438,7 +372,7 @@ static bool isOnFirstAccelerationCalibrationCycle(void) return accelerationRuntime.calibratingA == CALIBRATING_ACC_CYCLES; } -void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims) +void performAccelerometerCalibration(rollAndPitchTrims_t *rollAndPitchTrims) { static int32_t a[3]; diff --git a/src/main/sensors/acceleration_init.h b/src/main/sensors/acceleration_init.h index d40b10f0b4e..d87c03a7f7e 100644 --- a/src/main/sensors/acceleration_init.h +++ b/src/main/sensors/acceleration_init.h @@ -37,5 +37,5 @@ typedef struct accelerationRuntime_s { extern accelerationRuntime_t accelerationRuntime; -void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims); +void performAccelerometerCalibration(rollAndPitchTrims_t *rollAndPitchTrims); void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims); diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index 6ec8dc791d3..c09d466e5a3 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -37,27 +37,24 @@ #include "drivers/accgyro/accgyro.h" #include "drivers/accgyro/accgyro_virtual.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_icm426xx.h" + +#include "drivers/accgyro/accgyro_spi_l3gd20.h" #include "drivers/accgyro/accgyro_spi_lsm6dso.h" +#include "drivers/accgyro/accgyro_spi_lsm6dsv16x.h" + #include "drivers/accgyro/accgyro_spi_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" #include "drivers/accgyro/accgyro_spi_mpu9250.h" -#include "drivers/accgyro/accgyro_spi_lsm6dsv16x.h" - -#ifdef USE_GYRO_L3GD20 -#include "drivers/accgyro/accgyro_spi_l3gd20.h" -#endif - -#ifdef USE_GYRO_L3G4200D -#include "drivers/accgyro/legacy/accgyro_l3g4200d.h" -#endif #include "drivers/accgyro/gyro_sync.h" @@ -72,17 +69,6 @@ #include "sensors/gyro.h" #include "sensors/sensors.h" -#if !defined(USE_GYRO_L3G4200D) && !defined(USE_GYRO_L3GD20) \ - && !defined(USE_GYRO_MPU3050) && !defined(USE_GYRO_MPU6050) && !defined(USE_GYRO_MPU6500) \ - && !defined(USE_GYRO_SPI_MPU6000) && !defined(USE_GYRO_SPI_MPU6500) && !defined(USE_GYRO_SPI_MPU9250) \ - && !defined(USE_GYRO_SPI_ICM20602) && !defined(USE_GYRO_SPI_ICM20649) && !defined(USE_GYRO_SPI_ICM20689) \ - && !defined(USE_ACCGYRO_BMI160) && !defined(USE_ACCGYRO_BMI270) \ - && !defined(USE_GYRO_SPI_ICM42605) && !defined(USE_GYRO_SPI_ICM42688P) \ - && !defined(USE_ACCGYRO_LSM6DSO) && !defined(USE_ACCGYRO_LSM6DSV16X) \ - && !defined(USE_VIRTUAL_GYRO) -#error At least one USE_GYRO device definition required -#endif - #ifdef USE_MULTI_GYRO #define ACTIVE_GYRO ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyro.gyroSensor2 : &gyro.gyroSensor1) #else @@ -319,8 +305,6 @@ void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config) case GYRO_DEFAULT: case GYRO_VIRTUAL: case GYRO_MPU6050: - case GYRO_L3G4200D: - case GYRO_MPU3050: case GYRO_L3GD20: case GYRO_BMI160: case GYRO_BMI270: @@ -367,24 +351,6 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev) FALLTHROUGH; #endif -#ifdef USE_GYRO_L3G4200D - case GYRO_L3G4200D: - if (l3g4200dDetect(dev)) { - gyroHardware = GYRO_L3G4200D; - break; - } - FALLTHROUGH; -#endif - -#ifdef USE_GYRO_MPU3050 - case GYRO_MPU3050: - if (mpu3050Detect(dev)) { - gyroHardware = GYRO_MPU3050; - break; - } - FALLTHROUGH; -#endif - #ifdef USE_GYRO_L3GD20 case GYRO_L3GD20: if (l3gd20GyroDetect(dev)) { diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 508453ab7ec..a69492dcf27 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -91,6 +91,45 @@ // normalize serial ports definitions #include "serial_post.h" +#if defined(USE_ACC) \ + && !defined(USE_ACC_MPU6000) \ + && !defined(USE_ACC_MPU6050) \ + && !defined(USE_ACC_MPU6500) \ + && !defined(USE_ACCGYRO_BMI160) \ + && !defined(USE_ACCGYRO_BMI270) \ + && !defined(USE_ACC_SPI_ICM20602) \ + && !defined(USE_ACC_SPI_ICM20649) \ + && !defined(USE_ACC_SPI_ICM20689) \ + && !defined(USE_ACC_SPI_ICM42605) \ + && !defined(USE_ACC_SPI_ICM42688P) \ + && !defined(USE_ACCGYRO_LSM6DSO) \ + && !defined(USE_ACCGYRO_LSM6DSV16X) \ + && !defined(USE_ACC_SPI_MPU6000) \ + && !defined(USE_ACC_SPI_MPU6500) \ + && !defined(USE_ACC_SPI_MPU9250) \ + && !defined(USE_VIRTUAL_ACC) +#error At least one USE_ACC device definition required +#endif + +#if defined(USE_GYRO) \ + && !defined(USE_GYRO_MPU6050) \ + && !defined(USE_GYRO_MPU6500) \ + && !defined(USE_ACCGYRO_BMI160) \ + && !defined(USE_ACCGYRO_BMI270) \ + && !defined(USE_GYRO_SPI_ICM20602) \ + && !defined(USE_GYRO_SPI_ICM20649) \ + && !defined(USE_GYRO_SPI_ICM20689) \ + && !defined(USE_GYRO_SPI_ICM42605) \ + && !defined(USE_GYRO_SPI_ICM42688P) \ + && !defined(USE_ACCGYRO_LSM6DSO) \ + && !defined(USE_ACCGYRO_LSM6DSV16X) \ + && !defined(USE_GYRO_SPI_MPU6000) \ + && !defined(USE_GYRO_SPI_MPU6500) \ + && !defined(USE_GYRO_SPI_MPU9250) \ + && !defined(USE_VIRTUAL_GYRO) +#error At least one USE_GYRO device definition required +#endif + #if defined(USE_MAG) && !defined(USE_VIRTUAL_MAG) #ifndef USE_MAG_DATA_READY_SIGNAL @@ -426,12 +465,6 @@ #endif // Generate USE_SPI_GYRO or USE_I2C_GYRO -#if defined(USE_GYRO_L3G4200D) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6000) || defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU6500) -#ifndef USE_I2C_GYRO -#define USE_I2C_GYRO -#endif -#endif - #if defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) \ || defined(USE_GYRO_L3GD20) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI160) \ || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSV16X) || defined(USE_ACCGYRO_LSM6DSO)