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