AP_GyroFFT: allow all three peaks to be returned so that we can have three independent harmonic notches

log dynamic harmonic notch per-peak
move health check into update() and make accessors const
use AP_Vehicle log function when necessary

libraries/AP_GyroFFT/AP_GyroFFT.cpp

@@ -24,6 +24,7 @@
 #include <Filter/HarmonicNotchFilter.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Arming/AP_Arming.h>
+#include <AP_Vehicle/AP_Vehicle.h>
 #include <stdio.h>
 
 extern const AP_HAL::HAL& hal;
@@ -352,6 +353,23 @@ void AP_GyroFFT::update()
 
     _config._analysis_enabled = _analysis_enabled;
     _global_state = _thread_state;
+
+    // calculate health based on being 5 frames behind, SITL needs longer
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    const uint32_t output_delay = _frame_time_ms * FFT_MAX_MISSED_UPDATES * 2;
+#else
+    const uint32_t output_delay = _frame_time_ms * FFT_MAX_MISSED_UPDATES;
+#endif
+    uint32_t now = AP_HAL::millis();
+    _rpy_health.x = (now - _global_state._health_ms.x <= output_delay);
+    _rpy_health.y = (now - _global_state._health_ms.y <= output_delay);
+    _rpy_health.z = (now - _global_state._health_ms.z <= output_delay);
+
+    if (!_rpy_health.x && !_rpy_health.y) {
+        _health = 0;
+    } else {
+        _health = MIN(_global_state._health, _harmonics);
+    }
 }
 
 // analyse gyro data using FFT, returns number of samples still held
@@ -625,20 +643,13 @@ AP_GyroFFT::FrequencyPeak AP_GyroFFT::get_tracked_noise_peak() const
 
 // return an average center frequency weighted by bin energy
 // called from main thread
-float AP_GyroFFT::get_weighted_noise_center_freq_hz()
+float AP_GyroFFT::get_weighted_noise_center_freq_hz() const
 {
     if (!analysis_enabled()) {
         return _fft_min_hz;
     }
-    // calculate health based on being 5 frames behind, SITL needs longer
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    const uint32_t output_delay = _frame_time_ms * FFT_MAX_MISSED_UPDATES * 2;
-#else
-    const uint32_t output_delay = _frame_time_ms * FFT_MAX_MISSED_UPDATES;
-#endif
-    uint32_t now = AP_HAL::millis();
-    if (now - _global_state._health_ms.x > output_delay && now - _global_state._health_ms.y > output_delay) {
-        _health = 0;
+
+    if (!_health) {
 #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
         AP_Motors* motors = AP::motors();
         if (motors != nullptr) {
@@ -648,22 +659,62 @@ float AP_GyroFFT::get_weighted_noise_center_freq_hz()
 #endif
     }
 
-    _health = _global_state._health;
-
     const FrequencyPeak peak = get_tracked_noise_peak();
     // pitch was good or required, roll was not, use pitch only
-    if (now - _global_state._health_ms.x > output_delay || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_PITCH) {
+    if (!_rpy_health.x || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_PITCH) {
         return get_noise_center_freq_hz(peak).y;
     }
     // roll was good or required, pitch was not, use roll only
-    if (now - _global_state._health_ms.y > output_delay || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_ROLL) {
+    if (!_rpy_health.y || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_ROLL) {
         return get_noise_center_freq_hz(peak).x;
     }
 
     return calculate_weighted_freq_hz(get_center_freq_energy(peak), get_noise_center_freq_hz(peak));
 }
 
-float AP_GyroFFT::calculate_weighted_freq_hz(const Vector3f& energy, const Vector3f& freq)
+// return all the center frequencies weighted by bin energy
+// called from main thread
+uint8_t AP_GyroFFT::get_weighted_noise_center_frequencies_hz(uint8_t num_freqs, float* freqs) const
+{
+    if (!analysis_enabled()) {
+        freqs[0] = _fft_min_hz;
+        return 1;
+    }
+
+    if (!_health) {
+#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+        AP_Motors* motors = AP::motors();
+        if (motors != nullptr) {
+            // FFT is not healthy, fallback to throttle-based estimate
+            freqs[0] = constrain_float(_fft_min_hz * MAX(1.0f, sqrtf(motors->get_throttle_out() / _throttle_ref)), _fft_min_hz, _fft_max_hz);
+            return 1;
+        }
+#endif
+    }
+
+    const uint8_t tracked_peaks = MIN(_health, num_freqs);
+    // pitch was good or required, roll was not, use pitch only
+    if (!_rpy_health.x || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_PITCH) {
+        for (uint8_t i = 0; i < tracked_peaks; i++) {
+            freqs[i] = get_noise_center_freq_hz(FrequencyPeak(i)).y;
+        }
+        return tracked_peaks;
+    }
+    // roll was good or required, pitch was not, use roll only
+    if (!_rpy_health.y || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_ROLL) {
+        for (uint8_t i = 0; i < tracked_peaks; i++) {
+            freqs[i] = get_noise_center_freq_hz(FrequencyPeak(i)).x;
+        }
+        return tracked_peaks;
+    }
+
+    for (uint8_t i = 0; i < tracked_peaks; i++) {
+        freqs[i] = calculate_weighted_freq_hz(get_center_freq_energy(FrequencyPeak(i)), get_noise_center_freq_hz(FrequencyPeak(i)));
+    }
+    return tracked_peaks;
+}
+
+float AP_GyroFFT::calculate_weighted_freq_hz(const Vector3f& energy, const Vector3f& freq) const
 {
     // there is generally a lot of high-energy, slightly lower frequency noise on yaw, however this
     // appears to be a second-order effect as only targetting pitch and roll (x & y) produces much cleaner output all round
@@ -696,8 +747,7 @@ void AP_GyroFFT::write_log_messages()
 {
     if (!analysis_enabled()) {
         // still want to see the harmonic notch values
-        AP::logger().Write(
-            "FTN", "TimeUS,DnF", "sz", "F-", "Qf", AP_HAL::micros64(), AP::ins().get_gyro_dynamic_notch_center_freq_hz());
+        AP::vehicle()->write_notch_log_messages();
         return;
     }
 
@@ -719,10 +769,12 @@ void AP_GyroFFT::write_log_messages()
         get_raw_noise_harmonic_fit().z,
         _health, _output_cycle_micros);
 
-    log_noise_peak(0, FrequencyPeak::CENTER);
+    const float* notches = _ins->get_gyro_dynamic_notch_center_frequencies_hz();
+
+    log_noise_peak(0, FrequencyPeak::CENTER, notches[0]);
     if (_harmonics > 1) {
-        log_noise_peak(1, FrequencyPeak::LOWER_SHOULDER);
-        log_noise_peak(2, FrequencyPeak::UPPER_SHOULDER);
+        log_noise_peak(1, FrequencyPeak::LOWER_SHOULDER, notches[1]);
+        log_noise_peak(2, FrequencyPeak::UPPER_SHOULDER, notches[2]);
     }
 
 #if DEBUG_FFT
@@ -747,6 +799,7 @@ void AP_GyroFFT::write_log_messages()
 // @Field: PkX: noise frequency of the peak on roll
 // @Field: PkY: noise frequency of the peak on pitch
 // @Field: PkZ: noise frequency of the peak on yaw
+// @Field: DnF: dynamic harmonic notch centre frequency
 // @Field: BwX: bandwidth of the peak freqency on roll where edges are determined by FFT_ATT_REF
 // @Field: BwY: bandwidth of the peak freqency on pitch where edges are determined by FFT_ATT_REF
 // @Field: BwZ: bandwidth of the peak freqency on yaw where edges are determined by FFT_ATT_REF
@@ -755,14 +808,15 @@ void AP_GyroFFT::write_log_messages()
 // @Field: EnZ: power spectral density bin energy of the peak on roll
 
 // write a single log message
-void AP_GyroFFT::log_noise_peak(uint8_t id, FrequencyPeak peak)
+void AP_GyroFFT::log_noise_peak(uint8_t id, FrequencyPeak peak, float notch)
 {
-    AP::logger().Write("FTN2", "TimeUS,Id,PkX,PkY,PkZ,BwX,BwY,BwZ,EnX,EnY,EnZ", "s#zzzzzz---", "F----------", "QBfffffffff",
+    AP::logger().Write("FTN2", "TimeUS,Id,PkX,PkY,PkZ,DnF,BwX,BwY,BwZ,EnX,EnY,EnZ", "s#zzzzzzz---", "F-----------", "QBffffffffff",
         AP_HAL::micros64(),
         id,
         get_noise_center_freq_hz(peak).x,
         get_noise_center_freq_hz(peak).y,
         get_noise_center_freq_hz(peak).z,
+        notch,
         get_noise_center_bandwidth_hz(peak).x,
         get_noise_center_bandwidth_hz(peak).y,
         get_noise_center_bandwidth_hz(peak).z,
@@ -773,7 +827,7 @@ void AP_GyroFFT::log_noise_peak(uint8_t id, FrequencyPeak peak)
 
 // return an average noise bandwidth weighted by bin energy
 // called from main thread
-float AP_GyroFFT::get_weighted_noise_center_bandwidth_hz()
+float AP_GyroFFT::get_weighted_noise_center_bandwidth_hz() const
 {
     if (!analysis_enabled()) {
         return 0.0f;

libraries/AP_GyroFFT/AP_GyroFFT.h

@@ -80,7 +80,9 @@ public:
     // energy of the background noise at the detected center frequency
     const Vector3f& get_noise_signal_to_noise_db() const { return _global_state._center_snr; }
     // detected peak frequency weighted by energy
-    float get_weighted_noise_center_freq_hz();
+    float get_weighted_noise_center_freq_hz() const;
+    // all detected peak frequencies weighted by energy
+    uint8_t get_weighted_noise_center_frequencies_hz(uint8_t num_freqs, float* freqs) const;
     // detected peak frequency
     const Vector3f& get_raw_noise_center_freq_hz() const { return _global_state._center_freq_hz; }
     // match between first and second harmonics
@@ -94,7 +96,7 @@ public:
     const Vector3f& get_noise_center_bandwidth_hz() const { return get_noise_center_bandwidth_hz(FrequencyPeak::CENTER); }
     const Vector3f& get_noise_center_bandwidth_hz(FrequencyPeak peak) const { return _global_state._center_bandwidth_hz_filtered[peak]; };
     // weighted detected peak bandwidth
-    float get_weighted_noise_center_bandwidth_hz();
+    float get_weighted_noise_center_bandwidth_hz() const;
     // log gyro fft messages
     void write_log_messages();
 
@@ -170,7 +172,7 @@ private:
         return (_thread_state._center_bandwidth_hz_filtered[peak][axis] = _center_bandwidth_filter[peak].apply(axis, value));
     }
     // write single log mesages
-    void log_noise_peak(uint8_t id, FrequencyPeak peak);
+    void log_noise_peak(uint8_t id, FrequencyPeak peak, float notch_freq);
     // calculate the peak noise frequency
     void calculate_noise(bool calibrating, const EngineConfig& config);
     // calculate noise peaks based on energy and history
@@ -186,7 +188,7 @@ private:
     // return the instantaneous peak that is closest to the target estimate peak
     FrequencyPeak find_closest_peak(const FrequencyPeak target, const DistanceMatrix& distance_matrix, uint8_t ignore = 0) const;
     // detected peak frequency weighted by energy
-    float calculate_weighted_freq_hz(const Vector3f& energy, const Vector3f& freq);
+    float calculate_weighted_freq_hz(const Vector3f& energy, const Vector3f& freq) const;
     // update the estimation of the background noise energy
     void update_ref_energy(uint16_t max_bin);
     // test frequency detection for all of the allowable bins
@@ -269,8 +271,10 @@ private:
     float _harmonic_multiplier;
     // searched harmonics - inferred from harmonic notch harmonics
     uint8_t _harmonics;
-    // engine health
+    // engine health in tracked peaks
     uint8_t _health;
+    // engine health on roll/pitch/yaw
+    Vector3<uint8_t> _rpy_health;
 
     // smoothing filter on the output
     MedianLowPassFilter3dFloat _center_freq_filter[FrequencyPeak::MAX_TRACKED_PEAKS];