AP_GyroFFT: add notch tuning functions

calculate averaged FFT frequency and enable notch
scale notch frequency down to FFT_MINHZ
calculate harmonics for averaging correctly
expose enable flag

libraries/AP_GyroFFT/AP_GyroFFT.cpp

@@ -614,6 +614,14 @@ void AP_GyroFFT::update_freq_hover(float dt, float throttle_out)
     if (!analysis_enabled()) {
         return;
     }
+
+    // throttle averaging for average fft calculation
+    if (is_zero(_avg_throttle_out)) {
+        _avg_throttle_out = throttle_out;
+    } else {
+        _avg_throttle_out = constrain_float(_avg_throttle_out + (dt / (10.0f + dt)) * (throttle_out - _avg_throttle_out), 0.01f, 0.9f);
+    }
+
     // we have chosen to constrain the hover frequency to be within the range reachable by the third order expo polynomial.
     _freq_hover_hz = constrain_float(_freq_hover_hz + (dt / (10.0f + dt)) * (get_weighted_noise_center_freq_hz() - _freq_hover_hz), _fft_min_hz, _fft_max_hz);
     _bandwidth_hover_hz = constrain_float(_bandwidth_hover_hz + (dt / (10.0f + dt)) * (get_weighted_noise_center_bandwidth_hz() - _bandwidth_hover_hz), 0, _fft_max_hz * 0.5f);
@@ -633,6 +641,69 @@ void AP_GyroFFT::save_params_on_disarm()
     _throttle_ref.save();
 }
 
+    // notch tuning
+void AP_GyroFFT::start_notch_tune()
+{
+    if (!analysis_enabled()) {
+        return;
+    }
+
+    if (!hal.dsp->fft_start_average(_state)) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "FFT: Unable to start FFT averaging");
+    }
+    _avg_throttle_out = 0.0f;
+}
+
+void AP_GyroFFT::stop_notch_tune()
+{
+    if (!analysis_enabled()) {
+        return;
+    }
+
+    float freqs[FrequencyPeak::MAX_TRACKED_PEAKS] {};
+
+    uint16_t numpeaks = hal.dsp->fft_stop_average(_state, _config._fft_start_bin, _config._fft_end_bin, freqs);
+
+    if (numpeaks == 0) {
+        return;
+    }
+
+    float harmonic = freqs[0];
+    float harmonic_fit = 100.0f;
+
+    for (uint8_t i = 1; i < numpeaks; i++) {
+        if (freqs[i] < harmonic) {
+            const float fit = 100.0f * fabsf(harmonic - freqs[i] * _harmonic_multiplier) / harmonic;
+            if (isfinite(fit) && fit < _harmonic_fit && fit < harmonic_fit) {
+                harmonic = freqs[i];
+                harmonic_fit = fit;
+            }
+        }
+    }
+
+    gcs().send_text(MAV_SEVERITY_INFO, "FFT: Found peaks at %.1f/%.1f/%.1fHz", freqs[0], freqs[1], freqs[2]);
+    gcs().send_text(MAV_SEVERITY_NOTICE, "FFT: Selected %.1fHz with fit %.1f%%\n", harmonic, is_equal(harmonic_fit, 100.0f) ? 100.0f : 100.0f - harmonic_fit);
+
+    // if we don't have a throttle value then all bets are off
+    if (is_zero(_avg_throttle_out) || is_zero(harmonic)) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "FFT: Unable to calculate notch: need stable hover");
+        return;
+    }
+
+    // pick a ref which means the notch covers all the way down to FFT_MINHZ
+    const float thr_ref = _avg_throttle_out * sq((float)_fft_min_hz.get() / harmonic);
+
+    if (!_ins->setup_throttle_gyro_harmonic_notch((float)_fft_min_hz.get(), thr_ref)) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "FFT: Unable to set throttle notch with %.1fHz/%.2f",
+            (float)_fft_min_hz.get(), thr_ref);
+        // save results to FFT slots
+        _throttle_ref = thr_ref;
+        _freq_hover_hz = harmonic;
+    } else {
+        gcs().send_text(MAV_SEVERITY_NOTICE, "FFT: Notch frequency %.1fHz and ref %.2f selected", (float)_fft_min_hz.get(), thr_ref);
+    }
+}
+
 // return the noise peak that is being tracked
 // called from main thread
 AP_GyroFFT::FrequencyPeak AP_GyroFFT::get_tracked_noise_peak() const
@@ -704,7 +775,7 @@ float AP_GyroFFT::get_weighted_noise_center_freq_hz() const
     if (!_health) {
 #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
         AP_Motors* motors = AP::motors();
-        if (motors != nullptr) {
+        if (motors != nullptr && !is_zero(_throttle_ref)) {
             // FFT is not healthy, fallback to throttle-based estimate
             return constrain_float(_fft_min_hz * MAX(1.0f, sqrtf(motors->get_throttle_out() / _throttle_ref)), _fft_min_hz, _fft_max_hz);
         }

libraries/AP_GyroFFT/AP_GyroFFT.h

@@ -61,6 +61,8 @@ public:
     void update_thread();
     // start the update thread
     bool start_update_thread();
+    // is the subsystem enabled
+    bool enabled() const { return _enable; }
 
     // check at startup that standard frequencies can be detected
     bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len);
@@ -72,6 +74,9 @@ public:
     void save_params_on_disarm();
     // dynamically enable or disable the analysis through the aux switch
     void set_analysis_enabled(bool enabled) { _analysis_enabled = enabled; };
+    // notch tuning
+    void start_notch_tune();
+    void stop_notch_tune();
 
     // detected peak frequency filtered at 1/3 the update rate
     const Vector3f& get_noise_center_freq_hz() const { return get_noise_center_freq_hz(FrequencyPeak::CENTER); }
@@ -282,6 +287,8 @@ private:
     uint8_t _health;
     // engine health on roll/pitch/yaw
     Vector3<uint8_t> _rpy_health;
+    // averaged throttle output over averaging period
+    float _avg_throttle_out;
 
     // smoothing filter on the output
     MedianLowPassFilter3dFloat _center_freq_filter[FrequencyPeak::MAX_TRACKED_PEAKS];