From 039a556771e6665b0f1f4274631a40c41019ad37 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 16 Nov 2021 07:24:40 +0000 Subject: [PATCH] AP_Vehicle: correct update_dynamic_notch_at_specified_rate() --- libraries/AP_Vehicle/AP_Vehicle.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index be0d47774e..1baf1f6592 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -340,12 +340,17 @@ void AP_Vehicle::write_notch_log_messages() const // run notch update at either loop rate or 200Hz void AP_Vehicle::update_dynamic_notch_at_specified_rate() { + if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::LoopRateUpdate)) { + update_dynamic_notch(); + return; + } + + // decimated update at 200Hz const uint32_t now = AP_HAL::millis(); - if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::LoopRateUpdate) - || now - _last_notch_update_ms > 5) { - update_dynamic_notch(); + if (now - _last_notch_update_ms > 5) { _last_notch_update_ms = now; + update_dynamic_notch(); } }