From f2e39e0ce2743aae3a9685112d5dd7ba8f82c38f Mon Sep 17 00:00:00 2001
From: Michael Keller <github@ike.ch>
Date: Wed, 28 Apr 2021 02:34:52 +1200
Subject: [PATCH] Merge pull request #10693 from hydra/bf-icm42605-fixes-1

ICM42605 fixes.
---
 src/main/drivers/accgyro/accgyro_spi_icm42605.c | 16 ++++------------
 src/main/sensors/gyro_init.c                    |  2 +-
 src/main/target/common_post.h                   |  2 +-
 3 files changed, 6 insertions(+), 14 deletions(-)

diff --git a/src/main/drivers/accgyro/accgyro_spi_icm42605.c b/src/main/drivers/accgyro/accgyro_spi_icm42605.c
index cea9d0ac27..74a98323ca 100644
--- a/src/main/drivers/accgyro/accgyro_spi_icm42605.c
+++ b/src/main/drivers/accgyro/accgyro_spi_icm42605.c
@@ -177,7 +177,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 },
@@ -199,9 +199,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;
             }
@@ -213,22 +213,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 1a3475e18a..1eeb870db9 100644
--- a/src/main/sensors/gyro_init.c
+++ b/src/main/sensors/gyro_init.c
@@ -494,7 +494,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_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || 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 1282e7664c..b4324caeef 100644
--- a/src/main/target/common_post.h
+++ b/src/main/target/common_post.h
@@ -235,7 +235,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