Prearm check to validate backend read rates against scheduler loop rate

libraries/AP_Arming/AP_Arming.cpp

@@ -484,6 +484,12 @@ bool AP_Arming::ins_checks(bool report)
         }
 #endif
 
+        // check if IMU gyro updates are greater than or equal to Ardupilot Loop rate
+        char fail_msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
+        if (!ins.pre_arm_check_gyro_backend_rate_hz(fail_msg, ARRAY_SIZE(fail_msg))) {
+            check_failed(ARMING_CHECK_INS, report, "%s", fail_msg);
+            return false;
+        }
     }
 
 #if HAL_GYROFFT_ENABLED

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -39,6 +39,7 @@
 #include "AP_InertialSensor_Invensensev3.h"
 #include "AP_InertialSensor_NONE.h"
 #include "AP_InertialSensor_SCHA63T.h"
+#include <AP_Scheduler/AP_Scheduler.h>
 
 /* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop.
  * Output is on the debug console. */
@@ -1492,6 +1493,24 @@ bool AP_InertialSensor::gyro_calibrated_ok_all() const
     return (get_gyro_count() > 0);
 }
 
+// Prearm check to verify that we have sane sched loop rates set based on Gyro backend rates
+bool AP_InertialSensor::pre_arm_check_gyro_backend_rate_hz(char* fail_msg, uint16_t fail_msg_len) const
+{
+#if AP_SCHEDULER_ENABLED
+    for (uint8_t i=0; i<get_gyro_count(); i++) {
+        if (!_use(i) || _backends[i] == nullptr) {
+            continue;
+        }
+        if (_backends[i]->get_gyro_backend_rate_hz() < (2*_loop_rate)) {
+            hal.util->snprintf(fail_msg, fail_msg_len, "Gyro %d backend rate %dHz < sched loop ratex2 %dHz",
+                     i, _backends[i]->get_gyro_backend_rate_hz(), 2 * _loop_rate);
+            return false;
+        }
+    }
+#endif
+    return true;
+}
+
 // return true if gyro instance should be used (must be healthy and have it's use parameter set to 1)
 bool AP_InertialSensor::use_gyro(uint8_t instance) const
 {

libraries/AP_InertialSensor/AP_InertialSensor.h

@@ -158,6 +158,9 @@ class AP_InertialSensor : AP_AccelCal_Client
     uint16_t get_gyro_rate_hz(uint8_t instance) const { return uint16_t(_gyro_raw_sample_rates[instance] * _gyro_over_sampling[instance]); }
     uint16_t get_accel_rate_hz(uint8_t instance) const { return uint16_t(_accel_raw_sample_rates[instance] * _accel_over_sampling[instance]); }
 
+    // validate backend sample rates
+    bool pre_arm_check_gyro_backend_rate_hz(char* fail_msg, uint16_t fail_msg_len) const;
+
     // FFT support access
 #if HAL_GYROFFT_ENABLED
     const Vector3f& get_gyro_for_fft(void) const { return _gyro_for_fft[_first_usable_gyro]; }

libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

@@ -100,6 +100,12 @@ class AP_InertialSensor_Backend
     bool has_been_killed(uint8_t instance) const { return false; }
 #endif
 
+    // get the backend update rate for the gyro in Hz
+    // if the backend polling rate is the same as the sample rate or higher, return raw sample rate
+    // override and return the backend rate in Hz if it is lower than the sample rate
+    virtual uint16_t get_gyro_backend_rate_hz() const {
+        return _gyro_raw_sample_rate(gyro_instance);
+    }
 
     /*
       device driver IDs. These are used to fill in the devtype field

libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h

@@ -58,6 +58,11 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend
     // get a startup banner to output to the GCS
     bool get_output_banner(char* banner, uint8_t banner_len) override;
 
+    // get the gyro backend rate in Hz at which the FIFO is being read
+    uint16_t get_gyro_backend_rate_hz() const override {
+        return _gyro_backend_rate_hz;
+    }
+
     enum Invensense_Type {
         Invensense_MPU6000=0,
         Invensense_MPU6500,

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h

@@ -55,6 +55,11 @@ class AP_InertialSensor_Invensensev2 : public AP_InertialSensor_Backend
     // get a startup banner to output to the GCS
     bool get_output_banner(char* banner, uint8_t banner_len) override;
 
+    // get the gyro backend rate in Hz at which the FIFO is being read
+    uint16_t get_gyro_backend_rate_hz() const override {
+        return _gyro_backend_rate_hz;
+    }
+
     enum Invensensev2_Type {
         Invensensev2_ICM20948 = 0,
         Invensensev2_ICM20648,

libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h

@@ -75,6 +75,11 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend
     bool accumulate_samples(const struct FIFOData *data, uint8_t n_samples);
     bool accumulate_highres_samples(const struct FIFODataHighRes *data, uint8_t n_samples);
 
+    // get the gyro backend rate in Hz at which the FIFO is being read
+    uint16_t get_gyro_backend_rate_hz() const override {
+        return backend_rate_hz;
+    }
+
     // reset FIFO configure1 register
     uint8_t fifo_config1;
 

libraries/AP_InertialSensor/AP_InertialSensor_SITL.h

@@ -8,7 +8,7 @@
 
 // simulated sensor rates in Hz. This matches a pixhawk1
 const uint16_t INS_SITL_SENSOR_A[] = { 1000, 1000 };
-const uint16_t INS_SITL_SENSOR_B[] = { 760, 800 };
+const uint16_t INS_SITL_SENSOR_B[] = { 840, 900 };
 
 #include <SITL/SITL.h>