diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 9fe653c755..1b2b176a8e 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -103,6 +103,22 @@ void VehicleAngularVelocity::CheckFilters() if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { reset_filters = true; } + + if (reset_filters || (_required_sample_updates == 0)) { + if (_param_imu_gyro_rate_max.get() > 0) { + // determine number of sensor samples that will get closest to the desired rate + const float configured_interval_us = 1e6f / _param_imu_gyro_rate_max.get(); + const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f, + (float)sensor_gyro_s::ORB_QUEUE_LENGTH); + + _sensor_sub[_selected_sensor_sub_index].set_required_updates(samples); + _required_sample_updates = samples; + + } else { + _sensor_sub[_selected_sensor_sub_index].set_required_updates(1); + _required_sample_updates = 1; + } + } } if (!reset_filters) { @@ -190,6 +206,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) // reset sample interval accumulator on sensor change _timestamp_sample_last = 0; + _required_sample_updates = 0; return true; } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index df524998d9..2b82281436 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -103,6 +103,8 @@ private: static constexpr const float kInitialRateHz{1000.0f}; /**< sensor update rate used for initialization */ float _update_rate_hz{kInitialRateHz}; /**< current rate-controller loop update rate in [Hz] */ + uint8_t _required_sample_updates{0}; /**< number or sensor publications required for configured rate */ + // angular velocity filters math::LowPassFilter2pVector3f _lp_filter_velocity{kInitialRateHz, 30.0f}; math::NotchFilter _notch_filter_velocity{};