diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index c7a10665b..ed7e59451 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -44,6 +44,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #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_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" @@ -289,6 +290,22 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) } #endif +#ifdef USE_GYRO_SPI_ICM20649 +#ifdef ICM20649_SPI_INSTANCE + spiBusSetInstance(&gyro->bus, ICM20649_SPI_INSTANCE); +#endif +#ifdef ICM20649_CS_PIN + gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20649_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin; +#endif + sensor = icm20649SpiDetect(&gyro->bus); + if (sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = sensor; + gyro->mpuConfiguration.readFn = spiBusReadRegisterBuffer; + gyro->mpuConfiguration.writeFn = spiBusWriteRegister; + return true; + } +#endif + #ifdef USE_GYRO_SPI_ICM20689 #ifdef ICM20689_SPI_INSTANCE spiBusSetInstance(&gyro->bus, ICM20689_SPI_INSTANCE); diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 04d27c084..ed2f7ca97 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -23,7 +23,8 @@ //#define DEBUG_MPU_DATA_READY_INTERRUPT -#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) +#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20649) \ + || defined(USE_GYRO_SPI_ICM20689) #define GYRO_USES_SPI #endif @@ -40,6 +41,7 @@ #define ICM20601_WHO_AM_I_CONST (0xAC) #define ICM20602_WHO_AM_I_CONST (0x12) #define ICM20608G_WHO_AM_I_CONST (0xAF) +#define ICM20649_WHO_AM_I_CONST (0xE1) #define ICM20689_WHO_AM_I_CONST (0x98) @@ -189,6 +191,7 @@ typedef enum { ICM_20601_SPI, ICM_20602_SPI, ICM_20608_SPI, + ICM_20649_SPI, ICM_20689_SPI, BMI_160_SPI, } mpuSensor_e; diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.c b/src/main/drivers/accgyro/accgyro_spi_icm20649.c new file mode 100644 index 000000000..fb0030951 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.c @@ -0,0 +1,203 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it 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 is distributed in the hope that it 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 Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "common/axis.h" +#include "common/maths.h" + +#include "drivers/bus_spi.h" +#include "drivers/exti.h" +#include "drivers/gyro_sync.h" +#include "drivers/io.h" +#include "drivers/sensor.h" +#include "drivers/time.h" + +#include "accgyro.h" +#include "accgyro_mpu.h" +#include "accgyro_spi_icm20649.h" + +static bool use4kDps = true; // TODO: make these configurable for testing +static bool use30g = true; + +static void icm20649SpiInit(const busDevice_t *bus) +{ + static bool hardwareInitialised = false; + + if (hardwareInitialised) { + return; + } + + IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); + IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); + IOHi(bus->busdev_u.spi.csnPin); + + // all registers can be read/written at full speed (7MHz +-10%) + // TODO verify that this works at 9MHz and 10MHz on non F7 + spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD); + + hardwareInitialised = true; +} + +uint8_t icm20649SpiDetect(const busDevice_t *bus) +{ + icm20649SpiInit(bus); + + spiSetDivisor(bus->busdev_u.spi.instance, SPI_CLOCK_STANDARD); + + spiBusWriteRegister(bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe + delay(15); + + spiBusWriteRegister(bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET); + + uint8_t icmDetected = MPU_NONE; + uint8_t attemptsRemaining = 20; + do { + delay(150); + const uint8_t whoAmI = spiBusReadRegister(bus, ICM20649_RA_WHO_AM_I); + if (whoAmI == ICM20649_WHO_AM_I_CONST) { + icmDetected = ICM_20649_SPI; + } else { + icmDetected = MPU_NONE; + } + if (icmDetected != MPU_NONE) { + break; + } + if (!attemptsRemaining) { + return MPU_NONE; + } + } while (attemptsRemaining--); + + return icmDetected; + +} + +void icm20649AccInit(accDev_t *acc) +{ + // 2,048 LSB/g 16g + // 1,024 LSB/g 30g + acc->acc_1G = use30g ? 1024 : 2048; + + spiSetDivisor(acc->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); + + acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 + delay(15); + const uint8_t acc_fsr = use30g ? ICM20649_FSR_30G : ICM20649_FSR_16G; + acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1); + delay(15); + acc->mpuConfiguration.writeFn(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0 + delay(15); +} + +bool icm20649SpiAccDetect(accDev_t *acc) +{ + if (acc->mpuDetectionResult.sensor != ICM_20649_SPI) { + return false; + } + + acc->initFn = icm20649AccInit; + acc->readFn = icm20649AccRead; + + return true; +} + + +void icm20649GyroInit(gyroDev_t *gyro) +{ + mpuGyroInit(gyro); + + spiSetDivisor(gyro->bus.busdev_u.spi.instance, SPI_CLOCK_STANDARD); // ensure proper speed + + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe + delay(15); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET); + delay(100); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL); + delay(15); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2 + delay(15); + const uint8_t gyro_fsr = use4kDps ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS; + uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1_kHz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips) + raGyroConfigData |= gyro_fsr << 1 | gyro->lpf << 3; + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData); + delay(15); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops + delay(100); + + // Data ready interrupt configuration + // back to bank 0 + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); + delay(15); + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN + delay(15); + +#ifdef USE_MPU_DATA_READY_SIGNAL + gyro->mpuConfiguration.writeFn(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01); +#endif +} + +bool icm20649SpiGyroDetect(gyroDev_t *gyro) +{ + if (gyro->mpuDetectionResult.sensor != ICM_20649_SPI) + return false; + + gyro->initFn = icm20649GyroInit; + gyro->readFn = icm20649GyroReadSPI; + + // 16.4 dps/lsb 2kDps + // 8.2 dps/lsb 4kDps + gyro->scale = 1.0f / (use4kDps ? 8.2f : 16.4f); + + return true; +} + +bool icm20649GyroReadSPI(gyroDev_t *gyro) +{ + static const uint8_t dataToSend[7] = {ICM20649_RA_GYRO_XOUT_H | 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 icm20649AccRead(accDev_t *acc) +{ + uint8_t data[6]; + + const bool ack = acc->mpuConfiguration.readFn(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, 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; +} diff --git a/src/main/drivers/accgyro/accgyro_spi_icm20649.h b/src/main/drivers/accgyro/accgyro_spi_icm20649.h new file mode 100644 index 000000000..66739a8b0 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_icm20649.h @@ -0,0 +1,64 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it 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 is distributed in the hope that it 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 Cleanflight. If not, see . + */ +#pragma once + +#include "drivers/bus.h" + +#define ICM20649_BIT_RESET (0x80) + +#define ICM20649_RA_REG_BANK_SEL 0x7F + +// BANK 0 +#define ICM20649_RA_WHO_AM_I 0x00 +#define ICM20649_RA_PWR_MGMT_1 0x06 +#define ICM20649_RA_PWR_MGMT_2 0x07 +#define ICM20649_RA_INT_PIN_CFG 0x0F +#define ICM20649_RA_INT_ENABLE_1 0x11 +#define ICM20649_RA_GYRO_XOUT_H 0x33 +#define ICM20649_RA_ACCEL_XOUT_H 0x2D + +// BANK 2 +#define ICM20649_RA_GYRO_SMPLRT_DIV 0x00 +#define ICM20649_RA_GYRO_CONFIG_1 0x01 +#define ICM20649_RA_ACCEL_CONFIG 0x14 + +enum icm20649_gyro_fsr_e { + ICM20649_FSR_500DPS = 0, + ICM20649_FSR_1000DPS, + ICM20649_FSR_2000DPS, + ICM20649_FSR_4000DPS, + NUM_ICM20649_GYRO_FSR +}; + +enum icm20649_accel_fsr_e { + ICM20649_FSR_4G = 0, + ICM20649_FSR_8G, + ICM20649_FSR_16G, + ICM20649_FSR_30G, + NUM_ICM20649_ACCEL_FSR +}; + +void icm20649AccInit(accDev_t *acc); +void icm20649GyroInit(gyroDev_t *gyro); + +uint8_t icm20649SpiDetect(const busDevice_t *bus); + +bool icm20649SpiAccDetect(accDev_t *acc); +bool icm20649SpiGyroDetect(gyroDev_t *gyro); + +bool icm20649GyroReadSPI(gyroDev_t *gyro); +bool icm20649AccRead(accDev_t *acc); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 2b687885c..27c4c56c5 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -200,6 +200,9 @@ void spiPreInit(void) #ifdef USE_GYRO_SPI_MPU9250 spiPreInitCs(IO_TAG(MPU9250_CS_PIN)); #endif +#ifdef USE_GYRO_SPI_ICM20649 + spiPreInitCs(IO_TAG(ICM20649_CS_PIN)); +#endif #ifdef USE_GYRO_SPI_ICM20689 spiPreInitCs(IO_TAG(ICM20689_CS_PIN)); #endif diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e1329859c..282e03d4a 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -750,8 +750,10 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) if (acc.dev.acc_1G > 512*4) { scale = 8; - } else if (acc.dev.acc_1G >= 512) { + } else if (acc.dev.acc_1G > 512*2) { scale = 4; + } else if (acc.dev.acc_1G >= 512) { + scale = 2; } else { scale = 1; } diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 20b9132b6..2270864da 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -81,13 +81,15 @@ // sync with accelerationSensor_e const char * const lookupTableAccHardware[] = { "AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC", - "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608", "ICM20689", "BMI160", "FAKE" + "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608", "ICM20649", "ICM20689", + "BMI160", "FAKE" }; // sync with gyroSensor_e const char * const lookupTableGyroHardware[] = { "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", - "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20689", "BMI160", "FAKE" + "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", + "BMI160", "FAKE" }; #if defined(USE_SENSOR_NAMES) || defined(BARO) diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index d82d8d55e..2bad22a03 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -45,6 +45,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #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_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" @@ -243,6 +244,17 @@ retry: } break; } +#endif + ; // fallthrough + case ACC_ICM20649: +#ifdef USE_ACC_SPI_ICM20649 + if (icm20649SpiAccDetect(dev)) { + accHardware = ACC_ICM20649; +#ifdef ACC_ICM20649_ALIGN + dev->accAlign = ACC_ICM20649_ALIGN; +#endif + break; + } #endif ; // fallthrough case ACC_ICM20689: diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index b211abbb6..f583a841b 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -36,6 +36,7 @@ typedef enum { ACC_ICM20601, ACC_ICM20602, ACC_ICM20608G, + ACC_ICM20649, ACC_ICM20689, ACC_BMI160, ACC_FAKE diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 92871f3c7..aee591e44 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -44,6 +44,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #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_mpu6000.h" #include "drivers/accgyro/accgyro_spi_mpu6500.h" @@ -110,7 +111,8 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); #ifdef STM32F10X #define GYRO_SYNC_DENOM_DEFAULT 8 -#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \ + || defined(USE_GYRO_SPI_ICM20689) #define GYRO_SYNC_DENOM_DEFAULT 1 #else #define GYRO_SYNC_DENOM_DEFAULT 4 @@ -258,6 +260,17 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) } #endif +#ifdef USE_GYRO_SPI_ICM20649 + case GYRO_ICM20649: + if (icm20649SpiGyroDetect(dev)) { + gyroHardware = GYRO_ICM20649; +#ifdef GYRO_ICM20649_ALIGN + dev->gyroAlign = GYRO_ICM20649_ALIGN; +#endif + break; + } +#endif + #ifdef USE_GYRO_SPI_ICM20689 case GYRO_ICM20689: if (icm20689SpiGyroDetect(dev)) { @@ -302,7 +315,8 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) static bool gyroInitSensor(gyroSensor_t *gyroSensor) { -#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_ICM20689) +#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) #if defined(MPU_INT_EXTI) gyroSensor->gyroDev.mpuIntExtiTag = IO_TAG(MPU_INT_EXTI); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index fd055cedd..e68496095 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -35,6 +35,7 @@ typedef enum { GYRO_ICM20601, GYRO_ICM20602, GYRO_ICM20608G, + GYRO_ICM20649, GYRO_ICM20689, GYRO_BMI160, GYRO_FAKE