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
This commit is contained in:
Andy Piper 2022-03-06 13:51:56 +01:00 committed by Andrew Tridgell
parent bfc3a5a749
commit aaad4ace08
2 changed files with 79 additions and 1 deletions

View File

@ -614,6 +614,14 @@ void AP_GyroFFT::update_freq_hover(float dt, float throttle_out)
if (!analysis_enabled()) { if (!analysis_enabled()) {
return; 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. // 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); _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); _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(); _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 // return the noise peak that is being tracked
// called from main thread // called from main thread
AP_GyroFFT::FrequencyPeak AP_GyroFFT::get_tracked_noise_peak() const 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 (!_health) {
#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
AP_Motors* motors = AP::motors(); AP_Motors* motors = AP::motors();
if (motors != nullptr) { if (motors != nullptr && !is_zero(_throttle_ref)) {
// FFT is not healthy, fallback to throttle-based estimate // 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); return constrain_float(_fft_min_hz * MAX(1.0f, sqrtf(motors->get_throttle_out() / _throttle_ref)), _fft_min_hz, _fft_max_hz);
} }

View File

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