Backport ICM20602 Fast Reset and make it optional

libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc

@@ -340,3 +340,5 @@ define HAL_ADSB_SAGETECH_MXS_ENABLED HAL_ADSB_ENABLED
 # note that if firmware is build with --secure-bl then DFU is
 # disabled
 ENABLE_DFU_BOOT 1
+
+define HAL_ENABLE_FAST_FIFO_RESET_ICM20602 1

libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/defaults.parm

@@ -9,3 +9,5 @@ BATT2_VOLT_MULT 12.02
 # lower heater gains
 BRD_HEAT_I 0.07
 BRD_HEAT_P 50
+EK2_PRIMARY 1
+EK3_PRIMARY 1

libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat

@@ -479,3 +479,4 @@ STORAGE_FLASH_PAGE 1
 # ardupilot root
 ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
 
+define HAL_ENABLE_FAST_FIFO_RESET_ICM20602 1

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp

@@ -27,6 +27,7 @@
 #include <AP_Logger/AP_Logger.h>
 
 #include "AP_InertialSensor_Invensense.h"
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -46,6 +47,10 @@ extern const AP_HAL::HAL& hal;
 #define debug(fmt, args ...)  do {printf("MPU: " fmt "\n", ## args); } while(0)
 #endif
 
+#ifndef HAL_ENABLE_FAST_FIFO_RESET_ICM20602
+#define HAL_ENABLE_FAST_FIFO_RESET_ICM20602 0
+#endif
+
 /*
   EXT_SYNC allows for frame synchronisation with an external device
   such as a camera. When enabled the LSB of AccelZ holds the FSYNC bit
@@ -195,6 +200,16 @@ void AP_InertialSensor_Invensense::_fifo_reset(bool log_error)
     notify_gyro_fifo_reset(_gyro_instance);
 }
 
+void AP_InertialSensor_Invensense::_fast_fifo_reset()
+{
+    fast_reset_count++;
+    _register_write(MPUREG_USER_CTRL, _last_stat_user_ctrl | BIT_USER_CTRL_FIFO_RESET);
+
+    notify_accel_fifo_reset(_accel_instance);
+    notify_gyro_fifo_reset(_gyro_instance);
+}
+
+
 bool AP_InertialSensor_Invensense::_has_auxiliary_bus()
 {
     return _dev->bus_type() != AP_HAL::Device::BUS_TYPE_I2C;
@@ -225,6 +240,10 @@ void AP_InertialSensor_Invensense::start()
     case Invensense_ICM20602:
         gdev = DEVTYPE_INS_ICM20602;
         adev = DEVTYPE_INS_ICM20602;
+        // ICM20602 has a bug where sometimes the data gets a huge offset
+        // this seems to be fixed by doing a quick FIFO reset via USR_CTRL
+        // reg
+        _enable_fast_fifo_reset = HAL_ENABLE_FAST_FIFO_RESET_ICM20602;
         _enable_offset_checking = true;
         break;
     case Invensense_ICM20601:
@@ -434,6 +453,29 @@ bool AP_InertialSensor_Invensense::update() /* front end */
 
     _publish_temperature(_accel_instance, _temp_filtered);
 
+#if HAL_ENABLE_FAST_FIFO_RESET_ICM20602
+    if (fast_reset_count) {
+        // check if we have reported in the last 1 seconds or
+        // fast_reset_count changed
+#if HAL_GCS_ENABLED && BOARD_FLASH_SIZE > 1024
+        if (AP_HAL::millis() - last_fast_reset_count_report_ms > 1000) {
+            last_fast_reset_count_report_ms = AP_HAL::millis();
+            char param_name[sizeof("IMUx_RST")];
+            if (_gyro_instance <= 9) {
+                snprintf(param_name, sizeof(param_name), "IMU%u_RST", _gyro_instance);
+            } else {
+                snprintf(param_name, sizeof(param_name), "IMUx_RST");
+            }
+            gcs().send_named_float(param_name, fast_reset_count);
+        }
+#endif
+#if HAL_LOGGING_ENABLED
+        if (last_fast_reset_count != fast_reset_count) {
+            AP::logger().Write_MessageF("IMU%u fast fifo reset %u", _gyro_instance, fast_reset_count);
+        }
+#endif
+    }
+#endif
     return true;
 }
 
@@ -547,11 +589,16 @@ bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_sampl
 
         int16_t t2 = int16_val(data, 3);
         if (!_check_raw_temp(t2)) {
-            if (!hal.scheduler->in_expected_delay()) {
-                debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
+            if (_enable_fast_fifo_reset) {
+                _fast_fifo_reset();
+                return false;
+            } else {
+                if (!hal.scheduler->in_expected_delay()) {
+                    debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
+                }
+                _fifo_reset(true);
+                return false;
             }
-            _fifo_reset(true);
-            return false;
         }
         float temp = t2 * temp_sensitivity + temp_zero;
 
@@ -589,14 +636,22 @@ bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *sam
     for (uint8_t i = 0; i < n_samples; i++) {
         const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
 
-        // use temperatue to detect FIFO corruption
+        // use temperature to detect FIFO corruption
         int16_t t2 = int16_val(data, 3);
         if (!_check_raw_temp(t2)) {
-            if (!hal.scheduler->in_expected_delay()) {
-                debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
+#if HAL_ENABLE_FAST_FIFO_RESET_ICM20602
+            if (_enable_fast_fifo_reset) {
+                _fast_fifo_reset();
+                ret = false;
+            } else 
+#endif
+            {
+                if (!hal.scheduler->in_expected_delay()) {
+                    debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
+                }
+                _fifo_reset(true);
+                ret = false;
             }
-            _fifo_reset(true);
-            ret = false;
             break;
         }
         tsum += t2;
@@ -727,7 +782,7 @@ void AP_InertialSensor_Invensense::_read_fifo()
 
         if (_fast_sampling) {
             if (!_accumulate_sensor_rate_sampling(rx, n)) {
-                if (!hal.scheduler->in_expected_delay()) {
+                if (!hal.scheduler->in_expected_delay() && !_enable_fast_fifo_reset) {
                     debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/MPU_SAMPLE_SIZE);
                 }
                 break;

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h

@@ -86,6 +86,8 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend
 
     void _set_filter_register(void);
     void _fifo_reset(bool log_error) __RAMFUNC__;
+    void _fast_fifo_reset() __RAMFUNC__;
+
     bool _has_auxiliary_bus();
 
     /* Read samples from FIFO (FIFO enabled) */
@@ -129,12 +131,18 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend
     LowPassFilter2pFloat _temp_filter;
     uint32_t last_reset_ms;
     uint8_t reset_count;
+    uint8_t fast_reset_count;
+    uint8_t last_fast_reset_count;
+    uint32_t last_fast_reset_count_report_ms;
 
     enum Rotation _rotation;
 
     // enable checking of unexpected resets of offsets
     bool _enable_offset_checking;
 
+    // enable fast fifo reset instead of full fifo reset
+    bool _enable_fast_fifo_reset;
+
     // ICM-20602 y offset register. See usage for explanation
     uint8_t _saved_y_ofs_high;