From fd00fef01a5558aa291e326791529e29cb12619e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:52:42 +1000 Subject: [PATCH] Plane: support harmonic notch on 2nd RPM sensor --- ArduPlane/system.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 9bf33a7320..5be4f6db9b 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -471,14 +471,17 @@ void Plane::update_dynamic_notch(uint8_t idx) break; case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking + case HarmonicNotchDynamicMode::UpdateRPM2: { + uint8_t sensor = (ins.get_gyro_harmonic_notch_tracking_mode(idx)==HarmonicNotchDynamicMode::UpdateRPM?0:1); float rpm; - if (rpm_sensor.get_rpm(0, rpm)) { + if (rpm_sensor.get_rpm(sensor, rpm)) { // set the harmonic notch filter frequency from the main rotor rpm ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref * (1/60.0))); } else { ins.update_harmonic_notch_freq_hz(idx, ref_freq); } break; + } #if HAL_WITH_ESC_TELEM case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking // set the harmonic notch filter frequency scaled on measured frequency