Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 13:38:38 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GyroFFT: save a sliding window of output frequency bins

Browse Source 
allow configuration of DSP averaging frames via FFT_NUM_FRAMES
This commit is contained in:
Andy Piper 2022-03-06 13:51:56 +01:00 committed by Peter Hall
parent c02aef8a50
commit 684cf996e1
2 changed files with 27 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_GyroFFT/AP_GyroFFT.cpp
View File

@ -155,6 +155,14 @@ const AP_Param::GroupInfo AP_GyroFFT::var_info[] = {
// @User: Advanced
AP_GROUPINFO("HMNC_PEAK", 13, AP_GyroFFT, _harmonic_peak, 0),
// @Param: NUM_FRAMES
// @DisplayName: FFT output frames to retain and average
// @Description: Number of output frequency frames to retain and average in order to calculate final frequencies. Averaging output frames can drastically reduce noise and jitter at the cost of latency as long as the input is stable. The default is to perform no averaging. For rapidly changing frequencies (e.g. smaller aircraft) fewer frames should be averaged.
// @Range: 0 8
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("NUM_FRAMES", 14, AP_GyroFFT, _num_frames, 0),
AP_GROUPEND
};
@ -208,10 +216,13 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz)
// this is particularly a problem on IMUs with higher sample rates (e.g. BMI088)
// 16 gives a maximum output rate of 2Khz / 16 = 125Hz per axis or 375Hz in aggregate
_samples_per_frame = MAX(FFT_MIN_SAMPLES_PER_FRAME, 1 << lrintf(log2f(_samples_per_frame)));
if (_num_frames > 0) {
_num_frames = constrain_int16(_num_frames, 2, AP_HAL::DSP::MAX_SLIDING_WINDOW_SIZE);
}
// check that we have enough memory for the window size requested
// INS: XYZ_AXIS_COUNT * INS_MAX_INSTANCES * _window_size, DSP: 3 * _window_size, FFT: XYZ_AXIS_COUNT + 3 * _window_size
const uint32_t allocation_count = (XYZ_AXIS_COUNT * INS_MAX_INSTANCES + 3 + XYZ_AXIS_COUNT + 3) * sizeof(float);
const uint32_t allocation_count = (XYZ_AXIS_COUNT * INS_MAX_INSTANCES + 3 + XYZ_AXIS_COUNT + 3 + _num_frames) * sizeof(float);
if (allocation_count * FFT_DEFAULT_WINDOW_SIZE > hal.util->available_memory() / 2) {
gcs().send_text(MAV_SEVERITY_WARNING, "AP_GyroFFT: disabled, required %u bytes", (unsigned int)allocation_count * FFT_DEFAULT_WINDOW_SIZE);
return;
@ -287,7 +298,7 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz)
}
// initialise the HAL DSP subsystem
_state = hal.dsp->fft_init(_window_size, _fft_sampling_rate_hz);
_state = hal.dsp->fft_init(_window_size, _fft_sampling_rate_hz, _num_frames);
if (_state == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING, "Failed to initialize DSP engine");
return;
@ -1009,7 +1020,7 @@ void AP_GyroFFT::calculate_noise(bool calibrating, const EngineConfig& config)
#if DEBUG_FFT
WITH_SEMAPHORE(_sem);
_debug_state = _thread_state;
_debug_max_freq_bin = _state->_freq_bins[_state->_peak_data[FrequencyPeak::CENTER]._bin];
_debug_max_freq_bin = _state->get_freq_bin(_state->_peak_data[FrequencyPeak::CENTER]._bin);
_debug_max_bin_freq = _state->_peak_data[FrequencyPeak::CENTER]._freq_hz;
_debug_snr = snr;
_debug_max_bin = _state->_peak_data[FrequencyPeak::CENTER]._bin;
@ -1102,7 +1113,7 @@ bool AP_GyroFFT::calculate_filtered_noise(FrequencyPeak target_peak, FrequencyPe
if (freqs.is_valid(FrequencyPeak(source_peak))) {
// total peak energy requires an integration, as an approximation use amplitude * noise width * 5/6
update_tl_center_freq_energy(target_peak, _update_axis, _state->_freq_bins[nb] * peak_data->_noise_width_hz * 0.8333f);
update_tl_center_freq_energy(target_peak, _update_axis, _state->get_freq_bin(nb) * peak_data->_noise_width_hz * 0.8333f);
update_tl_noise_center_bandwidth_hz(target_peak, _update_axis, peak_data->_noise_width_hz);
update_tl_noise_center_freq_hz(target_peak, _update_axis, freqs.get_weighted_frequency(FrequencyPeak(source_peak)));
_missed_cycles[_update_axis][target_peak] = 0;
@ -1115,7 +1126,7 @@ bool AP_GyroFFT::calculate_filtered_noise(FrequencyPeak target_peak, FrequencyPe
}
// we failed to find a signal for more than FFT_MAX_MISSED_UPDATES cycles
update_tl_center_freq_energy(target_peak, _update_axis, _state->_freq_bins[nb] * peak_data->_noise_width_hz * 0.8333f); // use the actual energy detected rather than 0
update_tl_center_freq_energy(target_peak, _update_axis, _state->get_freq_bin(nb) * peak_data->_noise_width_hz * 0.8333f); // use the actual energy detected rather than 0
update_tl_noise_center_bandwidth_hz(target_peak, _update_axis, _bandwidth_hover_hz);
update_tl_noise_center_freq_hz(target_peak, _update_axis, config._fft_min_hz);
@ -1151,12 +1162,12 @@ bool AP_GyroFFT::get_weighted_frequency(FrequencyPeak peak, float& weighted_peak
const uint16_t bin = peak_data->_bin;
// calculate the SNR and center frequency energy
const float max_energy = MAX(1.0f, _state->_freq_bins[bin]);
const float max_energy = MAX(1.0f, _state->get_freq_bin(bin));
const float ref_energy = MAX(1.0f, _ref_energy[_update_axis][bin]);
snr = 10.f * (log10f(max_energy) - log10f(ref_energy));
// if the bin energy is above the noise threshold then we have a signal
if (!_thread_state._noise_needs_calibration && isfinite(_state->_freq_bins[bin]) && snr > config._snr_threshold_db) {
if (!_thread_state._noise_needs_calibration && isfinite(_state->get_freq_bin(bin)) && snr > config._snr_threshold_db) {
weighted_peak_freq_hz = constrain_float(peak_data->_freq_hz, (float)config._fft_min_hz, (float)config._fft_max_hz);
return true;
}
@ -1212,7 +1223,7 @@ void AP_GyroFFT::update_ref_energy(uint16_t max_bin)
// determine a PS noise reference at each of the possible center frequencies
if (_noise_cycles == 0 && _noise_calibration_cycles[_update_axis] > 0) {
for (uint16_t i = 1; i < _state->_bin_count; i++) {
_ref_energy[_update_axis][i] += _state->_freq_bins[i];
_ref_energy[_update_axis][i] += _state->get_freq_bin(i);
}
if (--_noise_calibration_cycles[_update_axis] == 0) {
for (uint16_t i = 1; i < _state->_bin_count; i++) {
@ -1271,6 +1282,12 @@ float AP_GyroFFT::self_test(float frequency, FloatBuffer& test_window)
_update_axis = 0;
// if using averaging we need to process _num_frames in order to not bias the result
for (uint8_t i = 1; i < _num_frames; i++) {
hal.dsp->fft_start(_state, test_window, 0);
hal.dsp->fft_analyse(_state, _config._fft_start_bin, _config._fft_end_bin, _config._attenuation_cutoff);
}
// final cycle is the one we want
hal.dsp->fft_start(_state, test_window, 0);
uint16_t max_bin = hal.dsp->fft_analyse(_state, _config._fft_start_bin, _config._fft_end_bin, _config._attenuation_cutoff);

2
libraries/AP_GyroFFT/AP_GyroFFT.h
View File

@ -340,6 +340,8 @@ private:
AP_Int8 _harmonic_fit;
// harmonic peak target
AP_Int8 _harmonic_peak;
// number of output frames to retain for averaging
AP_Int8 _num_frames;
AP_InertialSensor* _ins;
#if DEBUG_FFT
uint32_t _last_output_ms;