diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index f0759986cd..28a8e3db45 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -125,14 +125,14 @@
// sync with accelerationSensor_e
const char * const lookupTableAccHardware[] = {
"AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC",
- "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689",
+ "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605",
"BMI160", "FAKE"
};
-// sync with gyroSensor_e
+// sync with gyroHardware_e
const char * const lookupTableGyroHardware[] = {
"AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20",
- "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689",
+ "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605",
"BMI160", "FAKE"
};
diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h
index d2f7f6ee2c..73f45ed446 100644
--- a/src/main/drivers/accgyro/accgyro.h
+++ b/src/main/drivers/accgyro/accgyro.h
@@ -52,6 +52,7 @@ typedef enum {
GYRO_ICM20608G,
GYRO_ICM20649,
GYRO_ICM20689,
+ GYRO_ICM42605,
GYRO_BMI160,
GYRO_FAKE
} gyroHardware_e;
diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c
index 5f0dfae6c0..6d700cad4a 100644
--- a/src/main/drivers/accgyro/accgyro_mpu.c
+++ b/src/main/drivers/accgyro/accgyro_mpu.c
@@ -49,6 +49,7 @@
#include "drivers/accgyro/accgyro_spi_bmi160.h"
#include "drivers/accgyro/accgyro_spi_icm20649.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
+#include "drivers/accgyro/accgyro_spi_icm42605.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
@@ -210,6 +211,9 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
#ifdef USE_GYRO_SPI_ICM20689
icm20689SpiDetect, // icm20689SpiDetect detects ICM20602 and ICM20689
#endif
+#ifdef USE_GYRO_SPI_ICM42605
+ icm42605SpiDetect,
+#endif
#ifdef USE_ACCGYRO_BMI160
bmi160Detect,
#endif
diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h
index a90fa2f011..ea546db60b 100644
--- a/src/main/drivers/accgyro/accgyro_mpu.h
+++ b/src/main/drivers/accgyro/accgyro_mpu.h
@@ -45,6 +45,7 @@
#define ICM20608G_WHO_AM_I_CONST (0xAF)
#define ICM20649_WHO_AM_I_CONST (0xE1)
#define ICM20689_WHO_AM_I_CONST (0x98)
+#define ICM42605_WHO_AM_I_CONST (0x42)
// RA = Register Address
@@ -198,6 +199,7 @@ typedef enum {
ICM_20608_SPI,
ICM_20649_SPI,
ICM_20689_SPI,
+ ICM_42605_SPI,
BMI_160_SPI,
L3GD20_SPI,
} mpuSensor_e;
diff --git a/src/main/drivers/accgyro/accgyro_spi_icm42605.c b/src/main/drivers/accgyro/accgyro_spi_icm42605.c
new file mode 100644
index 0000000000..3106a32961
--- /dev/null
+++ b/src/main/drivers/accgyro/accgyro_spi_icm42605.c
@@ -0,0 +1,283 @@
+/*
+ * 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 .
+ */
+
+/*
+ * Author: Dominic Clifton
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+#include "build/debug.h"
+
+#include "drivers/accgyro/accgyro.h"
+#include "drivers/accgyro/accgyro_mpu.h"
+#include "drivers/accgyro/accgyro_spi_icm42605.h"
+#include "drivers/bus_spi.h"
+#include "drivers/exti.h"
+#include "drivers/io.h"
+#include "drivers/sensor.h"
+#include "drivers/time.h"
+
+#define ICM42605_RA_PWR_MGMT0 0x4E
+
+#define ICM42605_PWR_MGMT0_ACCEL_MODE_LN (3 << 0)
+#define ICM42605_PWR_MGMT0_GYRO_MODE_LN (3 << 2)
+#define ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF (0 << 5)
+#define ICM42605_PWR_MGMT0_TEMP_DISABLE_ON (1 << 5)
+
+#define ICM42605_RA_GYRO_CONFIG0 0x4F
+#define ICM42605_RA_ACCEL_CONFIG0 0x50
+
+#define ICM42605_RA_GYRO_ACCEL_CONFIG0 0x52
+
+#define ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY (14 << 4)
+#define ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY (14 << 0)
+
+#define ICM42605_RA_GYRO_DATA_X1 0x25
+#define ICM42605_RA_ACCEL_DATA_X1 0x1F
+
+#define ICM42605_RA_INT_CONFIG 0x14
+#define ICM42605_INT1_MODE_PULSED (0 << 2)
+#define ICM42605_INT1_MODE_LATCHED (1 << 2)
+#define ICM42605_INT1_DRIVE_CIRCUIT_OD (0 << 1)
+#define ICM42605_INT1_DRIVE_CIRCUIT_PP (1 << 1)
+#define ICM42605_INT1_POLARITY_ACTIVE_LOW (0 << 0)
+#define ICM42605_INT1_POLARITY_ACTIVE_HIGH (1 << 0)
+
+#define ICM42605_RA_INT_CONFIG0 0x63
+#define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR ((0 << 5) || (0 << 4))
+#define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (0 << 4)) // duplicate settings in datasheet, Rev 1.2.
+#define ICM42605_UI_DRDY_INT_CLEAR_ON_F1BR ((1 << 5) || (0 << 4))
+#define ICM42605_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR ((1 << 5) || (1 << 4))
+
+#define ICM42605_RA_INT_CONFIG1 0x64
+#define ICM42605_INT_ASYNC_RESET_BIT 4
+#define ICM42605_INT_TDEASSERT_DISABLE_BIT 5
+#define ICM42605_INT_TDEASSERT_ENABLED (0 << ICM42605_INT_TDEASSERT_DISABLE_BIT)
+#define ICM42605_INT_TDEASSERT_DISABLED (1 << ICM42605_INT_TDEASSERT_DISABLE_BIT)
+#define ICM42605_INT_TPULSE_DURATION_BIT 6
+#define ICM42605_INT_TPULSE_DURATION_100 (0 << ICM42605_INT_TPULSE_DURATION_BIT)
+#define ICM42605_INT_TPULSE_DURATION_8 (1 << ICM42605_INT_TPULSE_DURATION_BIT)
+
+
+#define ICM42605_RA_INT_SOURCE0 0x65
+#define ICM42605_UI_DRDY_INT1_EN_DISABLED (0 << 3)
+#define ICM42605_UI_DRDY_INT1_EN_ENABLED (1 << 3)
+
+static void icm42605SpiInit(const busDevice_t *bus)
+{
+ static bool hardwareInitialised = false;
+
+ if (hardwareInitialised) {
+ return;
+ }
+
+
+ spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
+
+ hardwareInitialised = true;
+}
+
+uint8_t icm42605SpiDetect(const busDevice_t *bus)
+{
+ icm42605SpiInit(bus);
+
+ spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION); //low speed
+
+ spiBusWriteRegister(bus, ICM42605_RA_PWR_MGMT0, 0x00);
+
+ uint8_t icmDetected = MPU_NONE;
+ uint8_t attemptsRemaining = 20;
+ do {
+ delay(150);
+ const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
+ switch (whoAmI) {
+ case ICM42605_WHO_AM_I_CONST:
+ icmDetected = ICM_42605_SPI;
+ break;
+ default:
+ icmDetected = MPU_NONE;
+ break;
+ }
+ if (icmDetected != MPU_NONE) {
+ break;
+ }
+ if (!attemptsRemaining) {
+ return MPU_NONE;
+ }
+ } while (attemptsRemaining--);
+
+ spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
+
+ return icmDetected;
+}
+
+void icm42605AccInit(accDev_t *acc)
+{
+ acc->acc_1G = 512 * 4;
+}
+
+bool icm42605AccRead(accDev_t *acc)
+{
+ uint8_t data[6];
+
+ const bool ack = busReadRegisterBuffer(&acc->bus, ICM42605_RA_ACCEL_DATA_X1, data, 6);
+ if (!ack) {
+ return false;
+ }
+
+ acc->ADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
+ acc->ADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
+ acc->ADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
+
+ return true;
+}
+bool icm42605SpiAccDetect(accDev_t *acc)
+{
+ switch (acc->mpuDetectionResult.sensor) {
+ case ICM_42605_SPI:
+ break;
+ default:
+ return false;
+ }
+
+ acc->initFn = icm42605AccInit;
+ acc->readFn = icm42605AccRead;
+
+ return true;
+}
+
+typedef struct odrEntry_s {
+ uint8_t khz;
+ uint8_t odr; // See GYRO_ODR in datasheet.
+} odrEntry_t;
+
+odrEntry_t khzToSupportedODRMap[] = {
+ { 8, 3 },
+ { 4, 4 },
+ { 2, 5 },
+ { 1, 6 },
+};
+
+void icm42605GyroInit(gyroDev_t *gyro)
+{
+ mpuGyroInit(gyro);
+
+ spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_INITIALIZATION);
+
+ spiBusWriteRegister(&gyro->bus, ICM42605_RA_PWR_MGMT0, ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42605_PWR_MGMT0_ACCEL_MODE_LN | ICM42605_PWR_MGMT0_GYRO_MODE_LN);
+ delay(15);
+
+ uint8_t outputDataRate = 0;
+ bool supportedODRFound = false;
+
+ if (gyro->gyroRateKHz) {
+ uint8_t gyroSyncDenominator = gyro->mpuDividerDrops + 1; // rebuild it in here, see gyro_sync.c
+ uint8_t desiredODRKhz = 8 / gyroSyncDenominator;
+ for (uint32_t i = 0; i < ARRAYLEN(khzToSupportedODRMap); i++) {
+ if (khzToSupportedODRMap[i].khz == desiredODRKhz) {
+ outputDataRate = khzToSupportedODRMap[i].odr;
+ supportedODRFound = true;
+ break;
+ }
+ }
+ }
+
+ if (!supportedODRFound) {
+ outputDataRate = 6;
+ gyro->gyroRateKHz = GYRO_RATE_1_kHz;
+ }
+
+ uint8_t value;
+
+ STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
+ spiBusWriteRegister(&gyro->bus, ICM42605_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (outputDataRate & 0x0F));
+ delay(15);
+
+ value = spiBusReadRegister(&gyro->bus, ICM42605_RA_GYRO_CONFIG0);
+ debug[1] = value;
+
+ STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
+ spiBusWriteRegister(&gyro->bus, ICM42605_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F));
+ delay(15);
+
+ value = spiBusReadRegister(&gyro->bus, ICM42605_RA_ACCEL_CONFIG0);
+ debug[2] = value;
+
+ spiBusWriteRegister(&gyro->bus, ICM42605_RA_GYRO_ACCEL_CONFIG0, ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY);
+
+ spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH);
+ spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_CONFIG0, ICM42605_UI_DRDY_INT_CLEAR_ON_SBR);
+
+#ifdef USE_MPU_DATA_READY_SIGNAL
+ spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_SOURCE0, ICM42605_UI_DRDY_INT1_EN_ENABLED);
+
+ uint8_t intConfig1Value = spiBusReadRegister(&gyro->bus, ICM42605_RA_INT_CONFIG1);
+ // Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
+ intConfig1Value &= ~(1 << ICM42605_INT_ASYNC_RESET_BIT);
+ intConfig1Value |= (ICM42605_INT_TPULSE_DURATION_8 | ICM42605_INT_TDEASSERT_DISABLED);
+
+ spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_CONFIG1, intConfig1Value);
+#endif
+ //
+
+ spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD);
+}
+
+bool icm42605GyroReadSPI(gyroDev_t *gyro)
+{
+ static const uint8_t dataToSend[7] = {ICM42605_RA_GYRO_DATA_X1 | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+ uint8_t data[7];
+
+ const bool ack = spiBusTransfer(&gyro->bus, dataToSend, data, 7);
+ if (!ack) {
+ return false;
+ }
+
+ gyro->gyroADCRaw[X] = (int16_t)((data[1] << 8) | data[2]);
+ gyro->gyroADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]);
+ gyro->gyroADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]);
+
+ return true;
+}
+
+bool icm42605SpiGyroDetect(gyroDev_t *gyro)
+{
+ switch (gyro->mpuDetectionResult.sensor) {
+ case ICM_42605_SPI:
+ break;
+ default:
+ return false;
+ }
+
+ gyro->initFn = icm42605GyroInit;
+ gyro->readFn = icm42605GyroReadSPI;
+
+ // 16.4 dps/lsb scalefactor
+ gyro->scale = 1.0f / 16.4f;
+
+ return true;
+}
diff --git a/src/main/drivers/accgyro/accgyro_spi_icm42605.h b/src/main/drivers/accgyro/accgyro_spi_icm42605.h
new file mode 100644
index 0000000000..ec302a4b48
--- /dev/null
+++ b/src/main/drivers/accgyro/accgyro_spi_icm42605.h
@@ -0,0 +1,34 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#include "drivers/bus.h"
+
+bool icm42605AccDetect(accDev_t *acc);
+bool icm42605GyroDetect(gyroDev_t *gyro);
+
+void icm42605AccInit(accDev_t *acc);
+void icm42605GyroInit(gyroDev_t *gyro);
+
+uint8_t icm42605SpiDetect(const busDevice_t *bus);
+
+bool icm42605SpiAccDetect(accDev_t *acc);
+bool icm42605SpiGyroDetect(gyroDev_t *gyro);
diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c
index 25e84822d4..1664255d53 100644
--- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c
+++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c
@@ -69,6 +69,9 @@ uint8_t mpu6500SpiDetect(const busDevice_t *bus)
case ICM20608G_WHO_AM_I_CONST:
mpuDetected = ICM_20608_SPI;
break;
+ case ICM42605_WHO_AM_I_CONST:
+ mpuDetected = ICM_42605_SPI;
+ break;
default:
mpuDetected = MPU_NONE;
}
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 0dce6835a4..a6270b7803 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -45,6 +45,7 @@
#include "drivers/accgyro/accgyro_spi_bmi160.h"
#include "drivers/accgyro/accgyro_spi_icm20649.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
+#include "drivers/accgyro/accgyro_spi_icm42605.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
@@ -273,6 +274,15 @@ retry:
FALLTHROUGH;
#endif
+#ifdef USE_ACC_SPI_ICM42605
+ case ACC_ICM42605:
+ if (icm42605SpiAccDetect(dev)) {
+ accHardware = ACC_ICM42605;
+ break;
+ }
+ FALLTHROUGH;
+#endif
+
#ifdef USE_ACCGYRO_BMI160
case ACC_BMI160:
if (bmi160SpiAccDetect(dev)) {
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index a0218fb285..cff3caf16d 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -42,6 +42,7 @@ typedef enum {
ACC_ICM20608G,
ACC_ICM20649,
ACC_ICM20689,
+ ACC_ICM42605,
ACC_BMI160,
ACC_FAKE
} accelerationSensor_e;
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index e3b13e0c42..9e5291fc5a 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -47,6 +47,8 @@
#include "drivers/accgyro/accgyro_spi_bmi160.h"
#include "drivers/accgyro/accgyro_spi_icm20649.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
+#include "drivers/accgyro/accgyro_spi_icm20689.h"
+#include "drivers/accgyro/accgyro_spi_icm42605.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
#include "drivers/accgyro/accgyro_spi_mpu9250.h"
@@ -339,6 +341,15 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
FALLTHROUGH;
#endif
+#ifdef USE_GYRO_SPI_ICM42605
+ case GYRO_ICM42605:
+ if (icm42605SpiGyroDetect(dev)) {
+ gyroHardware = GYRO_ICM42605;
+ break;
+ }
+ FALLTHROUGH;
+#endif
+
#ifdef USE_ACCGYRO_BMI160
case GYRO_BMI160:
if (bmi160SpiGyroDetect(dev)) {