diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index ec4fa12176..e61768a830 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -132,14 +132,14 @@
// sync with accelerationSensor_e
const char * const lookupTableAccHardware[] = {
"AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC",
- "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605",
+ "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P",
"BMI160", "BMI270", "LSM6DSO", "FAKE"
};
// sync with gyroHardware_e
const char * const lookupTableGyroHardware[] = {
"AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20",
- "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605",
+ "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", "ICM42605", "ICM42688P",
"BMI160", "BMI270", "LSM6SDO", "FAKE"
};
diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h
index 9193650526..e4c30d27da 100644
--- a/src/main/drivers/accgyro/accgyro.h
+++ b/src/main/drivers/accgyro/accgyro.h
@@ -54,6 +54,7 @@ typedef enum {
GYRO_ICM20649,
GYRO_ICM20689,
GYRO_ICM42605,
+ GYRO_ICM42688P,
GYRO_BMI160,
GYRO_BMI270,
GYRO_LSM6DSO,
diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c
index c23bb0b895..aa12522020 100644
--- a/src/main/drivers/accgyro/accgyro_mpu.c
+++ b/src/main/drivers/accgyro/accgyro_mpu.c
@@ -50,7 +50,7 @@
#include "drivers/accgyro/accgyro_spi_bmi270.h"
#include "drivers/accgyro/accgyro_spi_icm20649.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
-#include "drivers/accgyro/accgyro_spi_icm42605.h"
+#include "drivers/accgyro/accgyro_spi_icm426xx.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
@@ -236,8 +236,8 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
#ifdef USE_ACCGYRO_BMI270
bmi270Detect,
#endif
-#ifdef USE_GYRO_SPI_ICM42605
- icm42605SpiDetect,
+#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
+ icm426xxSpiDetect,
#endif
#ifdef USE_GYRO_SPI_ICM20649
icm20649SpiDetect,
diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h
index e2c1ce8125..f296ad541e 100644
--- a/src/main/drivers/accgyro/accgyro_mpu.h
+++ b/src/main/drivers/accgyro/accgyro_mpu.h
@@ -46,6 +46,7 @@
#define ICM20649_WHO_AM_I_CONST (0xE1)
#define ICM20689_WHO_AM_I_CONST (0x98)
#define ICM42605_WHO_AM_I_CONST (0x42)
+#define ICM42688P_WHO_AM_I_CONST (0x47)
// RA = Register Address
@@ -200,6 +201,7 @@ typedef enum {
ICM_20649_SPI,
ICM_20689_SPI,
ICM_42605_SPI,
+ ICM_42688P_SPI,
BMI_160_SPI,
BMI_270_SPI,
LSM6DSO_SPI,
diff --git a/src/main/drivers/accgyro/accgyro_spi_icm42605.c b/src/main/drivers/accgyro/accgyro_spi_icm42605.c
deleted file mode 100644
index 31a7a9a045..0000000000
--- a/src/main/drivers/accgyro/accgyro_spi_icm42605.c
+++ /dev/null
@@ -1,283 +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 .
- */
-
-/*
- * Author: Dominic Clifton
- */
-
-#include
-#include
-#include
-
-#include "platform.h"
-
-#ifdef USE_GYRO_SPI_ICM42605
-
-#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"
-
-// 24 MHz max SPI frequency
-#define ICM42605_MAX_SPI_CLK_HZ 24000000
-
-// 10 MHz max SPI frequency for intialisation
-#define ICM42605_MAX_SPI_INIT_CLK_HZ 1000000
-
-#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 extDevice_t *dev)
-{
- static bool hardwareInitialised = false;
-
- if (hardwareInitialised) {
- return;
- }
-
-
- spiSetClkDivisor(dev, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ));
-
- hardwareInitialised = true;
-}
-
-uint8_t icm42605SpiDetect(const extDevice_t *dev)
-{
- icm42605SpiInit(dev);
-
- spiSetClkDivisor(dev, spiCalculateDivider(ICM42605_MAX_SPI_INIT_CLK_HZ));
-
- spiWriteReg(dev, ICM42605_RA_PWR_MGMT0, 0x00);
-
- uint8_t icmDetected = MPU_NONE;
- uint8_t attemptsRemaining = 20;
- do {
- delay(150);
- const uint8_t whoAmI = spiReadRegMsk(dev, 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--);
-
- spiSetClkDivisor(dev, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ));
-
- 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->gyro->dev, 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;
-
-static odrEntry_t icm42605PkhzToSupportedODRMap[] = {
- { 8, 3 },
- { 4, 4 },
- { 2, 5 },
- { 1, 6 },
-};
-
-void icm42605GyroInit(gyroDev_t *gyro)
-{
- mpuGyroInit(gyro);
-
- spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42605_MAX_SPI_INIT_CLK_HZ));
-
- spiWriteReg(&gyro->dev, 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(icm42605PkhzToSupportedODRMap); i++) {
- if (icm42605PkhzToSupportedODRMap[i].khz == desiredODRKhz) {
- outputDataRate = icm42605PkhzToSupportedODRMap[i].odr;
- supportedODRFound = true;
- break;
- }
- }
- }
-
- if (!supportedODRFound) {
- outputDataRate = 6;
- gyro->gyroRateKHz = GYRO_RATE_1_kHz;
- }
-
- STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
- spiWriteReg(&gyro->dev, ICM42605_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (outputDataRate & 0x0F));
- delay(15);
-
- STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
- spiWriteReg(&gyro->dev, ICM42605_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F));
- delay(15);
-
- spiWriteReg(&gyro->dev, ICM42605_RA_GYRO_ACCEL_CONFIG0, ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY);
-
- spiWriteReg(&gyro->dev, ICM42605_RA_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH);
- spiWriteReg(&gyro->dev, ICM42605_RA_INT_CONFIG0, ICM42605_UI_DRDY_INT_CLEAR_ON_SBR);
-
-#ifdef USE_MPU_DATA_READY_SIGNAL
- spiWriteReg(&gyro->dev, ICM42605_RA_INT_SOURCE0, ICM42605_UI_DRDY_INT1_EN_ENABLED);
-
- uint8_t intConfig1Value = spiReadRegMsk(&gyro->dev, 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);
-
- spiWriteReg(&gyro->dev, ICM42605_RA_INT_CONFIG1, intConfig1Value);
-#endif
- //
-
- spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ));
-}
-
-bool icm42605GyroReadSPI(gyroDev_t *gyro)
-{
- STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {ICM42605_RA_GYRO_DATA_X1 | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
- STATIC_DMA_DATA_AUTO uint8_t data[7];
-
- const bool ack = spiReadWriteBufRB(&gyro->dev, 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;
-
- gyro->scale = GYRO_SCALE_2000DPS;
-
- return true;
-}
-#endif
diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c
new file mode 100644
index 0000000000..1cbb7ee2ff
--- /dev/null
+++ b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c
@@ -0,0 +1,290 @@
+/*
+ * 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"
+
+#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
+
+#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_icm426xx.h"
+#include "drivers/bus_spi.h"
+#include "drivers/exti.h"
+#include "drivers/io.h"
+#include "drivers/sensor.h"
+#include "drivers/time.h"
+
+// 24 MHz max SPI frequency
+#define ICM426XX_MAX_SPI_CLK_HZ 24000000
+
+// 10 MHz max SPI frequency for intialisation
+#define ICM426XX_MAX_SPI_INIT_CLK_HZ 1000000
+
+#define ICM426XX_RA_PWR_MGMT0 0x4E
+
+#define ICM426XX_PWR_MGMT0_ACCEL_MODE_LN (3 << 0)
+#define ICM426XX_PWR_MGMT0_GYRO_MODE_LN (3 << 2)
+#define ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF (0 << 5)
+#define ICM426XX_PWR_MGMT0_TEMP_DISABLE_ON (1 << 5)
+
+#define ICM426XX_RA_GYRO_CONFIG0 0x4F
+#define ICM426XX_RA_ACCEL_CONFIG0 0x50
+
+#define ICM426XX_RA_GYRO_ACCEL_CONFIG0 0x52
+
+#define ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY (14 << 4)
+#define ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY (14 << 0)
+
+#define ICM426XX_RA_GYRO_DATA_X1 0x25
+#define ICM426XX_RA_ACCEL_DATA_X1 0x1F
+
+#define ICM426XX_RA_INT_CONFIG 0x14
+#define ICM426XX_INT1_MODE_PULSED (0 << 2)
+#define ICM426XX_INT1_MODE_LATCHED (1 << 2)
+#define ICM426XX_INT1_DRIVE_CIRCUIT_OD (0 << 1)
+#define ICM426XX_INT1_DRIVE_CIRCUIT_PP (1 << 1)
+#define ICM426XX_INT1_POLARITY_ACTIVE_LOW (0 << 0)
+#define ICM426XX_INT1_POLARITY_ACTIVE_HIGH (1 << 0)
+
+#define ICM426XX_RA_INT_CONFIG0 0x63
+#define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR ((0 << 5) || (0 << 4))
+#define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (0 << 4)) // duplicate settings in datasheet, Rev 1.2.
+#define ICM426XX_UI_DRDY_INT_CLEAR_ON_F1BR ((1 << 5) || (0 << 4))
+#define ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR ((1 << 5) || (1 << 4))
+
+#define ICM426XX_RA_INT_CONFIG1 0x64
+#define ICM426XX_INT_ASYNC_RESET_BIT 4
+#define ICM426XX_INT_TDEASSERT_DISABLE_BIT 5
+#define ICM426XX_INT_TDEASSERT_ENABLED (0 << ICM426XX_INT_TDEASSERT_DISABLE_BIT)
+#define ICM426XX_INT_TDEASSERT_DISABLED (1 << ICM426XX_INT_TDEASSERT_DISABLE_BIT)
+#define ICM426XX_INT_TPULSE_DURATION_BIT 6
+#define ICM426XX_INT_TPULSE_DURATION_100 (0 << ICM426XX_INT_TPULSE_DURATION_BIT)
+#define ICM426XX_INT_TPULSE_DURATION_8 (1 << ICM426XX_INT_TPULSE_DURATION_BIT)
+
+
+#define ICM426XX_RA_INT_SOURCE0 0x65
+#define ICM426XX_UI_DRDY_INT1_EN_DISABLED (0 << 3)
+#define ICM426XX_UI_DRDY_INT1_EN_ENABLED (1 << 3)
+
+static void icm426xxSpiInit(const extDevice_t *dev)
+{
+ static bool hardwareInitialised = false;
+
+ if (hardwareInitialised) {
+ return;
+ }
+
+
+ spiSetClkDivisor(dev, spiCalculateDivider(ICM426XX_MAX_SPI_CLK_HZ));
+
+ hardwareInitialised = true;
+}
+
+uint8_t icm426xxSpiDetect(const extDevice_t *dev)
+{
+ icm426xxSpiInit(dev);
+
+ spiSetClkDivisor(dev, spiCalculateDivider(ICM426XX_MAX_SPI_INIT_CLK_HZ));
+
+ spiWriteReg(dev, ICM426XX_RA_PWR_MGMT0, 0x00);
+
+ uint8_t icmDetected = MPU_NONE;
+ uint8_t attemptsRemaining = 20;
+ do {
+ delay(150);
+ const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
+ switch (whoAmI) {
+ case ICM42605_WHO_AM_I_CONST:
+ icmDetected = ICM_42605_SPI;
+ break;
+ case ICM42688P_WHO_AM_I_CONST:
+ icmDetected = ICM_42688P_SPI;
+ break;
+ default:
+ icmDetected = MPU_NONE;
+ break;
+ }
+ if (icmDetected != MPU_NONE) {
+ break;
+ }
+ if (!attemptsRemaining) {
+ return MPU_NONE;
+ }
+ } while (attemptsRemaining--);
+
+ spiSetClkDivisor(dev, spiCalculateDivider(ICM426XX_MAX_SPI_CLK_HZ));
+
+ return icmDetected;
+}
+
+void icm426xxAccInit(accDev_t *acc)
+{
+ acc->acc_1G = 512 * 4;
+}
+
+bool icm426xxAccRead(accDev_t *acc)
+{
+ uint8_t data[6];
+
+ const bool ack = busReadRegisterBuffer(&acc->gyro->dev, ICM426XX_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 icm426xxSpiAccDetect(accDev_t *acc)
+{
+ switch (acc->mpuDetectionResult.sensor) {
+ case ICM_42605_SPI:
+ break;
+ case ICM_42688P_SPI:
+ break;
+ default:
+ return false;
+ }
+
+ acc->initFn = icm426xxAccInit;
+ acc->readFn = icm426xxAccRead;
+
+ return true;
+}
+
+typedef struct odrEntry_s {
+ uint8_t khz;
+ uint8_t odr; // See GYRO_ODR in datasheet.
+} odrEntry_t;
+
+static odrEntry_t icm426xxPkhzToSupportedODRMap[] = {
+ { 8, 3 },
+ { 4, 4 },
+ { 2, 5 },
+ { 1, 6 },
+};
+
+void icm426xxGyroInit(gyroDev_t *gyro)
+{
+ mpuGyroInit(gyro);
+
+ spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM426XX_MAX_SPI_INIT_CLK_HZ));
+
+ spiWriteReg(&gyro->dev, ICM426XX_RA_PWR_MGMT0, ICM426XX_PWR_MGMT0_TEMP_DISABLE_OFF | ICM426XX_PWR_MGMT0_ACCEL_MODE_LN | ICM426XX_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(icm426xxPkhzToSupportedODRMap); i++) {
+ if (icm426xxPkhzToSupportedODRMap[i].khz == desiredODRKhz) {
+ outputDataRate = icm426xxPkhzToSupportedODRMap[i].odr;
+ supportedODRFound = true;
+ break;
+ }
+ }
+ }
+
+ if (!supportedODRFound) {
+ outputDataRate = 6;
+ gyro->gyroRateKHz = GYRO_RATE_1_kHz;
+ }
+
+ STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
+ spiWriteReg(&gyro->dev, ICM426XX_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (outputDataRate & 0x0F));
+ delay(15);
+
+ STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
+ spiWriteReg(&gyro->dev, ICM426XX_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F));
+ delay(15);
+
+ spiWriteReg(&gyro->dev, ICM426XX_RA_GYRO_ACCEL_CONFIG0, ICM426XX_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM426XX_GYRO_UI_FILT_BW_LOW_LATENCY);
+
+ spiWriteReg(&gyro->dev, ICM426XX_RA_INT_CONFIG, ICM426XX_INT1_MODE_PULSED | ICM426XX_INT1_DRIVE_CIRCUIT_PP | ICM426XX_INT1_POLARITY_ACTIVE_HIGH);
+ spiWriteReg(&gyro->dev, ICM426XX_RA_INT_CONFIG0, ICM426XX_UI_DRDY_INT_CLEAR_ON_SBR);
+
+#ifdef USE_MPU_DATA_READY_SIGNAL
+ spiWriteReg(&gyro->dev, ICM426XX_RA_INT_SOURCE0, ICM426XX_UI_DRDY_INT1_EN_ENABLED);
+
+ uint8_t intConfig1Value = spiReadRegMsk(&gyro->dev, ICM426XX_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 << ICM426XX_INT_ASYNC_RESET_BIT);
+ intConfig1Value |= (ICM426XX_INT_TPULSE_DURATION_8 | ICM426XX_INT_TDEASSERT_DISABLED);
+
+ spiWriteReg(&gyro->dev, ICM426XX_RA_INT_CONFIG1, intConfig1Value);
+#endif
+ //
+
+ spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM426XX_MAX_SPI_CLK_HZ));
+}
+
+bool icm426xxGyroReadSPI(gyroDev_t *gyro)
+{
+ STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {ICM426XX_RA_GYRO_DATA_X1 | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+ STATIC_DMA_DATA_AUTO uint8_t data[7];
+
+ const bool ack = spiReadWriteBufRB(&gyro->dev, 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 icm426xxSpiGyroDetect(gyroDev_t *gyro)
+{
+ switch (gyro->mpuDetectionResult.sensor) {
+ case ICM_42605_SPI:
+ break;
+ case ICM_42688P_SPI:
+ break;
+ default:
+ return false;
+ }
+
+ gyro->initFn = icm426xxGyroInit;
+ gyro->readFn = icm426xxGyroReadSPI;
+
+ gyro->scale = GYRO_SCALE_2000DPS;
+
+ return true;
+}
+#endif
diff --git a/src/main/drivers/accgyro/accgyro_spi_icm42605.h b/src/main/drivers/accgyro/accgyro_spi_icm426xx.h
similarity index 73%
rename from src/main/drivers/accgyro/accgyro_spi_icm42605.h
rename to src/main/drivers/accgyro/accgyro_spi_icm426xx.h
index 56a8da612d..e72e04e072 100644
--- a/src/main/drivers/accgyro/accgyro_spi_icm42605.h
+++ b/src/main/drivers/accgyro/accgyro_spi_icm426xx.h
@@ -22,13 +22,13 @@
#include "drivers/bus.h"
-bool icm42605AccDetect(accDev_t *acc);
-bool icm42605GyroDetect(gyroDev_t *gyro);
+bool icm426xxAccDetect(accDev_t *acc);
+bool icm426xxGyroDetect(gyroDev_t *gyro);
-void icm42605AccInit(accDev_t *acc);
-void icm42605GyroInit(gyroDev_t *gyro);
+void icm426xxAccInit(accDev_t *acc);
+void icm426xxGyroInit(gyroDev_t *gyro);
-uint8_t icm42605SpiDetect(const extDevice_t *dev);
+uint8_t icm426xxSpiDetect(const extDevice_t *dev);
-bool icm42605SpiAccDetect(accDev_t *acc);
-bool icm42605SpiGyroDetect(gyroDev_t *gyro);
+bool icm426xxSpiAccDetect(accDev_t *acc);
+bool icm426xxSpiGyroDetect(gyroDev_t *gyro);
diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c
index 051cdbfa21..573b9f1db3 100644
--- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c
+++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c
@@ -77,6 +77,9 @@ uint8_t mpu6500SpiDetect(const extDevice_t *dev)
case ICM42605_WHO_AM_I_CONST:
mpuDetected = ICM_42605_SPI;
break;
+ case ICM42688P_WHO_AM_I_CONST:
+ mpuDetected = ICM_42688P_SPI;
+ break;
default:
mpuDetected = MPU_NONE;
}
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index 3b97237d3c..d82893046a 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -43,6 +43,7 @@ typedef enum {
ACC_ICM20649,
ACC_ICM20689,
ACC_ICM42605,
+ ACC_ICM42688P,
ACC_BMI160,
ACC_BMI270,
ACC_LSM6DSO,
diff --git a/src/main/sensors/acceleration_init.c b/src/main/sensors/acceleration_init.c
index 81a8c6e91b..2687a78d86 100644
--- a/src/main/sensors/acceleration_init.c
+++ b/src/main/sensors/acceleration_init.c
@@ -46,7 +46,7 @@
#include "drivers/accgyro/accgyro_spi_bmi270.h"
#include "drivers/accgyro/accgyro_spi_icm20649.h"
#include "drivers/accgyro/accgyro_spi_icm20689.h"
-#include "drivers/accgyro/accgyro_spi_icm42605.h"
+#include "drivers/accgyro/accgyro_spi_icm426xx.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
@@ -268,10 +268,21 @@ retry:
FALLTHROUGH;
#endif
-#ifdef USE_ACC_SPI_ICM42605
+#if defined(USE_ACC_SPI_ICM42605) || defined(USE_ACC_SPI_ICM42688P)
case ACC_ICM42605:
- if (icm42605SpiAccDetect(dev)) {
- accHardware = ACC_ICM42605;
+ case ACC_ICM42688P:
+ if (icm426xxSpiAccDetect(dev)) {
+ switch (dev->mpuDetectionResult.sensor) {
+ case ICM_42605_SPI:
+ accHardware = ACC_ICM42605;
+ break;
+ case ICM_42688P_SPI:
+ accHardware = ACC_ICM42688P;
+ break;
+ default:
+ accHardware = ACC_NONE;
+ break;
+ }
break;
}
FALLTHROUGH;
diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c
index fc7f5b694f..df9073f3da 100644
--- a/src/main/sensors/gyro_init.c
+++ b/src/main/sensors/gyro_init.c
@@ -45,7 +45,7 @@
#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_icm426xx.h"
#include "drivers/accgyro/accgyro_spi_lsm6dso.h"
#include "drivers/accgyro/accgyro_spi_mpu6000.h"
#include "drivers/accgyro/accgyro_spi_mpu6500.h"
@@ -439,10 +439,21 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
FALLTHROUGH;
#endif
-#ifdef USE_GYRO_SPI_ICM42605
+#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
case GYRO_ICM42605:
- if (icm42605SpiGyroDetect(dev)) {
- gyroHardware = GYRO_ICM42605;
+ case GYRO_ICM42688P:
+ if (icm426xxSpiGyroDetect(dev)) {
+ switch (dev->mpuDetectionResult.sensor) {
+ case ICM_42605_SPI:
+ gyroHardware = GYRO_ICM42605;
+ break;
+ case ICM_42688P_SPI:
+ gyroHardware = GYRO_ICM42688P;
+ break;
+ default:
+ gyroHardware = GYRO_NONE;
+ break;
+ }
break;
}
FALLTHROUGH;
@@ -500,7 +511,7 @@ static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t
{
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
|| defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
- || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_GYRO_SPI_ICM42605)
+ || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config);
diff --git a/src/main/target/NUCLEOH723ZG/target.h b/src/main/target/NUCLEOH723ZG/target.h
index 1d340a8e08..20decd198e 100644
--- a/src/main/target/NUCLEOH723ZG/target.h
+++ b/src/main/target/NUCLEOH723ZG/target.h
@@ -202,6 +202,8 @@
#define USE_ACC_SPI_MPU9250
#define USE_GYRO_SPI_ICM42605
#define USE_ACC_SPI_ICM42605
+#define USE_GYRO_SPI_ICM42688P
+#define USE_ACC_SPI_ICM42688P
#define GYRO_1_CS_PIN PD15
#define GYRO_1_SPI_INSTANCE SPI1
diff --git a/src/main/target/NUCLEOH723ZG/target.mk b/src/main/target/NUCLEOH723ZG/target.mk
index 5f22c29e85..a61ea8fb10 100644
--- a/src/main/target/NUCLEOH723ZG/target.mk
+++ b/src/main/target/NUCLEOH723ZG/target.mk
@@ -22,7 +22,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_spi_mpu9250.c \
- drivers/accgyro/accgyro_spi_icm42605.c \
+ drivers/accgyro/accgyro_spi_icm426xx.c \
drivers/accgyro/accgyro_mpu6050.c \
drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \
diff --git a/src/main/target/NUCLEOH725ZG/target.h b/src/main/target/NUCLEOH725ZG/target.h
index ef6d6f9581..796261dadc 100644
--- a/src/main/target/NUCLEOH725ZG/target.h
+++ b/src/main/target/NUCLEOH725ZG/target.h
@@ -211,6 +211,8 @@
#define USE_ACC_SPI_MPU9250
#define USE_GYRO_SPI_ICM42605
#define USE_ACC_SPI_ICM42605
+#define USE_GYRO_SPI_ICM42688P
+#define USE_ACC_SPI_ICM42688P
#define GYRO_1_CS_PIN PD15
#define GYRO_1_SPI_INSTANCE SPI1
diff --git a/src/main/target/NUCLEOH725ZG/target.mk b/src/main/target/NUCLEOH725ZG/target.mk
index 82a2350c06..ce6bd819de 100644
--- a/src/main/target/NUCLEOH725ZG/target.mk
+++ b/src/main/target/NUCLEOH725ZG/target.mk
@@ -22,7 +22,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_spi_mpu9250.c \
- drivers/accgyro/accgyro_spi_icm42605.c \
+ drivers/accgyro/accgyro_spi_icm426xx.c \
drivers/accgyro/accgyro_mpu6050.c \
drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \
diff --git a/src/main/target/NUCLEOH743/target.h b/src/main/target/NUCLEOH743/target.h
index 0d421ee61c..f0c5b27e9f 100644
--- a/src/main/target/NUCLEOH743/target.h
+++ b/src/main/target/NUCLEOH743/target.h
@@ -211,6 +211,8 @@
#define USE_ACC_SPI_MPU9250
#define USE_GYRO_SPI_ICM42605
#define USE_ACC_SPI_ICM42605
+#define USE_GYRO_SPI_ICM42688P
+#define USE_ACC_SPI_ICM42688P
#define GYRO_1_CS_PIN PD15
#define GYRO_1_SPI_INSTANCE SPI1
diff --git a/src/main/target/NUCLEOH743/target.mk b/src/main/target/NUCLEOH743/target.mk
index efcf18b414..dfe14af61f 100644
--- a/src/main/target/NUCLEOH743/target.mk
+++ b/src/main/target/NUCLEOH743/target.mk
@@ -25,7 +25,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_spi_mpu9250.c \
- drivers/accgyro/accgyro_spi_icm42605.c \
+ drivers/accgyro/accgyro_spi_icm426xx.c \
drivers/accgyro/accgyro_mpu6050.c \
drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \
diff --git a/src/main/target/NUCLEOH7A3ZI/target.h b/src/main/target/NUCLEOH7A3ZI/target.h
index 259a9f785c..0ccd67e6ad 100644
--- a/src/main/target/NUCLEOH7A3ZI/target.h
+++ b/src/main/target/NUCLEOH7A3ZI/target.h
@@ -202,6 +202,8 @@
#define USE_ACC_SPI_MPU9250
#define USE_GYRO_SPI_ICM42605
#define USE_ACC_SPI_ICM42605
+#define USE_GYRO_SPI_ICM42688P
+#define USE_ACC_SPI_ICM42688P
#define GYRO_1_CS_PIN PD15
#define GYRO_1_SPI_INSTANCE SPI1
diff --git a/src/main/target/NUCLEOH7A3ZI/target.mk b/src/main/target/NUCLEOH7A3ZI/target.mk
index c61d7f10d1..38a35f89ed 100644
--- a/src/main/target/NUCLEOH7A3ZI/target.mk
+++ b/src/main/target/NUCLEOH7A3ZI/target.mk
@@ -25,7 +25,7 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/accgyro/accgyro_spi_mpu6500.c \
drivers/accgyro/accgyro_spi_mpu9250.c \
- drivers/accgyro/accgyro_spi_icm42605.c \
+ drivers/accgyro/accgyro_spi_icm426xx.c \
drivers/accgyro/accgyro_mpu6050.c \
drivers/barometer/barometer_bmp085.c \
drivers/barometer/barometer_bmp280.c \
diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h
index 195dbc2036..fb38258ebb 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -244,7 +244,7 @@
#define USE_I2C_GYRO
#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)
+#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)
#define USE_SPI_GYRO
#endif