Dynamic FIFO Buffering

libraries/AP_AHRS/AP_AHRS.cpp

@@ -340,9 +340,21 @@ void AP_AHRS::reset_gyro_drift(void)
  */
 void AP_AHRS::update_state(void)
 {
+    const uint8_t primary_gyro = _get_primary_gyro_index();
+    const uint8_t primary_accel = _get_primary_accel_index();
+#if AP_INERTIALSENSOR_ENABLED
+    // tell the IMUS about primary changes
+    if (primary_gyro != state.primary_gyro) {
+        AP::ins().set_primary_gyro(primary_gyro);
+    }
+
+    if (primary_accel != state.primary_accel) {
+        AP::ins().set_primary_accel(primary_accel);
+    }
+#endif
     state.primary_IMU = _get_primary_IMU_index();
-    state.primary_gyro = _get_primary_gyro_index();
-    state.primary_accel = _get_primary_accel_index();
+    state.primary_gyro = primary_gyro;
+    state.primary_accel = primary_accel;
     state.primary_core = _get_primary_core_index();
     state.wind_estimate_ok = _wind_estimate(state.wind_estimate);
     state.EAS2TAS = AP_AHRS_Backend::get_EAS2TAS();
@@ -361,6 +373,7 @@ void AP_AHRS::update_state(void)
     state.velocity_NED_ok = _get_velocity_NED(state.velocity_NED);
 }
 
+// update run at loop rate
 void AP_AHRS::update(bool skip_ins_update)
 {
     // periodically checks to see if we should update the AHRS

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -1891,6 +1891,16 @@ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool conv
 }
 #endif
 
+// notify IMUs of the new primaries
+void AP_InertialSensor::set_primary_gyro(uint8_t instance)
+{
+    _primary_gyro = instance;
+}
+void AP_InertialSensor::set_primary_accel(uint8_t instance)
+{
+    _primary_accel = instance;
+}
+
 /*
   update gyro and accel values from backends
  */
@@ -1963,12 +1973,18 @@ void AP_InertialSensor::update(void)
         for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
             if (_gyro_healthy[i] && _use(i)) {
                 _first_usable_gyro = i;
+#if !AP_AHRS_ENABLED
+                _primary_gyro = _first_usable_gyro;
+#endif
                 break;
             }
         }
         for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
             if (_accel_healthy[i] && _use(i)) {
                 _first_usable_accel = i;
+#if !AP_AHRS_ENABLED
+                _primary_accel = _first_usable_accel;
+#endif
                 break;
             }
         }

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -283,6 +283,10 @@ class AP_InertialSensor : AP_AccelCal_Client
     // Returns newly calculated trim values if calculated
     bool get_new_trim(Vector3f &trim_rad);
 
+    // notify IMUs of the new primaries
+    void set_primary_gyro(uint8_t instance);
+    void set_primary_accel(uint8_t instance);
+
 #if HAL_INS_ACCELCAL_ENABLED
     // initialise and register accel calibrator
     // called during the startup of accel cal
@@ -659,10 +663,14 @@ class AP_InertialSensor : AP_AccelCal_Client
     bool _gyro_cal_ok[INS_MAX_INSTANCES];
     bool _accel_id_ok[INS_MAX_INSTANCES];
 
-    // primary accel and gyro
+    // first usable gyro and accel
     uint8_t _first_usable_gyro;
     uint8_t _first_usable_accel;
 
+    // primary accel and gyro
+    uint8_t _primary_gyro;
+    uint8_t _primary_accel;
+
     // mask of accels and gyros which we will be actively using
     // and this should wait for in wait_for_sample()
     uint8_t _gyro_wait_mask;
@@ -823,6 +831,8 @@ class AP_InertialSensor : AP_AccelCal_Client
     void update_backend_filters();
     // are rate loop samples enabled for this instance?
     bool is_rate_loop_gyro_enabled(uint8_t instance) const;
+    // is dynamic fifo enabled for this instance
+    bool is_dynamic_fifo_enabled(uint8_t instance) const;
     // endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
 };
 

libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp

@@ -18,6 +18,8 @@
 #define AP_HEATER_IMU_INSTANCE 0
 #endif
 
+#define PRIMARY_UPDATE_TIMEOUT_US 200000UL    // continue to notify the primary at 5Hz
+
 const extern AP_HAL::HAL& hal;
 
 AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
@@ -222,23 +224,20 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
     save_gyro_window(instance, gyro, filter_phase++);
 
     Vector3f gyro_filtered = gyro;
-
 #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
     // apply the harmonic notch filters
     for (auto &notch : _imu.harmonic_notches) {
         if (!notch.params.enabled()) {
             continue;
         }
         bool inactive = notch.is_inactive();
-#if AP_AHRS_ENABLED
         // by default we only run the expensive notch filters on the
         // currently active IMU we reset the inactive notch filters so
         // that if we switch IMUs we're not left with old data
         if (!notch.params.hasOption(HarmonicNotchFilterParams::Options::EnableOnAllIMUs) &&
-            instance != AP::ahrs().get_primary_gyro_index()) {
+            instance != _imu._primary_gyro) {
             inactive = true;
         }
-#endif
         if (inactive) {
             // while inactive we reset the filter so when it activates the first output
             // will be the first input sample
@@ -371,6 +370,7 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
 
     // 5us
     log_gyro_raw(instance, sample_us, gyro, _imu._gyro_filtered[instance]);
+    update_primary();
 }
 
 /*
@@ -458,6 +458,7 @@ void AP_InertialSensor_Backend::_notify_new_delta_angle(uint8_t instance, const
     }
 
     log_gyro_raw(instance, sample_us, gyro, _imu._gyro_filtered[instance]);
+    update_primary();
 }
 
 void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &raw_gyro, const Vector3f &filtered_gyro)
@@ -807,6 +808,21 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
     update_gyro_filters(instance);
 }
 
+void AP_InertialSensor_Backend::update_primary()
+{
+    // timing changes need to be made in the bus thread in order to take effect which is
+    // why they are actioned here. Currently the _primary_gyro and _primary_accel can never
+    // be different for a particular IMU
+    const bool is_new_primary = (gyro_instance == _imu._primary_gyro);
+    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(is_new_primary);
+        is_primary = is_new_primary;
+        last_primary_update_us = now_us;
+    }
+}
+
 /*
   propagate filter changes from front end to backend
  */
@@ -850,7 +866,6 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */
     update_accel_filters(instance);
 }
 
-
 /*
   propagate filter changes from front end to backend
  */

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -161,6 +161,8 @@ class AP_InertialSensor_Backend
     // instance numbers of accel and gyro data
     uint8_t gyro_instance;
     uint8_t accel_instance;
+    bool is_primary = true;
+    uint32_t last_primary_update_us;
 
     void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel) __RAMFUNC__;
     void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__;
@@ -292,6 +294,10 @@ class AP_InertialSensor_Backend
     void update_accel(uint8_t instance) __RAMFUNC__; /* front end */
     void update_accel_filters(uint8_t instance) __RAMFUNC__; /* front end */
 
+    // catch updates to the primary gyro and accel
+    void update_primary() __RAMFUNC__; /* backend */
+    virtual void set_primary(bool _is_primary) {}
+
     // support for updating filter at runtime
     uint16_t _last_accel_filter_hz;
     uint16_t _last_gyro_filter_hz;

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp

@@ -31,6 +31,7 @@
  */
 
 #include <AP_HAL/AP_HAL.h>
+#include "AP_InertialSensor_rate_config.h"
 #include "AP_InertialSensor_Invensensev3.h"
 #include <utility>
 #include <stdio.h>
@@ -170,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");
 
+/*
+    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,
@@ -400,6 +414,21 @@ bool AP_InertialSensor_Invensensev3::update()
     return true;
 }
 
+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 {
+            // 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
  */
@@ -556,7 +585,9 @@ void AP_InertialSensor_Invensensev3::read_fifo()
 
     // adjust the periodic callback to be synchronous with the incoming data
     // this means that we rarely run read_fifo() without updating the sensor data
-    dev->adjust_periodic_callback(periodic_handle, backend_period_us);
+    if (is_primary) {
+        dev->adjust_periodic_callback(periodic_handle, backend_period_us);
+    }
 
     while (n_samples > 0) {
         uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN);

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h

@@ -45,6 +45,9 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend
     // acclerometers on Invensense sensors will return values up to 32G
     const uint16_t multiplier_accel = INT16_MAX/(32*GRAVITY_MSS);
 
+protected:
+    void set_primary(bool _is_primary) override;
+
 private:
     AP_InertialSensor_Invensensev3(AP_InertialSensor &imu,
                                    AP_HAL::OwnPtr<AP_HAL::Device> dev,

libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp

@@ -196,8 +196,7 @@ void AP_InertialSensor::write_notch_log_messages() const
 
         // ask the HarmonicNotchFilter object for primary gyro to
         // log the actual notch centers
-        const uint8_t primary_gyro = AP::ahrs().get_primary_gyro_index();
-        notch.filter[primary_gyro].log_notch_centers(i, now_us);
+        notch.filter[_primary_gyro].log_notch_centers(i, now_us);
     }
 }
 #endif  // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED

libraries/AP_InertialSensor/FastRateBuffer.cpp

@@ -79,6 +79,16 @@ bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const
     return fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index();
 }
 
+// whether or not to use the dynamic fifo
+bool AP_InertialSensor::is_dynamic_fifo_enabled(uint8_t instance) const
+{
+    if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
+        return false;
+    }
+    return (_fast_sampling_mask & (1U<<instance)) != 0
+            && fast_rate_buffer->use_rate_loop_gyro_samples();
+}
+
 // get the next available gyro sample from the fast rate buffer
 bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro)
 {