From 98f3a4b7d52b20b5969f429ad4692eaa1e4955e7 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 3 Jun 2024 18:38:08 +0100 Subject: [PATCH] AP_InertialSensor: allow backend filters to be updated independently from a separate thread --- .../AP_InertialSensor_Backend.cpp | 19 ++++++++++++++++++- .../AP_InertialSensor_Backend.h | 2 ++ 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index bef05d69ab..944b20661a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -781,6 +781,14 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */ _imu._new_gyro_data[instance] = false; } + update_gyro_filters(instance); +} + +/* + propagate filter changes from front end to backend + */ +void AP_InertialSensor_Backend::update_gyro_filters(uint8_t instance) /* front end */ +{ // possibly update filter frequency const float gyro_rate = _gyro_raw_sample_rate(instance); @@ -815,7 +823,16 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */ _publish_accel(instance, _imu._accel_filtered[instance]); _imu._new_accel_data[instance] = false; } - + + update_accel_filters(instance); +} + + +/* + propagate filter changes from front end to backend + */ +void AP_InertialSensor_Backend::update_accel_filters(uint8_t instance) /* front end */ +{ // possibly update filter frequency if (_last_accel_filter_hz != _accel_filter_cutoff()) { _imu._accel_filter[instance].set_cutoff_frequency(_accel_raw_sample_rate(instance), _accel_filter_cutoff()); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index d6a3b5bdf0..00d149e149 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -276,9 +276,11 @@ protected: // common gyro update function for all backends void update_gyro(uint8_t instance) __RAMFUNC__; /* front end */ + void update_gyro_filters(uint8_t instance) __RAMFUNC__; /* front end */ // common accel update function for all backends void update_accel(uint8_t instance) __RAMFUNC__; /* front end */ + void update_accel_filters(uint8_t instance) __RAMFUNC__; /* front end */ // support for updating filter at runtime uint16_t _last_accel_filter_hz;