aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Vehicle: correct update_dynamic_notch_at_specified_rate()

Browse Source
This commit is contained in:
Andy Piper 2021-11-16 07:24:40 +00:00 committed by Peter Hall
parent ac263e5659
commit 77acf9bcc2
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

@ -383,12 +383,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();
}
}