From 8b60965be58f613c1d0442f513ee87fae402517b Mon Sep 17 00:00:00 2001
From: Freek van Tienen <freek.v.tienen@gmail.com>
Date: Mon, 4 Dec 2023 09:09:23 +0100
Subject: [PATCH] [imu] Fix Invensense V3 driver temeperature and disable AFSR
 (#3192)

---
 conf/modules/imu_cube.xml                  |  2 +-
 sw/airborne/peripherals/invensense3.c      | 21 +++++++++++++--------
 sw/airborne/peripherals/invensense3_regs.h |  1 +
 3 files changed, 15 insertions(+), 9 deletions(-)

diff --git a/conf/modules/imu_cube.xml b/conf/modules/imu_cube.xml
index fafe54341e..c9892a45a0 100644
--- a/conf/modules/imu_cube.xml
+++ b/conf/modules/imu_cube.xml
@@ -46,7 +46,7 @@
 
      <!-- Configure the heater (ICM20602) -->
     <define name="IMU_HEATER_GYRO_ID" value="IMU_CUBE2_ID"/>
-    <define name="IMU_HEATER_TARGET_TEMP" value="55.0"/>
+    <define name="IMU_HEATER_TARGET_TEMP" value="45.0"/>
     <define name="IMU_HEATER_P_GAIN" value="50.0"/>
     <define name="IMU_HEATER_I_GAIN" value="0.07"/>
 
diff --git a/sw/airborne/peripherals/invensense3.c b/sw/airborne/peripherals/invensense3.c
index c4be3c7a76..b54cf6b364 100644
--- a/sw/airborne/peripherals/invensense3.c
+++ b/sw/airborne/peripherals/invensense3.c
@@ -488,7 +488,7 @@ static void invensense3_parse_fifo_data(struct invensense3_t *inv, uint8_t *data
         
         // Temperature sensor register data TEMP_DATA is updated with new data at max(Accelerometer ODR, Gyroscope ODR)
         if(gyro_samplerate == faster_odr)
-          temp += data[13];
+          temp += (int8_t)data[13];
         
         i++;
       }
@@ -500,7 +500,7 @@ static void invensense3_parse_fifo_data(struct invensense3_t *inv, uint8_t *data
         accel[j].z =  (int16_t)((uint16_t)data[5] | data[6] << 8 );
 
         if(accel_samplerate == faster_odr)
-          temp += data[13];
+          temp += (int8_t)data[13];
 
         j++;
       }
@@ -919,27 +919,32 @@ static bool invensense3_config(struct invensense3_t *inv) {
         inv->config_idx++;
       break;
     case 12:
+      /* Disable AFSR, which changes noise sensitivity with angular rate, causing stuck gyro's for 2ms resulting in DC gyro bias */
+      if(invensense3_register_write(inv, INV3REG_INTF_CONFIG1, 0x51))
+        inv->config_idx++;
+      break;
+    case 13:
       /* FIFO Stop-on-Full mode (enable the FIFO) */
       if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG , FIFO_CONFIG_MODE_STOP_ON_FULL << FIFO_CONFIG_MODE_SHIFT))
         inv->config_idx++;
       break;
-    case 13:
+    case 14:
       /* FIFO content: enable accel, gyro, temperature */
       if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG1 , BIT_FIFO_CONFIG1_ACCEL_EN | 
                     BIT_FIFO_CONFIG1_GYRO_EN | BIT_FIFO_CONFIG1_TEMP_EN))
         inv->config_idx++;
       break;
-    case 14:
+    case 15:
       /* Set FIFO watermark to 1 (so that INT is triggered for each packet) */
       if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG2, 20))
         inv->config_idx++;
       break;
-    case 15:
+    case 16:
       /* Set FIFO watermark to 1 (so that INT is triggered for each packet) */
       if(invensense3_register_write(inv, INV3REG_FIFO_CONFIG3, 0x00))
         inv->config_idx++;
       break;
-    case 16:
+    case 17:
       /* Enable interrupt pin/status */
         switch(inv->parser) {
           case INVENSENSE3_PARSER_REGISTERS:
@@ -954,13 +959,13 @@ static bool invensense3_config(struct invensense3_t *inv) {
             break;
           }
       break;
-    case 17:
+    case 18:
       /* Interrupt pins ASYNC_RESET configuration */
       // Datasheet: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
       if(invensense3_register_write(inv, INV3REG_INT_CONFIG1, BIT_INT_ASYNC_RESET))
         inv->config_idx++;
       break;
-    case 18:
+    case 19:
       /* FIFO flush */
       if(invensense3_reset_fifo(inv))
         inv->config_idx++;
diff --git a/sw/airborne/peripherals/invensense3_regs.h b/sw/airborne/peripherals/invensense3_regs.h
index 5238655910..f1a07e9e24 100644
--- a/sw/airborne/peripherals/invensense3_regs.h
+++ b/sw/airborne/peripherals/invensense3_regs.h
@@ -66,6 +66,7 @@
 #       define FIFO_COUNT_BIG_ENDIAN                        0x20
 #       define FIFO_COUNT_REC                               0x40
 #       define FIFO_HOLD_LAST_DATA_EN                       0x80
+#define INV3REG_INTF_CONFIG1          INV3REG(INV3_BANK0,0x4DU)
 #define INV3REG_PWR_MGMT0             INV3REG(INV3_BANK0,0x4EU)
 #       define ACCEL_MODE_OFF                               0x00
 #       define ACCEL_MODE_LN                                0x03