From 46c62cfcf2524c771629e66d063fde21fc097ffe Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 21 Apr 2021 17:50:03 +0200 Subject: [PATCH] ICM42688P - Initial sensor support based on ICM42605 driver. --- src/main/cli/settings.c | 4 +- src/main/drivers/accgyro/accgyro.h | 1 + src/main/drivers/accgyro/accgyro_mpu.c | 4 + src/main/drivers/accgyro/accgyro_mpu.h | 2 + .../drivers/accgyro/accgyro_spi_icm42688p.c | 283 ++++++++++++++++++ .../drivers/accgyro/accgyro_spi_icm42688p.h | 34 +++ .../drivers/accgyro/accgyro_spi_mpu6500.c | 3 + src/main/sensors/acceleration.h | 1 + src/main/sensors/acceleration_init.c | 10 + src/main/sensors/gyro_init.c | 12 +- src/main/target/common_post.h | 2 +- 11 files changed, 352 insertions(+), 4 deletions(-) create mode 100644 src/main/drivers/accgyro/accgyro_spi_icm42688p.c create mode 100644 src/main/drivers/accgyro/accgyro_spi_icm42688p.h diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 8d2675d19..1f3986814 100644 --- a/src/main/cli/settings.c +++ b/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" }; diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 919365052..e4c30d27d 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 c23bb0b89..303a573d9 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/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 diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index e2c1ce812..d9296706d 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_icm42688p.c b/src/main/drivers/accgyro/accgyro_spi_icm42688p.c new file mode 100644 index 000000000..df13774d5 --- /dev/null +++ b/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 . + */ + +/* + * Author: Dominic Clifton + */ + +#include +#include +#include + +#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 diff --git a/src/main/drivers/accgyro/accgyro_spi_icm42688p.h b/src/main/drivers/accgyro/accgyro_spi_icm42688p.h new file mode 100644 index 000000000..6801de620 --- /dev/null +++ b/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 . + */ + +#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); diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c index 051cdbfa2..573b9f1db 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 3b97237d3..d82893046 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 81a8c6e91..4cc979f48 100644 --- a/src/main/sensors/acceleration_init.c +++ b/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)) { diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index f0baa5c70..c0dccd3df 100644 --- a/src/main/sensors/gyro_init.c +++ b/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); diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index e17b732cd..815a1560b 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