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

AP_Vehicle: support two full harmonic notch filters

This commit is contained in:
Andrew Tridgell 2022-04-13 13:36:18 +10:00
parent 3d025a7c5c
commit f9de6dff1c
2 changed files with 13 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -388,17 +388,18 @@ bool AP_Vehicle::is_crashed() 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;
}
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
if (ins.has_harmonic_option(i, HarmonicNotchFilterParams::Options::LoopRateUpdate)) {
update_dynamic_notch(i);
} else {
// decimated update at 200Hz
const uint32_t now = AP_HAL::millis();
// decimated update at 200Hz
const uint32_t now = AP_HAL::millis();
if (now - _last_notch_update_ms > 5) {
_last_notch_update_ms = now;
update_dynamic_notch();
if (now - _last_notch_update_ms[i] > 5) {
_last_notch_update_ms[i] = now;
update_dynamic_notch(i);
}
}
}
}

4
libraries/AP_Vehicle/AP_Vehicle.h
View File

@ -248,7 +248,7 @@ public:
#endif // AP_SCRIPTING_ENABLED
// update the harmonic notch
virtual void update_dynamic_notch() {};
virtual void update_dynamic_notch(uint8_t idx) {};
// zeroing the RC outputs can prevent unwanted motor movement:
virtual bool should_zero_rc_outputs_on_reboot() const { return false; }
@ -409,7 +409,7 @@ private:
bool likely_flying; // true if vehicle is probably flying
uint32_t _last_flying_ms; // time when likely_flying last went true
uint32_t _last_notch_update_ms; // last time update_dynamic_notch() was run
uint32_t _last_notch_update_ms[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS]; // last time update_dynamic_notch() was run
static AP_Vehicle *_singleton;