diff --git a/mk/source.mk b/mk/source.mk
index e4c60d55cb..a9a8ed0e49 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 1bfe829b79..9a9a749093 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 e4d6926031..0c63a8258f 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 4febfe2382..d1ebe2857a 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 a52b1a7984..0000000000
--- 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 3de31fae84..0000000000
--- 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 e4a0c86dfc..ae607d303a 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 ada4dadf78..3a15d0f057 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 9691c77818..7a2e02408b 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 633a614d29..eca0f03b6d 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 d64cd9fccb..0000000000
--- 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 27ad0e5b04..0000000000
--- 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 5eab61d8ec..0000000000
--- 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 93fac71f40..0000000000
--- 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 6dbcf4b96c..0000000000
--- 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 258cc2eeb3..0000000000
--- 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 21a8831244..0000000000
--- 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 f8024ddcf1..0000000000
--- 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 1b51e7adab..0000000000
--- 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 445bc08ee2..0000000000
--- 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 fd81054a28..208dc798ee 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 df9168ebae..66a139d664 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 62ce093395..127ea4a80c 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 @@ retry:
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 d40b10f0b4..d87c03a7f7 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 6ec8dc791d..c09d466e5a 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 508453ab7e..a69492dcf2 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)