AP_Vehicle: support two full harmonic notch filters

libraries/AP_Vehicle/AP_Vehicle.cpp

@@ -388,17 +388,18 @@ bool AP_Vehicle::is_crashed() const
 // run notch update at either loop rate or 200Hz
 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);
+            }
+        }
     }
 }
 

libraries/AP_Vehicle/AP_Vehicle.h

@@ -248,7 +248,7 @@ public:
 #endif // AP_SCRIPTING_ENABLED
 
     // 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:
     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
     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;