Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Vehicle: make sure notches are not spurious disabled when not using throttle notch

This commit is contained in:
Andy Piper 2022-09-19 16:08:47 +01:00 committed by Andrew Tridgell
parent 5cad520acf
commit c86f113bf9
1 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -459,11 +459,6 @@ void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch &notch)
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 &notch)
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