AP_InertialSensor: enable fast rate logic on Invensense v1 IMUs

Author: andyp1per

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp

@@ -26,6 +26,7 @@
 #include <AP_InternalError/AP_InternalError.h>
 #include <AP_Logger/AP_Logger.h>
 
+#include "AP_InertialSensor_rate_config.h"
 #include "AP_InertialSensor_Invensense.h"
 #include <GCS_MAVLink/GCS.h>
 
@@ -487,6 +488,21 @@ bool AP_InertialSensor_Invensense::update() /* front end */
     return true;
 }
 
+void AP_InertialSensor_Invensense::set_primary(bool _is_primary)
+{
+#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
+    if (_imu.is_dynamic_fifo_enabled(gyro_instance)) {
+        if (_is_primary) {
+            _dev->adjust_periodic_callback(periodic_handle, 1000000UL / _gyro_backend_rate_hz);
+        } else {
+            // scale down non-primary to 2x loop rate, but no greater than the default sampling rate
+            _dev->adjust_periodic_callback(periodic_handle,
+                                          1000000UL / constrain_int16(get_loop_rate_hz() * 2, 400, 1000));
+        }
+    }
+#endif
+}
+
 /*
   accumulate new samples
  */

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h

@@ -73,6 +73,8 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend
     // 24G, but they are not guaranteed to be remotely linear past
     // 16G
     const uint16_t multiplier_accel = INT16_MAX/(26*GRAVITY_MSS);
+protected:
+    void set_primary(bool _is_primary) override;
 
 private:
     AP_InertialSensor_Invensense(AP_InertialSensor &imu,