You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
246 lines
9.2 KiB
246 lines
9.2 KiB
/* |
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
|
|
Code by Andy Piper |
|
*/ |
|
#pragma once |
|
|
|
#include <AP_HAL/AP_HAL.h> |
|
#include <AP_Vehicle/AP_Vehicle_Type.h> |
|
|
|
#ifndef HAL_GYROFFT_ENABLED |
|
#define HAL_GYROFFT_ENABLED HAL_WITH_DSP |
|
#endif |
|
|
|
#if HAL_GYROFFT_ENABLED |
|
|
|
#include <AP_Common/AP_Common.h> |
|
#include <AP_HAL/AP_HAL.h> |
|
#include <AP_Param/AP_Param.h> |
|
#include <AP_Math/AP_Math.h> |
|
#include <AP_InertialSensor/AP_InertialSensor.h> |
|
#include <Filter/LowPassFilter.h> |
|
|
|
#define DEBUG_FFT 0 |
|
|
|
// a library that leverages the HAL DSP support to perform FFT analysis on gyro samples |
|
class AP_GyroFFT |
|
{ |
|
public: |
|
AP_GyroFFT(); |
|
|
|
// Do not allow copies |
|
AP_GyroFFT(const AP_GyroFFT &other) = delete; |
|
AP_GyroFFT &operator=(const AP_GyroFFT&) = delete; |
|
|
|
void init(uint32_t target_looptime); |
|
|
|
// cycle through the FFT steps - runs at 400Hz |
|
uint16_t update(); |
|
// capture gyro values at the appropriate update rate - runs at fast loop rate |
|
void sample_gyros(); |
|
// update calculated values of dynamic parameters - runs at 1Hz |
|
void update_parameters(); |
|
// thread for processing gyro data via FFT |
|
void update_thread(); |
|
// start the update thread |
|
void start_update_thread(); |
|
|
|
// check at startup that standard frequencies can be detected |
|
bool calibration_check(); |
|
// called when hovering to determine the average peak frequency and reference value |
|
void update_freq_hover(float dt, float throttle_out); |
|
// called to save the average peak frequency and reference value |
|
void save_params_on_disarm(); |
|
// dynamically enable or disable the analysis through the aux switch |
|
void set_analysis_enabled(bool enabled) { _analysis_enabled = enabled; }; |
|
|
|
// detected peak frequency filtered at 1/3 the update rate |
|
Vector3f get_noise_center_freq_hz() const { return _global_state._center_freq_hz_filtered; } |
|
// energy of the background noise at the detected center frequency |
|
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(); |
|
// detected peak frequency |
|
Vector3f get_raw_noise_center_freq_hz() const { return _global_state._center_freq_hz; } |
|
// match between first and second harmonics |
|
Vector3f get_raw_noise_harmonic_fit() const { return _global_state._harmonic_fit; } |
|
// energy of the detected peak frequency |
|
Vector3f get_center_freq_energy() const { return _global_state._center_freq_energy; } |
|
// index of the FFT bin containing the detected peak frequency |
|
Vector3<uint16_t> get_center_freq_bin() const { return _global_state._center_freq_bin; } |
|
// detected peak bandwidth |
|
Vector3f get_noise_center_bandwidth_hz() const { return _global_state._center_bandwidth_hz; }; |
|
// weighted detected peak bandwidth |
|
float get_weighted_noise_center_bandwidth_hz(); |
|
// log gyro fft messages |
|
void write_log_messages(); |
|
|
|
static const struct AP_Param::GroupInfo var_info[]; |
|
static AP_GyroFFT *get_singleton() { return _singleton; } |
|
|
|
private: |
|
// calculate the peak noise frequency |
|
void calculate_noise(uint16_t max_bin); |
|
// 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 |
|
float self_test_bin_frequencies(); |
|
// detect the provided frequency |
|
float self_test(float frequency, GyroWindow test_window); |
|
// whether to run analysis or not |
|
bool analysis_enabled() const { return _initialized && _analysis_enabled && _thread_created; }; |
|
// whether analysis can be run again or not |
|
bool start_analysis(); |
|
// the size of the ring buffer in both the IMU backend |
|
uint16_t get_buffer_size() const { return _state->_window_size + INS_MAX_GYRO_WINDOW_SAMPLES; } |
|
// semaphore for access to shared FFT data |
|
HAL_Semaphore _sem; |
|
|
|
// data accessible from the main thread protected by the semaphore |
|
struct EngineState { |
|
// energy of the detected peak frequency |
|
Vector3f _center_freq_energy; |
|
// energy of the detected peak frequency in dB |
|
Vector3f _center_freq_energy_db; |
|
// detected peak frequency |
|
Vector3f _center_freq_hz; |
|
// fit between first and second harmonics |
|
Vector3f _harmonic_fit; |
|
// bin of detected poeak frequency |
|
Vector3<uint16_t> _center_freq_bin; |
|
// filtered version of the peak frequency |
|
Vector3f _center_freq_hz_filtered; |
|
// signal to noise ratio of PSD at the detected centre frequency |
|
Vector3f _center_snr; |
|
// detected peak width |
|
Vector3f _center_bandwidth_hz; |
|
// axes that still require noise calibration |
|
uint8_t _noise_needs_calibration : 3; |
|
// whether the analyzer is mid-cycle |
|
bool _analysis_started = false; |
|
}; |
|
|
|
// Shared FFT engine state local to the FFT thread |
|
EngineState _thread_state; |
|
// Shared FFT engine state accessible by the main thread |
|
EngineState _global_state; |
|
|
|
// configuration data local to the FFT thread but set from the main thread |
|
struct EngineConfig { |
|
// whether the analyzer should be run |
|
bool _analysis_enabled; |
|
// minimum frequency of the detection window |
|
uint16_t _fft_min_hz; |
|
// maximum frequency of the detection window |
|
uint16_t _fft_max_hz; |
|
// configured start bin based on min hz |
|
uint16_t _fft_start_bin; |
|
// configured end bin based on max hz |
|
uint16_t _fft_end_bin; |
|
// attenuation cutoff for calculation of hover bandwidth |
|
float _attenuation_cutoff; |
|
// SNR Threshold |
|
float _snr_threshold_db; |
|
} _config; |
|
|
|
// number of sampeles needed before a new frame can be processed |
|
uint16_t _samples_per_frame; |
|
// downsampled gyro data circular buffer index frequency analysis |
|
uint16_t _circular_buffer_idx; |
|
// number of collected unprocessed gyro samples |
|
uint16_t _sample_count; |
|
|
|
// downsampled gyro data circular buffer for frequency analysis |
|
float* _downsampled_gyro_data[XYZ_AXIS_COUNT]; |
|
// accumulator for sampled gyro data |
|
Vector3f _oversampled_gyro_accum; |
|
// count of oversamples |
|
uint16_t _oversampled_gyro_count; |
|
// current write index in the buffer |
|
uint16_t _downsampled_gyro_idx; |
|
|
|
// state of the FFT engine |
|
AP_HAL::DSP::FFTWindowState* _state; |
|
// update state machine step information |
|
uint8_t _update_axis; |
|
// noise base of the gyros |
|
Vector3f* _ref_energy; |
|
// the number of cycles required to have a proper noise reference |
|
uint16_t _noise_cycles; |
|
// number of cycles over which to generate noise ensemble averages |
|
uint16_t _noise_calibration_cycles[XYZ_AXIS_COUNT]; |
|
// current _sample_mode |
|
uint8_t _current_sample_mode : 3; |
|
// harmonic multiplier for two highest peaks |
|
float _harmonic_multiplier; |
|
// searched harmonics - inferred from harmonic notch harmoncis |
|
uint8_t _harmonics; |
|
|
|
// smoothing filter on the output |
|
LowPassFilterFloat _center_freq_filter[XYZ_AXIS_COUNT]; |
|
// smoothing filter on the bandwidth |
|
LowPassFilterFloat _center_bandwidth_filter[XYZ_AXIS_COUNT]; |
|
|
|
// configured sampling rate |
|
uint16_t _fft_sampling_rate_hz; |
|
// number of cycles without a detected signal |
|
uint8_t _missed_cycles; |
|
// whether the analyzer initialized correctly |
|
bool _initialized; |
|
|
|
// whether the analyzer should be run |
|
bool _analysis_enabled = true; |
|
// whether the update thread was created |
|
bool _thread_created = false; |
|
// minimum frequency of the detection window |
|
AP_Int16 _fft_min_hz; |
|
// maximum frequency of the detection window |
|
AP_Int16 _fft_max_hz; |
|
// size of the FFT window |
|
AP_Int16 _window_size; |
|
// percentage overlap of FFT windows |
|
AP_Float _window_overlap; |
|
// overall enablement of the feature |
|
AP_Int8 _enable; |
|
// gyro rate sampling or cycle divider |
|
AP_Int8 _sample_mode; |
|
// learned throttle reference for the hover frequency |
|
AP_Float _throttle_ref; |
|
// learned hover filter frequency |
|
AP_Float _freq_hover_hz; |
|
// SNR Threshold |
|
AP_Float _snr_threshold_db; |
|
// attenuation to use for calculating the peak bandwidth at hover |
|
AP_Float _attenuation_power_db; |
|
// learned peak bandwidth at configured attenuation at hover |
|
AP_Float _bandwidth_hover_hz; |
|
AP_InertialSensor* _ins; |
|
#if DEBUG_FFT |
|
uint32_t _last_output_ms; |
|
EngineState _debug_state; |
|
float _debug_max_bin_freq; |
|
float _debug_max_freq_bin; |
|
uint16_t _debug_max_bin; |
|
float _debug_snr; |
|
#endif |
|
|
|
static AP_GyroFFT *_singleton; |
|
}; |
|
|
|
namespace AP { |
|
AP_GyroFFT *fft(); |
|
}; |
|
|
|
#endif // HAL_GYROFFT_ENABLED
|