Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

sensors/vehicle_angular_velocity: small FFT notch update simplification

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+5 -6
View File
@@ -543,18 +543,17 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
const float peak_freq = peak_frequencies[axis][peak]; const float peak_freq = peak_frequencies[axis][peak];
if (PX4_ISFINITE(peak_freq) && (peak_freq > peak_freq_min) && (peak_freq < peak_freq_max)) { 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) { const float notch_freq_diff = fabsf(nf.getNotchFreq() - peak_freq);
reset = true;
}
// update filter parameters if frequency changed or forced // 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); nf.setParameters(_filter_sample_rate_hz, peak_freq, bandwidth);
perf_count(_dynamic_notch_filter_fft_update_perf); 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()}; const Vector3f reset_angular_velocity{GetResetAngularVelocity()};
nf.reset(reset_angular_velocity(axis)); nf.reset(reset_angular_velocity(axis));
} }