From 86f5ccdb80664d2bd9733a2d4b861e1997f56a54 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Thu, 15 Apr 2021 21:42:16 +0200
Subject: [PATCH] ICM42605 - Fix missing use of USE_SPI_GYRO.

ICM42605 - Fix missing call to mpuDetect.

ICM42605 - Disable un-needed debug code by default.

ICM42605 - Delete unneeded reads of ICM42605_RA_GYRO_CONFIG0 and
ICM42605_RA_ACCEL_CONFIG0.

ICM42605 - rename the khzToSupportedODRMap and make it static.
---
 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 ea09ad85ba..f81699fcc5 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 9fda10993d..c41ab1c710 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 2c7d551179..2292513a12 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