From 7b4f6b6918b11da4910c3192647a53375da63cbd Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 8 Apr 2020 14:21:11 -0400 Subject: [PATCH] sensors/vehicle_angular_velocity: check filter based on time threshold instead of samples - fixes #14303 --- .../VehicleAcceleration.cpp | 26 ++++++------ .../VehicleAngularVelocity.cpp | 40 +++++++++++-------- 2 files changed, 36 insertions(+), 30 deletions(-) diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 389783f5c3..f14e53792b 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -83,30 +83,30 @@ void VehicleAcceleration::Stop() void VehicleAcceleration::CheckFilters() { - // check filter periodically (roughly once every 1-3 seconds depending on sensor configuration) - if (_interval_count > 2500) { - bool sample_rate_changed = false; + if (_interval_count > 1000) { + bool reset_filters = false; // calculate sensor update rate 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.e6f / sample_interval_avg; + _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_changed = true; - } + // check if sample rate error is greater than 1% + if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { + reset_filters = true; } } - const bool lp_updated = (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.01f); + if (!reset_filters) { + // accel low pass cutoff frequency changed + if (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.01f) { + reset_filters = true; + } + } - if (sample_rate_changed || lp_updated) { + if (reset_filters) { PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz); _filter_sample_rate = _update_rate_hz; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index d3ab17aedb..32fc701b9d 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -86,35 +86,41 @@ void VehicleAngularVelocity::Stop() void VehicleAngularVelocity::CheckFilters() { - // check filter periodically (roughly once every 1-3 seconds depending on sensor configuration) - if (_interval_count > 2500) { - bool sample_rate_changed = false; + if (_interval_count > 1000) { + bool reset_filters = false; // calculate sensor update rate 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.e6f / sample_interval_avg; + _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_changed = true; - } + // check if sample rate error is greater than 1% + if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { + reset_filters = true; } } - 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)); + if (!reset_filters) { + // gyro low pass cutoff frequency changed + if (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f) { + reset_filters = true; + } - const bool lp_acceleration_updated = (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > - 0.01f); + // gyro notch filter frequency or bandwidth changed + if ((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)) { + reset_filters = true; + } - if (sample_rate_changed || lp_velocity_updated || notch_updated || lp_acceleration_updated) { + // gyro derivative low pass cutoff changed + if (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.01f) { + reset_filters = true; + } + } + + if (reset_filters) { PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz); _filter_sample_rate = _update_rate_hz;