dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

- 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
Show Stats 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()
{
// 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;

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

@ -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;