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

sensors/vehicle_angular_velocity: small FFT notch update simplification

This commit is contained in:
Daniel Agar 2021-11-07 16:38:42 -05:00
parent 47afab62e7
commit e0d9ccdc18
1 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -543,18 +543,17 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
const float peak_freq = peak_frequencies[axis][peak];
if (PX4_ISFINITE(peak_freq) && (peak_freq > peak_freq_min) && (peak_freq < peak_freq_max)) {
// force reset if the notch frequency jumps significantly
if (fabsf(nf.getNotchFreq() - peak_freq) > bandwidth) {
reset = true;
}
const float notch_freq_diff = fabsf(nf.getNotchFreq() - peak_freq);
// update filter parameters if frequency changed or forced
if (force || (fabsf(nf.getNotchFreq() - peak_freq) > FLT_EPSILON)) {
if (force || reset || (notch_freq_diff > 0.1f)) {
nf.setParameters(_filter_sample_rate_hz, peak_freq, bandwidth);
perf_count(_dynamic_notch_filter_fft_update_perf);
}
if (force || reset) {
// force reset if the notch frequency jumps significantly
if (force || reset || (notch_freq_diff > bandwidth)) {
const Vector3f reset_angular_velocity{GetResetAngularVelocity()};
nf.reset(reset_angular_velocity(axis));
}