From 9013432d2715de4611649fa90b33fc9f073244c9 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 30 Jul 2020 19:42:40 +0100 Subject: [PATCH] AP_GyroFFT: slew FFT frequency output --- libraries/AP_GyroFFT/AP_GyroFFT.cpp | 43 +++++++++++++++++++++++------ libraries/AP_GyroFFT/AP_GyroFFT.h | 12 ++++++-- 2 files changed, 45 insertions(+), 10 deletions(-) diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index f256a2d7aa..cdc8b2abfe 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -422,7 +422,11 @@ uint16_t AP_GyroFFT::run_cycle() calculate_noise(false, config); // record how we are doing - _output_cycle_micros = AP_HAL::micros() - now; + _thread_state._last_output_us[_update_axis] = AP_HAL::micros(); + _output_cycle_micros = _thread_state._last_output_us[_update_axis] - now; + + // move onto the next axis + _update_axis = (_update_axis + 1) % XYZ_AXIS_COUNT; // ready to receive another frame, because lock contention is so expensive we don't lock // around this flag but rather rely on the semaphore at the beginning of the loop to @@ -647,6 +651,30 @@ AP_GyroFFT::FrequencyPeak AP_GyroFFT::get_tracked_noise_peak() const return FrequencyPeak::CENTER; } +// center frequency slewed from previous to current value at the output rate +float AP_GyroFFT::get_slewed_noise_center_freq_hz(FrequencyPeak peak, uint8_t axis) const +{ + uint32_t now = AP_HAL::micros(); + const float slew = MIN(1.0f, (now - _global_state._last_output_us[axis]) + * (_fft_sampling_rate_hz / _samples_per_frame) / 1e6f); + return (_global_state._prev_center_freq_hz_filtered[peak][axis] + + (_global_state._center_freq_hz_filtered[peak][axis] - _global_state._prev_center_freq_hz_filtered[peak][axis]) * slew); +} + +// weighted center frequency slewed from previous to current value at the output rate +float AP_GyroFFT::get_slewed_weighted_freq_hz(FrequencyPeak peak) const +{ + const Vector3f& energy = get_center_freq_energy(peak); + const float freq_x = get_slewed_noise_center_freq_hz(peak, 0); + const float freq_y = get_slewed_noise_center_freq_hz(peak, 1); + + if (!energy.is_nan() && !is_zero(energy.x) && !is_zero(energy.y)) { + return (freq_x * energy.x + freq_y * energy.y) / (energy.x + energy.y); + } else { + return (freq_x + freq_y) * 0.5f; + } +} + // return an average center frequency weighted by bin energy // called from main thread float AP_GyroFFT::get_weighted_noise_center_freq_hz() const @@ -668,14 +696,14 @@ float AP_GyroFFT::get_weighted_noise_center_freq_hz() const const FrequencyPeak peak = get_tracked_noise_peak(); // pitch was good or required, roll was not, use pitch only if (!_rpy_health.x || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_PITCH) { - return get_noise_center_freq_hz(peak).y; + return get_slewed_noise_center_freq_hz(peak, 1); // Y-axis } // roll was good or required, pitch was not, use roll only if (!_rpy_health.y || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_ROLL) { - return get_noise_center_freq_hz(peak).x; + return get_slewed_noise_center_freq_hz(peak, 0); // X-axis } - return calculate_weighted_freq_hz(get_center_freq_energy(peak), get_noise_center_freq_hz(peak)); + return get_slewed_weighted_freq_hz(peak); } // return all the center frequencies weighted by bin energy @@ -702,20 +730,20 @@ uint8_t AP_GyroFFT::get_weighted_noise_center_frequencies_hz(uint8_t 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; + freqs[i] = get_slewed_noise_center_freq_hz(FrequencyPeak(i), 1); // Y-axis } 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; + freqs[i] = get_slewed_noise_center_freq_hz(FrequencyPeak(i), 0); // X-axis } 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))); + freqs[i] = get_slewed_weighted_freq_hz(FrequencyPeak(i)); } return tracked_peaks; } @@ -905,7 +933,6 @@ void AP_GyroFFT::calculate_noise(bool calibrating, const EngineConfig& config) _debug_snr = snr; _debug_max_bin = _state->_peak_data[FrequencyPeak::CENTER]._bin; #endif - _update_axis = (_update_axis + 1) % XYZ_AXIS_COUNT; } diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.h b/libraries/AP_GyroFFT/AP_GyroFFT.h index d1eac15639..906ee96ef6 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.h +++ b/libraries/AP_GyroFFT/AP_GyroFFT.h @@ -77,6 +77,9 @@ public: // 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); } const Vector3f& get_noise_center_freq_hz(FrequencyPeak peak) const { return _global_state._center_freq_hz_filtered[peak]; } + // slew frequency values + float get_slewed_weighted_freq_hz(FrequencyPeak peak) const; + float get_slewed_noise_center_freq_hz(FrequencyPeak peak, uint8_t axis) const; // 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 @@ -163,6 +166,7 @@ private: float get_tl_noise_center_bandwidth_hz(FrequencyPeak peak, uint8_t axis) const { return _thread_state._center_bandwidth_hz_filtered[peak][axis]; }; // thread-local mutators of filtered state float update_tl_noise_center_freq_hz(FrequencyPeak peak, uint8_t axis, float value) { + _thread_state._prev_center_freq_hz_filtered[peak][axis] = _thread_state._center_freq_hz_filtered[peak][axis]; return (_thread_state._center_freq_hz_filtered[peak][axis] = _center_freq_filter[peak].apply(axis, value)); } float update_tl_center_freq_energy(FrequencyPeak peak, uint8_t axis, float value) { @@ -215,10 +219,10 @@ private: // fit between first and second harmonics Vector3f _harmonic_fit; // bin of detected peak frequency - Vector3 _center_freq_bin; + Vector3ui _center_freq_bin; // fft engine health uint8_t _health; - Vector3 _health_ms; + Vector3ul _health_ms; // fft engine output rate uint32_t _output_cycle_ms; // tracked frequency peak @@ -227,6 +231,10 @@ private: Vector3f _center_snr; // filtered version of the peak frequency Vector3f _center_freq_hz_filtered[FrequencyPeak::MAX_TRACKED_PEAKS]; + // previous filtered version of the peak frequency + Vector3f _prev_center_freq_hz_filtered[FrequencyPeak::MAX_TRACKED_PEAKS]; + // when we last calculated a value + Vector3ul _last_output_us; // filtered energy of the detected peak frequency Vector3f _center_freq_energy_filtered[FrequencyPeak::MAX_TRACKED_PEAKS]; // filtered detected peak width