From c2243c7fd8976a51b51dc255ccaa5349da62f7a4 Mon Sep 17 00:00:00 2001
From: Andrey Mironov <diehertz@gmail.com>
Date: Sun, 29 Jul 2018 10:13:53 +0300
Subject: [PATCH] Added 4K DPS awareness for ICM-20601

---
 src/main/drivers/accgyro/accgyro_mpu.h         | 16 ++++++++++++++++
 src/main/drivers/accgyro/accgyro_mpu6500.c     | 12 ++++++++++--
 src/main/drivers/accgyro/accgyro_spi_mpu6500.c |  7 ++++---
 3 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h
index d038ba5ed7..dca3d9625f 100644
--- a/src/main/drivers/accgyro/accgyro_mpu.h
+++ b/src/main/drivers/accgyro/accgyro_mpu.h
@@ -156,6 +156,14 @@ enum gyro_fsr_e {
     NUM_GYRO_FSR
 };
 
+enum icm_high_range_gyro_fsr_e {
+    ICM_HIGH_RANGE_FSR_500DPS = 0,
+    ICM_HIGH_RANGE_FSR_1000DPS,
+    ICM_HIGH_RANGE_FSR_2000DPS,
+    ICM_HIGH_RANGE_FSR_4000DPS,
+    NUM_ICM_HIGH_RANGE_GYRO_FSR
+};
+
 enum fchoice_b {
     FCB_DISABLED = 0x00,
     FCB_8800_32 = 0x01,
@@ -176,6 +184,14 @@ enum accel_fsr_e {
     NUM_ACCEL_FSR
 };
 
+enum icm_high_range_accel_fsr_e {
+    ICM_HIGH_RANGE_FSR_4G = 0,
+    ICM_HIGH_RANGE_FSR_8G,
+    ICM_HIGH_RANGE_FSR_16G,
+    ICM_HIGH_RANGE_FSR_32G,
+    NUM_ICM_HIGH_RANGE_ACCEL_FSR
+};
+
 typedef enum {
     GYRO_OVERFLOW_NONE = 0x00,
     GYRO_OVERFLOW_X = 0x01,
diff --git a/src/main/drivers/accgyro/accgyro_mpu6500.c b/src/main/drivers/accgyro/accgyro_mpu6500.c
index 7a7cac1854..dd534e392f 100644
--- a/src/main/drivers/accgyro/accgyro_mpu6500.c
+++ b/src/main/drivers/accgyro/accgyro_mpu6500.c
@@ -56,6 +56,14 @@ void mpu6500GyroInit(gyroDev_t *gyro)
 {
     mpuGyroInit(gyro);
 
+    int gyro_range = INV_FSR_2000DPS;
+    int accel_range = INV_FSR_16G;
+
+    if (gyro->mpuDetectionResult.sensor == ICM_20601_SPI) {
+        gyro_range = gyro->gyro_high_fsr ? ICM_HIGH_RANGE_FSR_4000DPS : ICM_HIGH_RANGE_FSR_2000DPS;
+        accel_range = ICM_HIGH_RANGE_FSR_16G;
+    }
+
     busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
     delay(100);
     busWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07);
@@ -64,9 +72,9 @@ void mpu6500GyroInit(gyroDev_t *gyro)
     delay(100);
     busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
     delay(15);
-    busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | mpuGyroFCHOICE(gyro));
+    busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, gyro_range << 3 | mpuGyroFCHOICE(gyro));
     delay(15);
-    busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
+    busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, accel_range << 3);
     delay(15);
     busWriteRegister(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
     delay(15);
diff --git a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c
index 22ed09d3ef..808ec6d789 100644
--- a/src/main/drivers/accgyro/accgyro_spi_mpu6500.c
+++ b/src/main/drivers/accgyro/accgyro_spi_mpu6500.c
@@ -128,7 +128,11 @@ bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
     case MPU_9250_SPI:
     case ICM_20608_SPI:
     case ICM_20602_SPI:
+        // 16.4 dps/lsb scalefactor
+        gyro->scale = 1.0f / 16.4f;
+        break;
     case ICM_20601_SPI:
+        gyro->scale = 1.0f / (gyro->gyro_high_fsr ? 8.2f : 16.4f);
         break;
     default:
         return false;
@@ -137,8 +141,5 @@ bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
     gyro->initFn = mpu6500SpiGyroInit;
     gyro->readFn = mpuGyroReadSPI;
 
-    // 16.4 dps/lsb scalefactor
-    gyro->scale = 1.0f / 16.4f;
-
     return true;
 }