diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 7df1ae4e75..3911823944 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -52,8 +52,6 @@ VehicleAcceleration::VehicleAcceleration() : VehicleAcceleration::~VehicleAcceleration() { Stop(); - - perf_free(_interval_perf); } bool VehicleAcceleration::Start() @@ -85,40 +83,40 @@ void VehicleAcceleration::Stop() void VehicleAcceleration::CheckFilters() { - if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) { - _filter_check_last = hrt_absolute_time(); + // check filter periodically (roughly once every 1-3 seconds depending on sensor configuration) + if (_interval_count > 2500) { + bool sample_rate_changed = false; // calculate sensor update rate - const float sample_interval_avg = perf_mean(_interval_perf); + const float sample_interval_avg = _interval_sum / _interval_count; if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) { - const float update_rate_hz = 1.0f / sample_interval_avg; + const float update_rate_hz = 1.e6f / sample_interval_avg; if ((fabsf(update_rate_hz) > 0.0f) && PX4_ISFINITE(update_rate_hz)) { _update_rate_hz = update_rate_hz; // check if sample rate error is greater than 1% if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { - ++_sample_rate_incorrect_count; + sample_rate_changed = true; } } } - const bool sample_rate_updated = (_sample_rate_incorrect_count > 50); const bool lp_updated = (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.01f); - if (sample_rate_updated || lp_updated) { - PX4_INFO("updating filter, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz); + if (sample_rate_changed || lp_updated) { + PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz); _filter_sample_rate = _update_rate_hz; // update software low pass filters _lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get()); _lp_filter.reset(_acceleration_prev); - - // reset state - _sample_rate_incorrect_count = 0; } + + // reset sample interval accumulator + _timestamp_sample_last = 0; } } @@ -167,8 +165,8 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force) _corrections.set_device_id(report.device_id); - // reset sample rate monitor - _sample_rate_incorrect_count = 0; + // reset sample interval accumulator on sensor change + _timestamp_sample_last = 0; return true; } @@ -218,7 +216,17 @@ void VehicleAcceleration::Run() if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) { if (sensor_updated) { - perf_count_interval(_interval_perf, sensor_data.timestamp_sample); + // collect sample interval average for filters + if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) { + _interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last); + _interval_count++; + + } else { + _interval_sum = 0.f; + _interval_count = 0.f; + } + + _timestamp_sample_last = sensor_data.timestamp_sample; } CheckFilters(); diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 4617e72a6a..288cdff45a 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -90,25 +90,25 @@ private: SensorCorrections _corrections; - perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; - matrix::Vector3f _bias{0.f, 0.f, 0.f}; matrix::Vector3f _acceleration_prev{0.f, 0.f, 0.f}; hrt_abstime _last_publish{0}; - hrt_abstime _filter_check_last{0}; 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] */ math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.0f}; float _filter_sample_rate{kInitialRateHz}; - int _sample_rate_incorrect_count{0}; uint32_t _selected_sensor_device_id{0}; uint8_t _selected_sensor_sub_index{0}; + hrt_abstime _timestamp_sample_last{0}; + float _interval_sum{0.f}; + float _interval_count{0.f}; + DEFINE_PARAMETERS( (ParamFloat) _param_imu_accel_cutoff ) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 1cab9af024..f84cf6d8b6 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -55,8 +55,6 @@ VehicleAngularVelocity::VehicleAngularVelocity() : VehicleAngularVelocity::~VehicleAngularVelocity() { Stop(); - - perf_free(_interval_perf); } bool VehicleAngularVelocity::Start() @@ -88,28 +86,27 @@ void VehicleAngularVelocity::Stop() void VehicleAngularVelocity::CheckFilters() { - if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) { - _filter_check_last = hrt_absolute_time(); + // check filter periodically (roughly once every 1-3 seconds depending on sensor configuration) + if (_interval_count > 2500) { + bool sample_rate_changed = false; // calculate sensor update rate - const float sample_interval_avg = perf_mean(_interval_perf); + const float sample_interval_avg = _interval_sum / _interval_count; if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) { - const float update_rate_hz = 1.0f / sample_interval_avg; + const float update_rate_hz = 1.e6f / sample_interval_avg; if ((fabsf(update_rate_hz) > 0.0f) && PX4_ISFINITE(update_rate_hz)) { _update_rate_hz = update_rate_hz; // check if sample rate error is greater than 1% if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { - ++_sample_rate_incorrect_count; + sample_rate_changed = true; } } } - const bool sample_rate_updated = (_sample_rate_incorrect_count > 50); - const bool lp_velocity_updated = (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f); const bool notch_updated = ((fabsf(_notch_filter_velocity.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f) || (fabsf(_notch_filter_velocity.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f)); @@ -117,8 +114,8 @@ void VehicleAngularVelocity::CheckFilters() const bool lp_acceleration_updated = (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.01f); - if (sample_rate_updated || lp_velocity_updated || notch_updated || lp_acceleration_updated) { - PX4_INFO("updating filter, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz); + if (sample_rate_changed || lp_velocity_updated || notch_updated || lp_acceleration_updated) { + PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz); _filter_sample_rate = _update_rate_hz; // update software low pass filters @@ -130,10 +127,10 @@ void VehicleAngularVelocity::CheckFilters() _lp_filter_acceleration.set_cutoff_frequency(_filter_sample_rate, _param_imu_dgyro_cutoff.get()); _lp_filter_acceleration.reset(_angular_acceleration_prev); - - // reset state - _sample_rate_incorrect_count = 0; } + + // reset sample interval accumulator + _timestamp_sample_last = 0; } } @@ -182,8 +179,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force) _corrections.set_device_id(report.device_id); - // reset sample rate monitor - _sample_rate_incorrect_count = 0; + // reset sample interval accumulator on sensor change + _timestamp_sample_last = 0; return true; } @@ -233,7 +230,17 @@ void VehicleAngularVelocity::Run() if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) { if (sensor_updated) { - perf_count_interval(_interval_perf, sensor_data.timestamp_sample); + // collect sample interval average for filters + if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) { + _interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last); + _interval_count++; + + } else { + _interval_sum = 0.f; + _interval_count = 0.f; + } + + _timestamp_sample_last = sensor_data.timestamp_sample; } // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index b1669db492..e426d28db6 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -93,8 +93,6 @@ private: SensorCorrections _corrections; - perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; - matrix::Vector3f _bias{0.f, 0.f, 0.f}; matrix::Vector3f _angular_acceleration_prev{0.f, 0.f, 0.f}; @@ -102,7 +100,6 @@ private: hrt_abstime _timestamp_sample_prev{0}; hrt_abstime _last_publish{0}; - hrt_abstime _filter_check_last{0}; 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] */ @@ -114,11 +111,14 @@ private: math::LowPassFilter2pVector3f _lp_filter_acceleration{kInitialRateHz, 10.0f}; float _filter_sample_rate{kInitialRateHz}; - int _sample_rate_incorrect_count{0}; uint32_t _selected_sensor_device_id{0}; uint8_t _selected_sensor_sub_index{0}; + hrt_abstime _timestamp_sample_last{0}; + float _interval_sum{0.f}; + float _interval_count{0.f}; + DEFINE_PARAMETERS( (ParamFloat) _param_imu_gyro_cutoff, (ParamFloat) _param_imu_gyro_nf_freq,