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

17
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 // run notch update at either loop rate or 200Hz
void AP_Vehicle::update_dynamic_notch_at_specified_rate() void AP_Vehicle::update_dynamic_notch_at_specified_rate()
{ {
if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::LoopRateUpdate)) { for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
update_dynamic_notch(); if (ins.has_harmonic_option(i, HarmonicNotchFilterParams::Options::LoopRateUpdate)) {
return; update_dynamic_notch(i);
} } else {
// decimated update at 200Hz // decimated update at 200Hz
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
if (now - _last_notch_update_ms > 5) { if (now - _last_notch_update_ms[i] > 5) {
_last_notch_update_ms = now; _last_notch_update_ms[i] = now;
update_dynamic_notch(); update_dynamic_notch(i);
}
}
} }
} }

4
libraries/AP_Vehicle/AP_Vehicle.h
View File

@ -248,7 +248,7 @@ public:
#endif // AP_SCRIPTING_ENABLED #endif // AP_SCRIPTING_ENABLED
// update the harmonic notch // 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: // zeroing the RC outputs can prevent unwanted motor movement:
virtual bool should_zero_rc_outputs_on_reboot() const { return false; } 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 bool likely_flying; // true if vehicle is probably flying
uint32_t _last_flying_ms; // time when likely_flying last went true 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; static AP_Vehicle *_singleton;