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

AP_Vehicle: correct update_dynamic_notch_at_specified_rate()

This commit is contained in:
Andy Piper 2021-11-16 07:24:40 +00:00 committed by Randy Mackay
parent 944471bc30
commit 039a556771
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -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();
}
}