yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_angular_velocity: check filter based on time threshold instead of samples

Browse Source 
- fixes #14303
This commit is contained in:
Daniel Agar
2020-04-08 14:21:11 -04:00
parent 276b2cad5a
commit 7b4f6b6918
2 changed files with 36 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
View File

@@ -83,30 +83,30 @@ void VehicleAcceleration::Stop()
void VehicleAcceleration::CheckFilters() void VehicleAcceleration::CheckFilters()
{ {
// check filter periodically (roughly once every 1-3 seconds depending on sensor configuration) if (_interval_count > 1000) {
if (_interval_count > 2500) { bool reset_filters = false;
bool sample_rate_changed = false;
// calculate sensor update rate // calculate sensor update rate
const float sample_interval_avg = _interval_sum / _interval_count; const float sample_interval_avg = _interval_sum / _interval_count;
if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) { 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)) { // check if sample rate error is greater than 1%
_update_rate_hz = update_rate_hz; if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
reset_filters = true;
// 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;
}
} }
} }
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); PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
_filter_sample_rate = _update_rate_hz; _filter_sample_rate = _update_rate_hz;

40
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@@ -86,35 +86,41 @@ void VehicleAngularVelocity::Stop()
void VehicleAngularVelocity::CheckFilters() void VehicleAngularVelocity::CheckFilters()
{ {
// check filter periodically (roughly once every 1-3 seconds depending on sensor configuration) if (_interval_count > 1000) {
if (_interval_count > 2500) { bool reset_filters = false;
bool sample_rate_changed = false;
// calculate sensor update rate // calculate sensor update rate
const float sample_interval_avg = _interval_sum / _interval_count; const float sample_interval_avg = _interval_sum / _interval_count;
if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) { 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)) { // check if sample rate error is greater than 1%
_update_rate_hz = update_rate_hz; if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
reset_filters = true;
// 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;
}
} }
} }
const bool lp_velocity_updated = (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f); if (!reset_filters) {
const bool notch_updated = ((fabsf(_notch_filter_velocity.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f) // gyro low pass cutoff frequency changed
|| (fabsf(_notch_filter_velocity.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f)); 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()) > // gyro notch filter frequency or bandwidth changed
0.01f); 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); PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
_filter_sample_rate = _update_rate_hz; _filter_sample_rate = _update_rate_hz;