From 13107dfa6dee4589bbfa8ff2df6891d39b71fd2e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Apr 2022 13:52:42 +1000 Subject: [PATCH] Copter: support harmonic notch on 2nd RPM sensor --- ArduCopter/system.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 263a93026d5..36df04162d3 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -256,14 +256,17 @@ void Copter::update_dynamic_notch(uint8_t idx) #if RPM_ENABLED == ENABLED 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 / 60.0f)); + ins.update_harmonic_notch_freq_hz(idx, MAX(ref_freq, rpm * ref * (1.0/60))); } else { ins.update_harmonic_notch_freq_hz(idx, ref_freq); } break; + } #endif #if HAL_WITH_ESC_TELEM case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking