From 586c2064a606f865aeffe83b5fae2289f2b3b621 Mon Sep 17 00:00:00 2001
From: QioTek <71515778+QioTek@users.noreply.github.com>
Date: Mon, 13 Dec 2021 12:43:51 +0800
Subject: [PATCH] AP_InertialSensor: added support for IIM-42652 and ICM-40605
 sensors

handle new Invensense v3 sensor types
---
 .../AP_InertialSensor_Backend.h               |  2 +
 .../AP_InertialSensor_Invensensev3.cpp        | 38 +++++++++++++++++--
 .../AP_InertialSensor_Invensensev3.h          |  7 +++-
 3 files changed, 42 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
index 9c5d44f8d8..fda3da647d 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -121,6 +121,8 @@ public:
         DEVTYPE_INS_ICM40609 = 0x33,
         DEVTYPE_INS_ICM42688 = 0x34,
         DEVTYPE_INS_ICM42605 = 0x35,
+        DEVTYPE_INS_ICM40605 = 0x36,
+        DEVTYPE_INS_IIM42652 = 0x37,
     };
 
 protected:
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
index b38288651c..c83203fc01 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
@@ -19,6 +19,8 @@
    ICM-40609
    ICM-42688
    ICM-42605
+   ICM-40605
+   IIM-42652
 
   Note that this sensor includes 32kHz internal sampling and an
   anti-aliasing filter, which means this driver can be a lot simpler
@@ -62,9 +64,11 @@ static const float GYRO_SCALE = (0.0174532f / 16.4f);
 #define INV3REG_BANK_SEL      0x76
 
 // WHOAMI values
+#define INV3_ID_ICM40605      0x33
 #define INV3_ID_ICM40609      0x3b
 #define INV3_ID_ICM42605      0x42
 #define INV3_ID_ICM42688      0x47
+#define INV3_ID_IIM42652      0x6f
 
 // run output data at 2kHz
 #define INV3_ODR 2000
@@ -127,7 +131,7 @@ void AP_InertialSensor_Invensensev3::fifo_reset()
     // FIFO_MODE stop-on-full
     register_write(INV3REG_FIFO_CONFIG, 0x80);
     // FIFO partial disable, enable accel, gyro, temperature
-    register_write(INV3REG_FIFO_CONFIG1, 0x07);
+    register_write(INV3REG_FIFO_CONFIG1, fifo_config1);
     // little-endian, fifo count in records, last data hold for ODR mismatch
     register_write(INV3REG_INTF_CONFIG0, 0xC0);
     register_write(INV3REG_SIGNAL_PATH_RESET, 2);
@@ -143,24 +147,40 @@ void AP_InertialSensor_Invensensev3::start()
     // initially run the bus at low speed
     dev->set_speed(AP_HAL::Device::SPEED_LOW);
 
-    // always use FIFO
-    fifo_reset();
-
     // grab the used instances
     enum DevTypes devtype;
     switch (inv3_type) {
+    case Invensensev3_Type::IIM42652:
+        devtype = DEVTYPE_INS_IIM42652;
+        fifo_config1 = 0x07;
+        temp_sensitivity = 1.0 / 2.07;
+        break;
     case Invensensev3_Type::ICM42688:
         devtype = DEVTYPE_INS_ICM42688;
+        fifo_config1 = 0x07;
+        temp_sensitivity = 1.0 / 2.07;
         break;
     case Invensensev3_Type::ICM42605:
         devtype = DEVTYPE_INS_ICM42605;
+        fifo_config1 = 0x07;
+        temp_sensitivity = 1.0 / 2.07;
+        break;
+    case Invensensev3_Type::ICM40605:
+        devtype = DEVTYPE_INS_ICM40605;
+        fifo_config1 = 0x0F;
+        temp_sensitivity = 1.0 * 128 / 115.49;
         break;
     case Invensensev3_Type::ICM40609:
     default:
         devtype = DEVTYPE_INS_ICM40609;
+        temp_sensitivity = 1.0 / 2.07;
+        fifo_config1 = 0x07;
         break;
     }
 
+    // always use FIFO
+    fifo_reset();
+
     if (!_imu.register_gyro(gyro_instance, INV3_ODR, dev->get_bus_id_devtype(devtype)) ||
         !_imu.register_accel(accel_instance, INV3_ODR, dev->get_bus_id_devtype(devtype))) {
         return;
@@ -343,6 +363,14 @@ bool AP_InertialSensor_Invensensev3::check_whoami(void)
         inv3_type = Invensensev3_Type::ICM42605;
         accel_scale = (GRAVITY_MSS / 2048);
         return true;
+    case INV3_ID_ICM40605:
+        inv3_type = Invensensev3_Type::ICM40605;
+        accel_scale = (GRAVITY_MSS / 2048);
+        return true;
+    case INV3_ID_IIM42652:
+        inv3_type = Invensensev3_Type::IIM42652;
+        accel_scale = (GRAVITY_MSS / 2048);
+        return true;
     }
     // not a value WHOAMI result
     return false;
@@ -368,7 +396,9 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void)
         _clip_limit = 29.5f * GRAVITY_MSS;
         break;
     case Invensensev3_Type::ICM42688:
+    case Invensensev3_Type::IIM42652:
     case Invensensev3_Type::ICM42605:
+    case Invensensev3_Type::ICM40605:
         _clip_limit = 15.5f * GRAVITY_MSS;
         break;
     }
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
index 5b5a0698bf..eaf1616f44 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
@@ -33,6 +33,8 @@ public:
         ICM40609 = 0,
         ICM42688,
         ICM42605,
+        ICM40605,
+        IIM42652,
     };
 
     // acclerometers on Invensense sensors will return values up to 32G
@@ -63,8 +65,11 @@ private:
     uint8_t gyro_instance;
     uint8_t accel_instance;
 
+    // reset FIFO configure1 register
+    uint8_t fifo_config1;
+
     // temp scaling for FIFO temperature
-    const float temp_sensitivity = 1.0f/2.07;
+    float temp_sensitivity;
     const float temp_zero = 25; // degC
     
     const enum Rotation rotation;