From 8acbc50c5238b1e584013cd9198f40166395b227 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 19 Nov 2024 10:50:31 +1100 Subject: [PATCH 1/2] AP_InertialSensor: prearm check to validate backend read rates against scheduler loop rate --- .../AP_InertialSensor/AP_InertialSensor.cpp | 19 +++++++++++++++++++ .../AP_InertialSensor/AP_InertialSensor.h | 3 +++ .../AP_InertialSensor_Backend.h | 6 ++++++ .../AP_InertialSensor_Invensense.h | 5 +++++ .../AP_InertialSensor_Invensensev2.h | 5 +++++ .../AP_InertialSensor_Invensensev3.h | 5 +++++ 6 files changed, 43 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 25ff1dcc0fcc0..2c306b1198818 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/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 /* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop. * Output is on the debug console. */ @@ -1502,6 +1503,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; iget_gyro_backend_rate_hz() < (1.8*_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 { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 5bd13fae14923..1d78f08e99489 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/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]; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index f98925dd10123..d1708de71c5f2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index 7ba132f3a309a..223e2d59b5f39 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/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, diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h index 1b14062863a29..98fbad8d9818b 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h +++ b/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, diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h index 2ea558b3fddda..94204f5be5fe1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -78,6 +78,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; From dd644b33124d3fe47964f0207cf0518701446702 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Tue, 19 Nov 2024 10:51:44 +1100 Subject: [PATCH 2/2] AP_Arming: prearm check to validate backend read rates against scheduler loop rate --- libraries/AP_Arming/AP_Arming.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 78713fb304603..a7abc77e6fee0 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -513,6 +513,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