From b4d5aeacfee28c292e65e7c84d5ef01be9267d8d 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/AP_InertialSensor.cpp | 8 +++++ .../AP_InertialSensor/AP_InertialSensor.h | 1 + .../AP_InertialSensor_Backend.cpp | 34 ++++++++++++++++++- .../AP_InertialSensor_Backend.h | 9 +++++ .../AP_InertialSensor_Invensense.h | 1 + .../AP_InertialSensor_Invensensev2.h | 2 +- .../AP_InertialSensor_Invensensev3.h | 2 +- .../AP_InertialSensor_Logging.cpp | 5 +-- 8 files changed, 55 insertions(+), 7 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 6d254dccec9071..00a2d2b99b2690 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1896,6 +1896,7 @@ void AP_InertialSensor::update(void) _delta_velocity_valid[i] = false; _delta_angle_valid[i] = false; } + for (uint8_t i=0; i<_backend_count; i++) { _backends[i]->update(); } @@ -2798,6 +2799,13 @@ void AP_InertialSensor::force_save_calibration(void) } } +void AP_InertialSensor::update_backend_filters() +{ + for (uint8_t i=0; i<_backend_count; i++) { + _backends[i]->update_filters(); + } +} + namespace AP { AP_InertialSensor &ins() diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 30e60bec6c2313..76508e4665d91d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -263,6 +263,7 @@ class AP_InertialSensor : AP_AccelCal_Client AuxiliaryBus *get_auxiliary_bus(int16_t backend_id, uint8_t instance); void detect_backends(void); + void update_backend_filters(); // accel peak hold detector void set_accel_peak_hold(uint8_t instance, const Vector3f &accel); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index bef05d69ab01cb..3a062300948874 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -17,6 +17,13 @@ #define AP_HEATER_IMU_INSTANCE 0 #endif +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +// hal.console can be accessed from bus threads on ChibiOS +#define debug(fmt, args ...) do {hal.console->printf("IMU: " fmt "\n", ## args); } while(0) +#else +#define debug(fmt, args ...) do {printf("IMU: " fmt "\n", ## args); } while(0) +#endif + const extern AP_HAL::HAL& hal; AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) : @@ -781,6 +788,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 +830,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()); @@ -823,6 +847,14 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */ } } +void AP_InertialSensor_Backend::update_filters() +{ + WITH_SEMAPHORE(_sem); + + update_accel_filters(accel_instance); + update_gyro_filters(gyro_instance); +} + #if HAL_LOGGING_ENABLED bool AP_InertialSensor_Backend::should_log_imu_raw() const { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index d6a3b5bdf07897..29ef38729fa9ee 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -53,6 +53,13 @@ class AP_InertialSensor_Backend */ virtual bool update() = 0; /* front end */ + /* + * Update the filter parameters. Called by the frontend to propagate + * filter parameters to the frontend structure via the + * update_gyro_filters() and update_accel_filters() functions + */ + void update_filters() __RAMFUNC__; /* front end */ + /* * optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples */ @@ -276,9 +283,11 @@ class AP_InertialSensor_Backend // 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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h index 522dfd527d3c31..0a33a95198c3c4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.h @@ -46,6 +46,7 @@ class AP_InertialSensor_Invensense : public AP_InertialSensor_Backend /* update accel and gyro state */ bool update() override __RAMFUNC__; /* front end */ + void accumulate() override; /* front end */ /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h index b6e21d0034d1d5..3472cef647812f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev2.h @@ -42,7 +42,7 @@ class AP_InertialSensor_Invensensev2 : public AP_InertialSensor_Backend enum Rotation rotation); /* update accel and gyro state */ - bool update() override; + bool update() override __RAMFUNC__; void accumulate() override; /* diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h index b3946aa81dd471..4561d02b424dcc 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -24,7 +24,7 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend enum Rotation rotation); /* update accel and gyro state */ - bool update() override; + bool update() override __RAMFUNC__; void accumulate() override; void start() override; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp index 4fa6c6ba641183..5878b1ed38a82c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp @@ -193,10 +193,7 @@ void AP_InertialSensor::write_notch_log_messages() const notches[0]); } - // 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[AP::ahrs().get_primary_gyro_index()].log_notch_centers(i, now_us); } } #endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED