From 4199ccc29284fb1469c2a23438e9e9f1da5e9c9f Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 18 Jan 2023 22:57:44 +0000 Subject: [PATCH] AP_GyroFFT: correct notch calculation for FFT tune. use freq_min_ratio for scaling add harmonic calculation test set harmonics in notch setup --- libraries/AP_GyroFFT/AP_GyroFFT.cpp | 60 ++++++++++++------- libraries/AP_GyroFFT/AP_GyroFFT.h | 5 ++ .../AP_GyroFFT/tests/test_harmonic_fit.cpp | 23 +++++++ libraries/AP_GyroFFT/tests/wscript | 7 +++ 4 files changed, 75 insertions(+), 20 deletions(-) create mode 100644 libraries/AP_GyroFFT/tests/test_harmonic_fit.cpp create mode 100644 libraries/AP_GyroFFT/tests/wscript diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index 76c9dd0573..c1059a0950 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -701,7 +701,40 @@ void AP_GyroFFT::start_notch_tune() if (!hal.dsp->fft_start_average(_state)) { gcs().send_text(MAV_SEVERITY_WARNING, "FFT: Unable to start FFT averaging"); } + // throttle averaging for average fft calculation _avg_throttle_out = 0.0f; +#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) + AP_Motors* motors = AP::motors(); + if (motors != nullptr) { + _avg_throttle_out = motors->get_throttle_hover(); + } +#endif +} + +// calculate the frequency to be used for the harmonic notch +float AP_GyroFFT::calculate_notch_frequency(float* freqs, uint16_t numpeaks, float harmonic_fit, uint8_t& harmonics) +{ + float harmonic = freqs[0]; + harmonics = 1; + + for (uint8_t i = 1; i < numpeaks; i++) { + if (freqs[i] < harmonic) { + for (uint8_t x = 2; x <=HNF_MAX_HARMONICS; x++) { + if (is_harmonic_of(harmonic, freqs[i], x, harmonic_fit)) { + harmonic = freqs[i]; + } + } + } + } + // select the harmonics that were matched + for (uint8_t i = 0; i < numpeaks; i++) { + for (uint8_t x = 1; x <=HNF_MAX_HARMONICS; x++) { + if (is_harmonic_of(freqs[i], harmonic, x, harmonic_fit)) { + harmonics |= 1<<(x - 1); + } + } + } + return harmonic; } void AP_GyroFFT::stop_notch_tune() @@ -718,21 +751,11 @@ void AP_GyroFFT::stop_notch_tune() 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; - } - } - } + uint8_t harmonics; + float harmonic = calculate_notch_frequency(freqs, numpeaks, _harmonic_fit, harmonics); 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); + gcs().send_text(MAV_SEVERITY_NOTICE, "FFT: Selected %.1fHz\n", harmonic); // if we don't have a throttle value then all bets are off if (is_zero(_avg_throttle_out) || is_zero(harmonic)) { @@ -740,17 +763,14 @@ void AP_GyroFFT::stop_notch_tune() 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)) { + if (!_ins->setup_throttle_gyro_harmonic_notch(harmonic, (float)_fft_min_hz.get(), _avg_throttle_out, harmonics)) { gcs().send_text(MAV_SEVERITY_WARNING, "FFT: Unable to set throttle notch with %.1fHz/%.2f", - (float)_fft_min_hz.get(), thr_ref); + harmonic, _avg_throttle_out); // save results to FFT slots - _throttle_ref.set(thr_ref); + _throttle_ref.set(_avg_throttle_out); _freq_hover_hz.set(harmonic); } else { - gcs().send_text(MAV_SEVERITY_NOTICE, "FFT: Notch frequency %.1fHz and ref %.2f selected", (float)_fft_min_hz.get(), thr_ref); + gcs().send_text(MAV_SEVERITY_NOTICE, "FFT: Notch frequency %.1fHz and ref %.2f selected", harmonic, _avg_throttle_out); } } diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.h b/libraries/AP_GyroFFT/AP_GyroFFT.h index 3e85ded2c7..c422372970 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.h +++ b/libraries/AP_GyroFFT/AP_GyroFFT.h @@ -113,6 +113,11 @@ public: bool check_esc_noise() const { return (_options & uint32_t(Options::ESCNoiseCheck)) != 0; } // look for a frequency in the detected noise float has_noise_at_frequency_hz(float freq) const; + static float calculate_notch_frequency(float* freqs, uint16_t numpeaks, float harmonic_fit, uint8_t& harmonics); + static bool is_harmonic_of(float harmonic, float fundamental, uint8_t mult, float _fit) { + const float fit = 100.0f * fabsf(harmonic - fundamental * mult) / harmonic; + return (isfinite(fit) && fit < _fit); + } static const struct AP_Param::GroupInfo var_info[]; static AP_GyroFFT *get_singleton() { return _singleton; } diff --git a/libraries/AP_GyroFFT/tests/test_harmonic_fit.cpp b/libraries/AP_GyroFFT/tests/test_harmonic_fit.cpp new file mode 100644 index 0000000000..7478382e71 --- /dev/null +++ b/libraries/AP_GyroFFT/tests/test_harmonic_fit.cpp @@ -0,0 +1,23 @@ +#include +#include +#include +#include +#include + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +TEST(harmonic_fit_Test, Basic) +{ +#if HAL_GYROFFT_ENABLED + float freqs[] { 176.9, 57.2, 228.7 }; + uint8_t harmonics; + float harmonic = AP_GyroFFT::calculate_notch_frequency(freqs, 3, 10, harmonics); + + printf("FFT: Found peaks at %.1f/%.1f/%.1fHz\n", freqs[0], freqs[1], freqs[2]); + printf("FFT: Selected %.1fHz %d\n", harmonic, harmonics); + + EXPECT_TRUE(is_equal(harmonic, 57.2f)); +#endif +} + +AP_GTEST_MAIN() diff --git a/libraries/AP_GyroFFT/tests/wscript b/libraries/AP_GyroFFT/tests/wscript new file mode 100644 index 0000000000..cd3e5e3ce7 --- /dev/null +++ b/libraries/AP_GyroFFT/tests/wscript @@ -0,0 +1,7 @@ +#!/usr/bin/env python +# encoding: utf-8 + +def build(bld): + bld.ap_find_tests( + use='ap', + )