ICM42688P - Initial sensor support based on ICM42605 driver.

src/main/cli/settings.c

@@ -131,14 +131,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"
 };
 

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,

src/main/drivers/accgyro/accgyro_mpu.c

@@ -51,6 +51,7 @@
 #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_icm42688p.h"
 #include "drivers/accgyro/accgyro_spi_lsm6dso.h"
 #include "drivers/accgyro/accgyro_spi_mpu6000.h"
 #include "drivers/accgyro/accgyro_spi_mpu6500.h"
@@ -239,6 +240,9 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
 #ifdef USE_GYRO_SPI_ICM42605
     icm42605SpiDetect,
 #endif
+#ifdef USE_GYRO_SPI_ICM42688P
+    icm42688PSpiDetect,
+#endif
 #ifdef USE_GYRO_SPI_ICM20649
     icm20649SpiDetect,
 #endif

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,

src/main/drivers/accgyro/accgyro_spi_icm42688p.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 <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * Author: Dominic Clifton
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include "platform.h"
+
+#ifdef 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_icm42688P.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 ICM42688P_MAX_SPI_CLK_HZ 24000000
+
+// 10 MHz max SPI frequency for intialisation
+#define ICM42688P_MAX_SPI_INIT_CLK_HZ 1000000
+
+#define ICM42688P_RA_PWR_MGMT0                       0x4E
+
+#define ICM42688P_PWR_MGMT0_ACCEL_MODE_LN            (3 << 0)
+#define ICM42688P_PWR_MGMT0_GYRO_MODE_LN             (3 << 2)
+#define ICM42688P_PWR_MGMT0_TEMP_DISABLE_OFF         (0 << 5)
+#define ICM42688P_PWR_MGMT0_TEMP_DISABLE_ON          (1 << 5)
+
+#define ICM42688P_RA_GYRO_CONFIG0                    0x4F
+#define ICM42688P_RA_ACCEL_CONFIG0                   0x50
+
+#define ICM42688P_RA_GYRO_ACCEL_CONFIG0              0x52
+
+#define ICM42688P_ACCEL_UI_FILT_BW_LOW_LATENCY       (14 << 4)
+#define ICM42688P_GYRO_UI_FILT_BW_LOW_LATENCY        (14 << 0)
+
+#define ICM42688P_RA_GYRO_DATA_X1                    0x25
+#define ICM42688P_RA_ACCEL_DATA_X1                   0x1F
+
+#define ICM42688P_RA_INT_CONFIG                      0x14
+#define ICM42688P_INT1_MODE_PULSED                   (0 << 2)
+#define ICM42688P_INT1_MODE_LATCHED                  (1 << 2)
+#define ICM42688P_INT1_DRIVE_CIRCUIT_OD              (0 << 1)
+#define ICM42688P_INT1_DRIVE_CIRCUIT_PP              (1 << 1)
+#define ICM42688P_INT1_POLARITY_ACTIVE_LOW           (0 << 0)
+#define ICM42688P_INT1_POLARITY_ACTIVE_HIGH          (1 << 0)
+
+#define ICM42688P_RA_INT_CONFIG0                     0x63
+#define ICM42688P_UI_DRDY_INT_CLEAR_ON_SBR           ((0 << 5) || (0 << 4))
+#define ICM42688P_UI_DRDY_INT_CLEAR_ON_SBR_DUPLICATE ((0 << 5) || (1 << 4)) // duplicate settings in datasheet, Rev 1.3.
+#define ICM42688P_UI_DRDY_INT_CLEAR_ON_F1BR          ((1 << 5) || (0 << 4))
+#define ICM42688P_UI_DRDY_INT_CLEAR_ON_SBR_AND_F1BR  ((1 << 5) || (1 << 4))
+
+#define ICM42688P_RA_INT_CONFIG1                     0x64
+#define ICM42688P_INT_ASYNC_RESET_BIT                4
+#define ICM42688P_INT_TDEASSERT_DISABLE_BIT          5
+#define ICM42688P_INT_TDEASSERT_ENABLED              (0 << ICM42688P_INT_TDEASSERT_DISABLE_BIT)
+#define ICM42688P_INT_TDEASSERT_DISABLED             (1 << ICM42688P_INT_TDEASSERT_DISABLE_BIT)
+#define ICM42688P_INT_TPULSE_DURATION_BIT            6
+#define ICM42688P_INT_TPULSE_DURATION_100            (0 << ICM42688P_INT_TPULSE_DURATION_BIT)
+#define ICM42688P_INT_TPULSE_DURATION_8              (1 << ICM42688P_INT_TPULSE_DURATION_BIT)
+
+
+#define ICM42688P_RA_INT_SOURCE0                     0x65
+#define ICM42688P_UI_DRDY_INT1_EN_DISABLED           (0 << 3)
+#define ICM42688P_UI_DRDY_INT1_EN_ENABLED            (1 << 3)
+
+static void icm42688PSpiInit(const extDevice_t *dev)
+{
+    static bool hardwareInitialised = false;
+
+    if (hardwareInitialised) {
+        return;
+    }
+
+
+    spiSetClkDivisor(dev, spiCalculateDivider(ICM42688P_MAX_SPI_CLK_HZ));
+
+    hardwareInitialised = true;
+}
+
+uint8_t icm42688PSpiDetect(const extDevice_t *dev)
+{
+    icm42688PSpiInit(dev);
+
+    spiSetClkDivisor(dev, spiCalculateDivider(ICM42688P_MAX_SPI_INIT_CLK_HZ));
+
+    spiWriteReg(dev, ICM42688P_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 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(ICM42688P_MAX_SPI_CLK_HZ));
+
+    return icmDetected;
+}
+
+void icm42688PAccInit(accDev_t *acc)
+{
+    acc->acc_1G = 512 * 4;
+}
+
+bool icm42688PAccRead(accDev_t *acc)
+{
+    uint8_t data[6];
+
+    const bool ack = busReadRegisterBuffer(&acc->gyro->dev, ICM42688P_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 icm42688PSpiAccDetect(accDev_t *acc)
+{
+    switch (acc->mpuDetectionResult.sensor) {
+    case ICM_42688P_SPI:
+        break;
+    default:
+        return false;
+    }
+
+    acc->initFn = icm42688PAccInit;
+    acc->readFn = icm42688PAccRead;
+
+    return true;
+}
+
+typedef struct odrEntry_s {
+    uint8_t khz;
+    uint8_t odr; // See GYRO_ODR in datasheet.
+} odrEntry_t;
+
+static odrEntry_t icm42688PkhzToSupportedODRMap[] = {
+    { 8, 3 },
+    { 4, 4 },
+    { 2, 5 },
+    { 1, 6 },
+};
+
+void icm42688PGyroInit(gyroDev_t *gyro)
+{
+    mpuGyroInit(gyro);
+
+    spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42688P_MAX_SPI_INIT_CLK_HZ));
+
+    spiWriteReg(&gyro->dev, ICM42688P_RA_PWR_MGMT0, ICM42688P_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42688P_PWR_MGMT0_ACCEL_MODE_LN | ICM42688P_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(icm42688PkhzToSupportedODRMap); i++) {
+            if (icm42688PkhzToSupportedODRMap[i].khz == desiredODRKhz) {
+                outputDataRate = icm42688PkhzToSupportedODRMap[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, ICM42688P_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, ICM42688P_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F));
+    delay(15);
+
+    spiWriteReg(&gyro->dev, ICM42688P_RA_GYRO_ACCEL_CONFIG0, ICM42688P_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42688P_GYRO_UI_FILT_BW_LOW_LATENCY);
+
+    spiWriteReg(&gyro->dev, ICM42688P_RA_INT_CONFIG, ICM42688P_INT1_MODE_PULSED | ICM42688P_INT1_DRIVE_CIRCUIT_PP | ICM42688P_INT1_POLARITY_ACTIVE_HIGH);
+    spiWriteReg(&gyro->dev, ICM42688P_RA_INT_CONFIG0, ICM42688P_UI_DRDY_INT_CLEAR_ON_SBR);
+
+#ifdef USE_MPU_DATA_READY_SIGNAL
+    spiWriteReg(&gyro->dev, ICM42688P_RA_INT_SOURCE0, ICM42688P_UI_DRDY_INT1_EN_ENABLED);
+
+    uint8_t intConfig1Value = spiReadRegMsk(&gyro->dev, ICM42688P_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 << ICM42688P_INT_ASYNC_RESET_BIT);
+    intConfig1Value |= (ICM42688P_INT_TPULSE_DURATION_8 | ICM42688P_INT_TDEASSERT_DISABLED);
+
+    spiWriteReg(&gyro->dev, ICM42688P_RA_INT_CONFIG1, intConfig1Value);
+#endif
+    //
+
+    spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42688P_MAX_SPI_CLK_HZ));
+}
+
+bool icm42688PGyroReadSPI(gyroDev_t *gyro)
+{
+    STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {ICM42688P_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 icm42688PSpiGyroDetect(gyroDev_t *gyro)
+{
+    switch (gyro->mpuDetectionResult.sensor) {
+    case ICM_42688P_SPI:
+        break;
+    default:
+        return false;
+    }
+
+    gyro->initFn = icm42688PGyroInit;
+    gyro->readFn = icm42688PGyroReadSPI;
+
+    gyro->scale = GYRO_SCALE_2000DPS;
+
+    return true;
+}
+#endif

src/main/drivers/accgyro/accgyro_spi_icm42688p.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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "drivers/bus.h"
+
+bool icm42688PAccDetect(accDev_t *acc);
+bool icm42688PGyroDetect(gyroDev_t *gyro);
+
+void icm42688PAccInit(accDev_t *acc);
+void icm42688PGyroInit(gyroDev_t *gyro);
+
+uint8_t icm42688PSpiDetect(const extDevice_t *dev);
+
+bool icm42688PSpiAccDetect(accDev_t *acc);
+bool icm42688PSpiGyroDetect(gyroDev_t *gyro);

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;
     }

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,

src/main/sensors/acceleration_init.c

@@ -47,6 +47,7 @@
 #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_icm42688p.h"
 #include "drivers/accgyro/accgyro_spi_lsm6dso.h"
 #include "drivers/accgyro/accgyro_spi_mpu6000.h"
 #include "drivers/accgyro/accgyro_spi_mpu6500.h"
@@ -277,6 +278,15 @@ retry:
         FALLTHROUGH;
 #endif
 
+#ifdef USE_ACC_SPI_ICM42688P
+    case ACC_ICM42688P:
+        if (icm42688PSpiAccDetect(dev)) {
+            accHardware = ACC_ICM42688P;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
 #ifdef USE_ACCGYRO_BMI160
     case ACC_BMI160:
         if (bmi160SpiAccDetect(dev)) {

src/main/sensors/gyro_init.c

@@ -46,6 +46,7 @@
 #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_icm42688p.h"
 #include "drivers/accgyro/accgyro_spi_lsm6dso.h"
 #include "drivers/accgyro/accgyro_spi_mpu6000.h"
 #include "drivers/accgyro/accgyro_spi_mpu6500.h"
@@ -475,6 +476,15 @@ STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
         FALLTHROUGH;
 #endif
 
+#ifdef USE_GYRO_SPI_ICM42688P
+    case GYRO_ICM42688P:
+        if (icm42688PSpiGyroDetect(dev)) {
+            gyroHardware = GYRO_ICM42688P;
+            break;
+        }
+        FALLTHROUGH;
+#endif
+
 #ifdef USE_ACCGYRO_BMI160
     case GYRO_BMI160:
         if (bmi160SpiGyroDetect(dev)) {
@@ -527,7 +537,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);
 

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