AP_InertialSensor: only update the primary once. scale FIFO reads to …

…2x loop rate.

only increase FIFO buffer when compiled with fast rate

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -817,8 +817,7 @@ void AP_InertialSensor_Backend::update_primary()
     uint32_t now_us = AP_HAL::micros();
     if (is_primary != is_new_primary
         || AP_HAL::timeout_expired(last_primary_update_us, now_us, PRIMARY_UPDATE_TIMEOUT_US)) {
-        set_primary_gyro(is_new_primary);
-        set_primary_accel(is_new_primary);
+        set_primary(is_new_primary);
         is_primary = is_new_primary;
         last_primary_update_us = now_us;
     }

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -296,8 +296,7 @@ class AP_InertialSensor_Backend
 
     // catch updates to the primary gyro and accel
     void update_primary() __RAMFUNC__; /* backend */
-    virtual void set_primary_gyro(bool is_primary_gyro) {}
-    virtual void set_primary_accel(bool is_primary_accel) {}
+    virtual void set_primary(bool _is_primary) {}
 
     // support for updating filter at runtime
     uint16_t _last_accel_filter_hz;

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

@@ -171,7 +171,20 @@ struct PACKED FIFODataHighRes {
 static_assert(sizeof(FIFOData) == 16, "FIFOData must be 16 bytes");
 static_assert(sizeof(FIFODataHighRes) == 20, "FIFODataHighRes must be 20 bytes");
 
-#define INV3_FIFO_BUFFER_LEN 16
+/*
+    Ideally we would like the fifo buffer to be big enough to hold all of the packets that might have been
+    accumulated between reads. This is so that they can all be read in a single SPI transaction and avoid
+    the overhead of multiple reads. The maximum number of samples for 20-bit high res that can be store in the
+    fifo is 2k/20, or 105 samples. The likely maximum required for dynamic fifo is the output data rate / 2x loop rate,
+    4k/400 or 10 samples in the extreme case we are trying to support. We need double this to account for the
+    fact that we might only have 9 samples at the time the fifo is read and hence the next time it is read we
+    could have 19 sample
+ */
+#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
+#define INV3_FIFO_BUFFER_LEN 24
+#else
+#define INV3_FIFO_BUFFER_LEN 8
+#endif
 
 AP_InertialSensor_Invensensev3::AP_InertialSensor_Invensensev3(AP_InertialSensor &imu,
                                                                AP_HAL::OwnPtr<AP_HAL::Device> _dev,
@@ -401,14 +414,16 @@ bool AP_InertialSensor_Invensensev3::update()
     return true;
 }
 
-void AP_InertialSensor_Invensensev3::set_primary_gyro(bool _is_primary)
+void AP_InertialSensor_Invensensev3::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, backend_period_us);
         } else {
-            dev->adjust_periodic_callback(periodic_handle, backend_period_us * get_fast_sampling_rate());
+            // 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

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h

@@ -46,7 +46,7 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend
     const uint16_t multiplier_accel = INT16_MAX/(32*GRAVITY_MSS);
 
 protected:
-    void set_primary_gyro(bool is_primary) override;
+    void set_primary(bool _is_primary) override;
 
 private:
     AP_InertialSensor_Invensensev3(AP_InertialSensor &imu,