AP_HAL_ChibiOS: correct high resolution scaling on 18bit ICM4xxxx IMUs

ArduPilot · Sep 22, 2023 · 47707e3 · 47707e3
1 parent 470d9cc
commit 47707e3
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 4 deletions.
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
@@ -301,12 +301,13 @@ void AP_InertialSensor_Invensensev3::start()
     // optionally enable high resolution mode
     if (highres_sampling) {
         fifo_config1 |= (1U<<4);  // FIFO_HIRES_EN
-        gyro_scale = GYRO_SCALE_HIGHRES;
+        gyro_scale = GYRO_SCALE_HIGHRES_2000DPS;
         accel_scale = ACCEL_SCALE_HIGHRES_16G;
         temp_sensitivity = 1.0 / 132.48;
         if (inv3_type == Invensensev3_Type::ICM45686) {
             temp_sensitivity = 1.0 / 128.0;
             accel_scale = ACCEL_SCALE_HIGHRES_32G;
+            gyro_scale = GYRO_SCALE_HIGHRES_4000DPS;
         } else if (inv3_type == Invensensev3_Type::ICM42670) {
             temp_sensitivity = 1.0 / 128.0;
         }

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
@@ -101,9 +101,10 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend
     */
     static constexpr float GYRO_SCALE_4000DPS = radians(1) / (SCALE_RANGE_16BIT / 4000.0);
     /*
-      highres gyro is always 131 LSB/DPS regardless of scale
+      highres gyro is always 131 LSB/DPS modified by the data size transmitted
     */
-    static constexpr float GYRO_SCALE_HIGHRES = radians(1) / (SCALE_RANGE_19BIT / 2000.0);
+    static constexpr float GYRO_SCALE_HIGHRES_2000DPS = radians(1) / (SCALE_RANGE_20BIT / 2000.0);
+    static constexpr float GYRO_SCALE_HIGHRES_4000DPS = radians(1) / (SCALE_RANGE_20BIT / 4000.0);
     /*
       Accel scale 16g (2048 LSB/g)
     */
@@ -114,7 +115,6 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend
     static constexpr float ACCEL_SCALE_32G = (GRAVITY_MSS / (SCALE_RANGE_16BIT / 32));
     /*
       highres accel is 16384 LSB/g on 45686 amd 8192 LSB/g on all others
-      scaled to the packet size gives 32768 LSB/g on all sensors
     */
     static constexpr float ACCEL_SCALE_HIGHRES_16G = (GRAVITY_MSS / (SCALE_RANGE_20BIT / 16));
     static constexpr float ACCEL_SCALE_HIGHRES_32G = (GRAVITY_MSS / (SCALE_RANGE_20BIT / 32));