diff --git a/src/main/drivers/accgyro/accgyro_spi_icm42605.c b/src/main/drivers/accgyro/accgyro_spi_icm42605.c index ea09ad85b..f81699fcc 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm42605.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm42605.c @@ -183,7 +183,7 @@ typedef struct odrEntry_s { uint8_t odr; // See GYRO_ODR in datasheet. } odrEntry_t; -odrEntry_t khzToSupportedODRMap[] = { +static odrEntry_t icm42605PkhzToSupportedODRMap[] = { { 8, 3 }, { 4, 4 }, { 2, 5 }, @@ -205,9 +205,9 @@ void icm42605GyroInit(gyroDev_t *gyro) 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; + for (uint32_t i = 0; i < ARRAYLEN(icm42605PkhzToSupportedODRMap); i++) { + if (icm42605PkhzToSupportedODRMap[i].khz == desiredODRKhz) { + outputDataRate = icm42605PkhzToSupportedODRMap[i].odr; supportedODRFound = true; break; } @@ -219,22 +219,14 @@ void icm42605GyroInit(gyroDev_t *gyro) 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); diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c index 9fda10993..c41ab1c71 100644 --- a/src/main/sensors/gyro_init.c +++ b/src/main/sensors/gyro_init.c @@ -505,7 +505,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_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_GYRO_SPI_ICM42605) bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config); diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 2c7d55117..2292513a1 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -243,7 +243,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) +#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) #define USE_SPI_GYRO #endif