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

AP_RPM: support two full harmonic notch filters

This commit is contained in:
Andrew Tridgell 2022-04-13 13:36:18 +10:00 committed by Randy Mackay
parent ead011c615
commit 8dbf3f39cb
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_RPM/RPM_HarmonicNotch.cpp
View File

@ -32,11 +32,14 @@ AP_RPM_HarmonicNotch::AP_RPM_HarmonicNotch(AP_RPM &_ap_rpm, uint8_t _instance, A
void AP_RPM_HarmonicNotch::update(void)
{
AP_InertialSensor& ins = AP::ins();
if (ins.get_gyro_harmonic_notch_tracking_mode() != HarmonicNotchDynamicMode::Fixed) {
state.rate_rpm = ins.get_gyro_dynamic_notch_center_freq_hz() * 60.0f;
state.rate_rpm *= ap_rpm._params[state.instance].scaling;
state.signal_quality = 0.5f;
state.last_reading_ms = AP_HAL::millis();
for (uint8_t i=0; i<HAL_INS_NUM_HARMONIC_NOTCH_FILTERS; i++) {
if (ins.gyro_harmonic_notch_enabled(i) &&
ins.get_gyro_harmonic_notch_tracking_mode(i) != HarmonicNotchDynamicMode::Fixed) {
state.rate_rpm = ins.get_gyro_dynamic_notch_center_freq_hz(i) * 60.0f;
state.rate_rpm *= ap_rpm._params[state.instance].scaling;
state.signal_quality = 0.5f;
state.last_reading_ms = AP_HAL::millis();
}
}
}