From adaed75a0d76c33ee3deee2478cf0cc6d8ea7f28 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Mon, 19 Sep 2022 16:08:47 +0100 Subject: [PATCH] AP_Vehicle: make sure notches are not spurious disabled when not using throttle notch --- libraries/AP_Vehicle/AP_Vehicle.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index ad25978652..5c4ecb1a34 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -459,11 +459,6 @@ void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch ¬ch) const float min_ratio = notch.params.freq_min_ratio(); const AP_Motors* motors = AP::motors(); - if (motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN) { - notch.set_inactive(true); - } else { - notch.set_inactive(false); - } const float motors_throttle = motors != nullptr ? MAX(0,motors->get_throttle_out()) : 0; float throttle_freq = ref_freq * MAX(min_ratio, sqrtf(motors_throttle / ref)); @@ -485,6 +480,13 @@ void AP_Vehicle::update_dynamic_notch(AP_InertialSensor::HarmonicNotch ¬ch) return; } + const AP_Motors* motors = AP::motors(); + if (motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN) { + notch.set_inactive(true); + } else { + notch.set_inactive(false); + } + switch (notch.params.tracking_mode()) { case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle