Plane: added dynamic harmonic notch support

ArduPlane/ArduPlane.cpp

@@ -107,8 +107,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
     SCHED_TASK(landing_gear_update, 5, 50),
 #endif
 #if EFI_ENABLED
-    SCHED_TASK(efi_update,             10,    200)
+    SCHED_TASK(efi_update,             10,    200),
 #endif
+    SCHED_TASK(update_dynamic_notch,   50,    200),
 };
 
 constexpr int8_t Plane::_failsafe_priorities[7];

ArduPlane/Plane.h

@@ -916,6 +916,7 @@ private:
     void startup_INS_ground(void);
     bool should_log(uint32_t mask);
     int8_t throttle_percentage(void);
+    void update_dynamic_notch();
     bool auto_takeoff_check(void);
     void takeoff_calc_roll(void);
     void takeoff_calc_pitch(void);

ArduPlane/system.cpp

@@ -475,3 +475,45 @@ int8_t Plane::throttle_percentage(void)
     }
     return constrain_int16(throttle, -100, 100);
 }
+
+// update the harmonic notch filter center frequency dynamically
+void Plane::update_dynamic_notch()
+{
+    if (!ins.gyro_harmonic_notch_enabled()) {
+        return;
+    }
+    const float ref_freq = ins.get_gyro_harmonic_notch_center_freq_hz();
+    const float ref = ins.get_gyro_harmonic_notch_reference();
+
+    if (is_zero(ref)) {
+        ins.update_harmonic_notch_freq_hz(ref_freq);
+        return;
+    }
+
+    switch (ins.get_gyro_harmonic_notch_tracking_mode()) {
+    case HarmonicNotchDynamicMode::UpdateThrottle: // throttle based tracking
+            // set the harmonic notch filter frequency approximately scaled on motor rpm implied by throttle
+            if (quadplane.available()) {
+                ins.update_harmonic_notch_freq_hz(ref_freq * MAX(1.0f, sqrtf(quadplane.motors->get_throttle_out() / ref)));
+            }
+            break;
+
+        case HarmonicNotchDynamicMode::UpdateRPM: // rpm sensor based tracking
+            if (rpm_sensor.healthy(0)) {
+                // set the harmonic notch filter frequency from the main rotor rpm
+                ins.update_harmonic_notch_freq_hz(MAX(ref_freq, rpm_sensor.get_rpm(0) * ref / 60.0f));
+            } else {
+                ins.update_harmonic_notch_freq_hz(ref_freq);
+            }
+            break;
+#ifdef HAVE_AP_BLHELI_SUPPORT
+        case HarmonicNotchDynamicMode::UpdateBLHeli: // BLHeli based tracking
+            ins.update_harmonic_notch_freq_hz(MAX(ref_freq, AP_BLHeli::get_singleton()->get_average_motor_frequency_hz() * ref));
+            break;
+#endif
+        case HarmonicNotchDynamicMode::Fixed: // static
+        default:
+            ins.update_harmonic_notch_freq_hz(ref_freq);
+            break;
+    }
+}