From 8dbf3f39cbee5eb76a17681300edcf581f0d9dde Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 13 Apr 2022 13:36:18 +1000
Subject: [PATCH] AP_RPM: support two full harmonic notch filters

---
 libraries/AP_RPM/RPM_HarmonicNotch.cpp | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_RPM/RPM_HarmonicNotch.cpp b/libraries/AP_RPM/RPM_HarmonicNotch.cpp
index 883c45eb57..677ef01bae 100644
--- a/libraries/AP_RPM/RPM_HarmonicNotch.cpp
+++ b/libraries/AP_RPM/RPM_HarmonicNotch.cpp
@@ -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();
+        }
     }
 }