From c8f90b03d5eee9e25ce20c75c6a4431d02d4683a Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 25 May 2024 09:30:00 +0100 Subject: [PATCH 1/9] AP_InertialSensor: keep a record of the priamry gyro and use it add callbacks for when gyrp/accel primary changes --- .../AP_InertialSensor/AP_InertialSensor.cpp | 11 +++++++ .../AP_InertialSensor/AP_InertialSensor.h | 6 +++- .../AP_InertialSensor_Backend.cpp | 30 ++++++++++++++++--- .../AP_InertialSensor_Backend.h | 8 +++++ .../AP_InertialSensor_Logging.cpp | 3 +- 5 files changed, 51 insertions(+), 7 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 24f2c51287c99..e93ac7ee931c5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1959,16 +1959,27 @@ void AP_InertialSensor::update(void) } } +#if AP_AHRS_ENABLED + // ask AHRS for the true primary, might just be us though + _primary_gyro = AP::ahrs().get_primary_gyro_index(); + _primary_accel = AP::ahrs().get_primary_accel_index(); +#endif // set primary to first healthy accel and gyro for (uint8_t i=0; i Date: Wed, 11 Dec 2024 20:42:46 +0000 Subject: [PATCH 2/9] AP_InertialSensor: adjust read rate of Invensense v3 depending on primary configure dynamic fifo rates based on primary gyro instance add set_primary_gyro() and set_primary_accel() adjust FIFO buffer sizes --- .../AP_InertialSensor/AP_InertialSensor.cpp | 15 ++++++++++----- .../AP_InertialSensor/AP_InertialSensor.h | 4 ++++ .../AP_InertialSensor_Invensensev3.cpp | 19 +++++++++++++++++-- .../AP_InertialSensor_Invensensev3.h | 5 +++++ .../AP_InertialSensor_config.h | 8 ++++++++ 5 files changed, 44 insertions(+), 7 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index e93ac7ee931c5..b1a5d6d866e9e 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/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 */ @@ -1959,11 +1969,6 @@ void AP_InertialSensor::update(void) } } -#if AP_AHRS_ENABLED - // ask AHRS for the true primary, might just be us though - _primary_gyro = AP::ahrs().get_primary_gyro_index(); - _primary_accel = AP::ahrs().get_primary_accel_index(); -#endif // set primary to first healthy accel and gyro for (uint8_t i=0; i _dev, @@ -400,6 +400,19 @@ bool AP_InertialSensor_Invensensev3::update() return true; } +#if AP_INERTIALSENSOR_DYNAMIC_FIFO +void AP_InertialSensor_Invensensev3::set_primary_gyro(bool is_primary) +{ + if (((1U<adjust_periodic_callback(periodic_handle, backend_period_us); + } else { + dev->adjust_periodic_callback(periodic_handle, backend_period_us * get_fast_sampling_rate()); + } + } +} +#endif + /* accumulate new samples */ @@ -556,7 +569,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_gyro) { + dev->adjust_periodic_callback(periodic_handle, backend_period_us); + } while (n_samples > 0) { uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h index 889fda4f76c86..d0b49ab2222ad 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -45,6 +45,11 @@ 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: +#if AP_INERTIALSENSOR_DYNAMIC_FIFO + void set_primary_gyro(bool is_primary) override; +#endif + private: AP_InertialSensor_Invensensev3(AP_InertialSensor &imu, AP_HAL::OwnPtr dev, diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_config.h b/libraries/AP_InertialSensor/AP_InertialSensor_config.h index 5ad27d0300334..aae26186ad9fb 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_config.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_config.h @@ -73,3 +73,11 @@ #ifndef AP_INERTIALSENSOR_KILL_IMU_ENABLED #define AP_INERTIALSENSOR_KILL_IMU_ENABLED 1 #endif + +#ifndef AP_INERTIALSENSOR_DYNAMIC_FIFO +#define AP_INERTIALSENSOR_DYNAMIC_FIFO 1 +#endif + +#ifndef AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK +#define AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK 127 +#endif From 94643cadb698ce761295881e3adf61ce145ebe8d Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 12 Sep 2024 11:48:34 +0100 Subject: [PATCH 3/9] AP_AHRS: configure primary gyro/accel in IMU when it changes --- libraries/AP_AHRS/AP_AHRS.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 8f16ba46131e8..05421e2720782 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/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 From 5cc71b0a562add5fa55bfd2e0bd7fdce43b9783f Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 4 Dec 2024 20:22:15 +0000 Subject: [PATCH 4/9] AP_InertialSensor: only use dynamic FIFO when using fast rate loop --- libraries/AP_InertialSensor/AP_InertialSensor.h | 2 ++ .../AP_InertialSensor_Invensense.cpp | 1 + .../AP_InertialSensor_Invensensev2.cpp | 1 + .../AP_InertialSensor_Invensensev3.cpp | 7 ++++--- .../AP_InertialSensor_Invensensev3.h | 2 -- .../AP_InertialSensor/AP_InertialSensor_config.h | 8 -------- .../AP_InertialSensor/AP_InertialSensor_rate_config.h | 4 ++++ libraries/AP_InertialSensor/FastRateBuffer.cpp | 11 +++++++++++ 8 files changed, 23 insertions(+), 13 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index a0c6c5ea4d429..be4bc5ce4d6cd 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -831,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 }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index 18eb0a2080fa5..ee71ae48d14f2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -26,6 +26,7 @@ #include #include +#include "AP_InertialSensor_rate_config.h" #include "AP_InertialSensor_Invensense.h" #include diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp index b8279d339fd62..f234275c395e1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp @@ -23,6 +23,7 @@ #include +#include "AP_InertialSensor_rate_config.h" #include "AP_InertialSensor_Invensensev2.h" extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index 18f2cee9f9d33..aec47327f7035 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -31,6 +31,7 @@ */ #include +#include "AP_InertialSensor_rate_config.h" #include "AP_InertialSensor_Invensensev3.h" #include #include @@ -400,18 +401,18 @@ bool AP_InertialSensor_Invensensev3::update() return true; } -#if AP_INERTIALSENSOR_DYNAMIC_FIFO void AP_InertialSensor_Invensensev3::set_primary_gyro(bool is_primary) { - if (((1U<adjust_periodic_callback(periodic_handle, backend_period_us); } else { dev->adjust_periodic_callback(periodic_handle, backend_period_us * get_fast_sampling_rate()); } } -} #endif +} /* accumulate new samples diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h index d0b49ab2222ad..2c20db6914be7 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -46,9 +46,7 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend const uint16_t multiplier_accel = INT16_MAX/(32*GRAVITY_MSS); protected: -#if AP_INERTIALSENSOR_DYNAMIC_FIFO void set_primary_gyro(bool is_primary) override; -#endif private: AP_InertialSensor_Invensensev3(AP_InertialSensor &imu, diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_config.h b/libraries/AP_InertialSensor/AP_InertialSensor_config.h index aae26186ad9fb..5ad27d0300334 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_config.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_config.h @@ -73,11 +73,3 @@ #ifndef AP_INERTIALSENSOR_KILL_IMU_ENABLED #define AP_INERTIALSENSOR_KILL_IMU_ENABLED 1 #endif - -#ifndef AP_INERTIALSENSOR_DYNAMIC_FIFO -#define AP_INERTIALSENSOR_DYNAMIC_FIFO 1 -#endif - -#ifndef AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK -#define AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK 127 -#endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h b/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h index 8bf45fecab7f0..b7305e76964a0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h @@ -7,3 +7,7 @@ #ifndef AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED #define AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED (AP_INERTIALSENSOR_ENABLED && HAL_INS_RATE_LOOP && AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduCopter)) #endif + +#ifndef AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK +#define AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK 127 +#endif diff --git a/libraries/AP_InertialSensor/FastRateBuffer.cpp b/libraries/AP_InertialSensor/FastRateBuffer.cpp index 29601965d38ed..b47450493d41e 100644 --- a/libraries/AP_InertialSensor/FastRateBuffer.cpp +++ b/libraries/AP_InertialSensor/FastRateBuffer.cpp @@ -79,6 +79,17 @@ 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 ((1U<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) { From c5be91723da15745098b2c937699261e806f2b95 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 10 Dec 2024 10:46:13 +0000 Subject: [PATCH 5/9] AP_InertialSensor: periodically notify of primary gyro and accels --- .../AP_InertialSensor/AP_InertialSensor_Backend.cpp | 12 ++++++++++-- .../AP_InertialSensor/AP_InertialSensor_Backend.h | 2 ++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 4a43aeef562b5..8f985ed8bafa3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/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) : @@ -813,9 +815,12 @@ void AP_InertialSensor_Backend::update_primary_gyro() // timing changes need to be made in the bus thread in order to take effect which is // why they are actioned here const bool is_new_primary = gyro_instance == _imu._primary_gyro; - if (is_primary_gyro != is_new_primary) { + uint32_t now_us = AP_HAL::micros(); + if (is_primary_gyro != is_new_primary + || AP_HAL::timeout_expired(last_primary_gyro_update_us, now_us, PRIMARY_UPDATE_TIMEOUT_US)) { set_primary_gyro(is_new_primary); is_primary_gyro = is_new_primary; + last_primary_gyro_update_us = now_us; } } @@ -867,9 +872,12 @@ void AP_InertialSensor_Backend::update_primary_accel() // timing changes need to be made in the bus thread in order to take effect which is // why they are actioned here const bool is_new_primary = accel_instance == _imu._primary_accel; - if (is_primary_accel != is_new_primary) { + uint32_t now_us = AP_HAL::micros(); + if (is_primary_accel != is_new_primary + || AP_HAL::timeout_expired(last_primary_accel_update_us, now_us, PRIMARY_UPDATE_TIMEOUT_US)) { set_primary_accel(is_new_primary); is_primary_accel = is_new_primary; + last_primary_accel_update_us = now_us; } } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 1fedf3c242b32..443ebdb0fdce0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -161,8 +161,10 @@ class AP_InertialSensor_Backend // instance numbers of accel and gyro data uint8_t gyro_instance; bool is_primary_gyro = true; + uint32_t last_primary_gyro_update_us; uint8_t accel_instance; bool is_primary_accel = true; + uint32_t last_primary_accel_update_us; void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel) __RAMFUNC__; void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro) __RAMFUNC__; From 612e5d1f5546ea4e2697321e0fee6a3b3c0a6325 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 19 Dec 2024 10:13:35 +0000 Subject: [PATCH 6/9] AP_InertialSensor: have a single callback for primary switching --- .../AP_InertialSensor_Backend.cpp | 34 ++++++------------- .../AP_InertialSensor_Backend.h | 13 +++---- .../AP_InertialSensor_Invensensev3.cpp | 6 ++-- 3 files changed, 18 insertions(+), 35 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 8f985ed8bafa3..2aeffb24cb8c7 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -370,7 +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_gyro(); + update_primary(); } /* @@ -458,7 +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_gyro(); + 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) @@ -616,7 +616,6 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance, // assume we're doing pre-filter logging: log_accel_raw(instance, sample_us, accel); #endif - update_primary_accel(); } /* @@ -694,7 +693,6 @@ void AP_InertialSensor_Backend::_notify_new_delta_velocity(uint8_t instance, con // assume we're doing pre-filter logging log_accel_raw(instance, sample_us, accel); #endif - update_primary_accel(); } @@ -810,17 +808,19 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */ update_gyro_filters(instance); } -void AP_InertialSensor_Backend::update_primary_gyro() +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 + // 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_gyro != is_new_primary - || AP_HAL::timeout_expired(last_primary_gyro_update_us, now_us, PRIMARY_UPDATE_TIMEOUT_US)) { + 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); - is_primary_gyro = is_new_primary; - last_primary_gyro_update_us = now_us; + set_primary_accel(is_new_primary); + is_primary = is_new_primary; + last_primary_update_us = now_us; } } @@ -867,20 +867,6 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */ update_accel_filters(instance); } -void AP_InertialSensor_Backend::update_primary_accel() -{ - // timing changes need to be made in the bus thread in order to take effect which is - // why they are actioned here - const bool is_new_primary = accel_instance == _imu._primary_accel; - uint32_t now_us = AP_HAL::micros(); - if (is_primary_accel != is_new_primary - || AP_HAL::timeout_expired(last_primary_accel_update_us, now_us, PRIMARY_UPDATE_TIMEOUT_US)) { - set_primary_accel(is_new_primary); - is_primary_accel = is_new_primary; - last_primary_accel_update_us = now_us; - } -} - /* propagate filter changes from front end to backend */ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 443ebdb0fdce0..29b49910a1e01 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -160,11 +160,9 @@ class AP_InertialSensor_Backend // instance numbers of accel and gyro data uint8_t gyro_instance; - bool is_primary_gyro = true; - uint32_t last_primary_gyro_update_us; uint8_t accel_instance; - bool is_primary_accel = true; - uint32_t last_primary_accel_update_us; + 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__; @@ -297,10 +295,9 @@ class AP_InertialSensor_Backend void update_accel_filters(uint8_t instance) __RAMFUNC__; /* front end */ // catch updates to the primary gyro and accel - void update_primary_gyro() __RAMFUNC__; /* backend */ - void update_primary_accel() __RAMFUNC__; /* backend */ - virtual void set_primary_gyro(bool is_primary) {} - virtual void set_primary_accel(bool is_primary) {} + void update_primary() __RAMFUNC__; /* backend */ + virtual void set_primary_gyro(bool is_primary_gyro) {} + virtual void set_primary_accel(bool is_primary_accel) {} // support for updating filter at runtime uint16_t _last_accel_filter_hz; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index aec47327f7035..2c0ca4994b588 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -401,11 +401,11 @@ bool AP_InertialSensor_Invensensev3::update() return true; } -void AP_InertialSensor_Invensensev3::set_primary_gyro(bool is_primary) +void AP_InertialSensor_Invensensev3::set_primary_gyro(bool _is_primary) { #if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED if (_imu.is_dynamic_fifo_enabled(gyro_instance)) { - if (is_primary) { + 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()); @@ -570,7 +570,7 @@ 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 - if (is_primary_gyro) { + if (is_primary) { dev->adjust_periodic_callback(periodic_handle, backend_period_us); } From 88f4b22e4de650650a86c095f40176662df5914c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 19 Dec 2024 10:18:09 +0000 Subject: [PATCH 7/9] AP_InertialSensor: remove dynamic fifo mask --- libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h | 4 ---- libraries/AP_InertialSensor/FastRateBuffer.cpp | 3 +-- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h b/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h index b7305e76964a0..8bf45fecab7f0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h @@ -7,7 +7,3 @@ #ifndef AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED #define AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED (AP_INERTIALSENSOR_ENABLED && HAL_INS_RATE_LOOP && AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduCopter)) #endif - -#ifndef AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK -#define AP_INERTIALSENSOR_DYNAMIC_FIFO_MASK 127 -#endif diff --git a/libraries/AP_InertialSensor/FastRateBuffer.cpp b/libraries/AP_InertialSensor/FastRateBuffer.cpp index b47450493d41e..44ef58513982e 100644 --- a/libraries/AP_InertialSensor/FastRateBuffer.cpp +++ b/libraries/AP_InertialSensor/FastRateBuffer.cpp @@ -85,8 +85,7 @@ bool AP_InertialSensor::is_dynamic_fifo_enabled(uint8_t instance) const if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) { return false; } - return ((1U<use_rate_loop_gyro_samples(); } From c9a7d1f8129c4f3b3e07fe399eaffa4830c438df Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 19 Dec 2024 18:10:37 +0000 Subject: [PATCH 8/9] AP_InertialSensor: remove rate config include --- libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp | 1 - libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index ee71ae48d14f2..18eb0a2080fa5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -26,7 +26,6 @@ #include #include -#include "AP_InertialSensor_rate_config.h" #include "AP_InertialSensor_Invensense.h" #include diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp index f234275c395e1..b8279d339fd62 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.cpp @@ -23,7 +23,6 @@ #include -#include "AP_InertialSensor_rate_config.h" #include "AP_InertialSensor_Invensensev2.h" extern const AP_HAL::HAL& hal; From 631c78ac1c3e6065702d03389d5cbd23cf9494ce Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 21 Dec 2024 20:18:15 +0000 Subject: [PATCH 9/9] AP_InertialSensor: only update the primary once. scale FIFO reads to 2x loop rate. only increase FIFO buffer when compiled with fast rate --- .../AP_InertialSensor_Backend.cpp | 5 ++--- .../AP_InertialSensor_Backend.h | 3 +-- .../AP_InertialSensor_Invensensev3.cpp | 21 ++++++++++++++++--- .../AP_InertialSensor_Invensensev3.h | 2 +- 4 files changed, 22 insertions(+), 9 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 2aeffb24cb8c7..01ab1528b79fc 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -813,12 +813,11 @@ 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; + 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_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; } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 29b49910a1e01..f98925dd10123 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index 2c0ca4994b588..5b6d1162f4e03 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/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 _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 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h index 2c20db6914be7..2ea558b3fddda 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/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,