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