Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             : 
      15             :     Code by Andy Piper
      16             :  */
      17             : #pragma once
      18             : 
      19             : #include <AP_HAL/AP_HAL.h>
      20             : 
      21             : #if HAL_GYROFFT_ENABLED
      22             : 
      23             : #include <AP_Common/AP_Common.h>
      24             : #include <AP_HAL/utility/RingBuffer.h>
      25             : #include <AP_Param/AP_Param.h>
      26             : #include <AP_Math/AP_Math.h>
      27             : #include <AP_InertialSensor/AP_InertialSensor.h>
      28             : #include <Filter/LowPassFilter.h>
      29             : #include <Filter/FilterWithBuffer.h>
      30             : 
      31             : #define DEBUG_FFT   0
      32             : 
      33             : // a library that leverages the HAL DSP support to perform FFT analysis on gyro samples
      34             : class AP_GyroFFT
      35             : {
      36             :     friend class ReplayGyroFFT;
      37             : 
      38             : public:
      39             :     typedef AP_HAL::DSP::FrequencyPeak FrequencyPeak;
      40             : 
      41             :     enum class Options : uint32_t {
      42             :         FFTPostFilter = 1 << 0,
      43             :         ESCNoiseCheck = 1 << 1
      44             :     };
      45             : 
      46             :     AP_GyroFFT();
      47             : 
      48             :     // Do not allow copies
      49             :     CLASS_NO_COPY(AP_GyroFFT);
      50             : 
      51             :     void init(uint16_t loop_rate_hz);
      52             : 
      53             :     // cycle through the FFT steps - runs in the FFT thread
      54             :     uint16_t run_cycle();
      55             :     // capture gyro values at the appropriate update rate - runs at fast loop rate
      56             :     void sample_gyros();
      57             :     // update the engine state - runs at 400Hz
      58             :     void update();
      59             :     // update calculated values of dynamic parameters - runs at 1Hz
      60      149217 :     void update_parameters() { update_parameters(false); }
      61             :     // thread for processing gyro data via FFT
      62             :     void update_thread();
      63             :     // start the update thread
      64             :     bool start_update_thread();
      65             :     // is the subsystem enabled
      66        2088 :     bool enabled() const { return _enable; }
      67             : 
      68             :     // check at startup that standard frequencies can be detected
      69             :     bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len);
      70             :     // make sure calibration is set done
      71             :     bool prepare_for_arming();
      72             :     // called when hovering to determine the average peak frequency and reference value
      73             :     void update_freq_hover(float dt, float throttle_out);
      74             :     // called to save the average peak frequency and reference value
      75             :     void save_params_on_disarm();
      76             :     // dynamically enable or disable the analysis through the aux switch
      77          20 :     void set_analysis_enabled(bool enabled) { _analysis_enabled = enabled; };
      78             :     // notch tuning
      79             :     void start_notch_tune();
      80             :     void stop_notch_tune();
      81             : 
      82             :     // detected peak frequency filtered at 1/3 the update rate
      83             :     const Vector3f& get_noise_center_freq_hz() const { return get_noise_center_freq_hz(FrequencyPeak::CENTER); }
      84      382526 :     const Vector3f& get_noise_center_freq_hz(FrequencyPeak peak) const { return _global_state._center_freq_hz_filtered[peak]; }
      85             :     // frequency values
      86             :     float get_weighted_freq_hz(FrequencyPeak peak) const;
      87             :     // energy of the background noise at the detected center frequency
      88       68568 :     const Vector3f& get_noise_signal_to_noise_db() const { return get_noise_signal_to_noise_db(FrequencyPeak::CENTER); }
      89      277878 :     const Vector3f& get_noise_signal_to_noise_db(FrequencyPeak peak) const { return _global_state._center_freq_snr[peak];; }
      90             :     // detected peak frequency weighted by energy
      91             :     float get_weighted_noise_center_freq_hz() const;
      92             :     // all detected peak frequencies weighted by energy
      93             :     uint8_t get_weighted_noise_center_frequencies_hz(uint8_t num_freqs, float* freqs) const;
      94             :     // detected peak frequency
      95             :     const Vector3f& get_raw_noise_center_freq_hz() const { return _global_state._center_freq_hz; }
      96             :     // match between first and second harmonics
      97       68568 :     const Vector3f& get_raw_noise_harmonic_fit() const { return _global_state._harmonic_fit; }
      98             :     // energy of the detected peak frequency
      99             :     const Vector3f& get_center_freq_energy() const { return get_center_freq_energy(FrequencyPeak::CENTER); }
     100      454284 :     const Vector3f& get_center_freq_energy(FrequencyPeak peak) const { return _global_state._center_freq_energy_filtered[peak]; }
     101             :     // index of the FFT bin containing the detected peak frequency
     102             :     const Vector3<uint16_t>& get_center_freq_bin() const { return _global_state._center_freq_bin; }
     103             :     // detected peak bandwidth
     104             :     const Vector3f& get_noise_center_bandwidth_hz() const { return get_noise_center_bandwidth_hz(FrequencyPeak::CENTER); }
     105      283696 :     const Vector3f& get_noise_center_bandwidth_hz(FrequencyPeak peak) const { return _global_state._center_bandwidth_hz_filtered[peak]; };
     106             :     // weighted detected peak bandwidth
     107             :     float get_weighted_noise_center_bandwidth_hz() const;
     108             :     // log gyro fft messages
     109             :     void write_log_messages();
     110             :     // post filter mask of IMUs
     111       41208 :     bool using_post_filter_samples() const { return (_options & uint32_t(Options::FFTPostFilter)) != 0; }
     112             :     // post filter mask of IMUs
     113      502380 :     bool check_esc_noise() const { return (_options & uint32_t(Options::ESCNoiseCheck)) != 0; }
     114             :     // look for a frequency in the detected noise
     115             :     float has_noise_at_frequency_hz(float freq) const;
     116             :     static float calculate_notch_frequency(float* freqs, uint16_t numpeaks, float harmonic_fit, uint8_t& harmonics);
     117         174 :     static bool is_harmonic_of(float harmonic, float fundamental, uint8_t mult, float _fit) {
     118         174 :         const float fit = 100.0f * fabsf(harmonic - fundamental * mult) / harmonic;
     119         174 :         return (isfinite(fit) && fit < _fit);
     120             :     }
     121             : 
     122             :     static const struct AP_Param::GroupInfo var_info[];
     123       50265 :     static AP_GyroFFT *get_singleton() { return _singleton; }
     124             : 
     125             : private:
     126             :     // configuration data local to the FFT thread but set from the main thread
     127             :     struct EngineConfig {
     128             :         // whether the analyzer should be run
     129             :         bool _analysis_enabled;
     130             :         // minimum frequency of the detection window
     131             :         uint16_t _fft_min_hz;
     132             :         // maximum frequency of the detection window
     133             :         uint16_t _fft_max_hz;
     134             :         // configured start bin based on min hz
     135             :         uint16_t _fft_start_bin;
     136             :         // configured end bin based on max hz
     137             :         uint16_t _fft_end_bin;
     138             :         // attenuation cutoff for calculation of hover bandwidth
     139             :         float _attenuation_cutoff;
     140             :         // SNR Threshold
     141             :         float _snr_threshold_db;
     142             :     } _config;
     143             : 
     144             :     // smoothing filter that first takes the median from a sliding window and then
     145             :     // applies a low pass filter to the result
     146             :     class MedianLowPassFilter3dFloat {
     147             :     public:
     148      132048 :         MedianLowPassFilter3dFloat() { }
     149             : 
     150             :         float apply(uint8_t axis, float sample);
     151             :         float get(uint8_t axis) const { return _lowpass_filter[axis].get(); }
     152             : 
     153         180 :         void set_cutoff_frequency(float sample_freq, float cutoff_freq) {
     154         720 :             for (uint8_t i = 0; i < XYZ_AXIS_COUNT; i++) {
     155         540 :                 _lowpass_filter[i].set_cutoff_frequency(sample_freq, cutoff_freq);
     156             :             }
     157         180 :         }
     158             : 
     159             :     private:
     160             :         LowPassFilterConstDtFloat _lowpass_filter[XYZ_AXIS_COUNT];
     161             :         FilterWithBuffer<float,3> _median_filter[XYZ_AXIS_COUNT];
     162             :     };
     163             : 
     164             :     // structure for holding noise peak data while calculating swaps
     165             :     class FrequencyData {
     166             :     public:
     167             :         FrequencyData(const AP_GyroFFT& gyrofft, const EngineConfig& config);
     168     2777842 :         float get_weighted_frequency(FrequencyPeak i) const { return frequency[i]; }
     169      839202 :         float get_signal_to_noise(FrequencyPeak i) const { return snr[i]; }
     170     3321355 :         bool is_valid(FrequencyPeak i) const { return valid[i]; }
     171             :     private:
     172             :         float frequency[FrequencyPeak::MAX_TRACKED_PEAKS];
     173             :         float snr[FrequencyPeak::MAX_TRACKED_PEAKS];
     174             :         bool valid[FrequencyPeak::MAX_TRACKED_PEAKS];
     175             :     };
     176             :     // distance matrix between filtered and instantaneous peaks
     177             :     typedef float DistanceMatrix[FrequencyPeak::MAX_TRACKED_PEAKS][FrequencyPeak::MAX_TRACKED_PEAKS];
     178             : 
     179             :     // thread-local accessors of filtered state
     180     2013099 :     float get_tl_noise_center_freq_hz(FrequencyPeak peak, uint8_t axis) const { return _thread_state._center_freq_hz_filtered[peak][axis]; }
     181      358130 :     float get_tl_center_freq_energy(FrequencyPeak peak, uint8_t axis) const { return _thread_state._center_freq_energy_filtered[peak][axis]; }
     182             :     float get_tl_noise_center_bandwidth_hz(FrequencyPeak peak, uint8_t axis) const { return _thread_state._center_bandwidth_hz_filtered[peak][axis]; };
     183             :     // thread-local mutators of filtered state
     184      755054 :     float update_tl_noise_center_freq_hz(FrequencyPeak peak, uint8_t axis, float value) {
     185      755054 :         return (_thread_state._center_freq_hz_filtered[peak][axis] = _center_freq_filter[peak].apply(axis, value));
     186             :     }
     187      755054 :     float update_tl_center_freq_energy(FrequencyPeak peak, uint8_t axis, float value) {
     188      755054 :         return (_thread_state._center_freq_energy_filtered[peak][axis] = _center_freq_energy_filter[peak].apply(axis, value));
     189             :     }
     190      755054 :     float update_tl_noise_center_bandwidth_hz(FrequencyPeak peak, uint8_t axis, float value) {
     191      755054 :         return (_thread_state._center_bandwidth_hz_filtered[peak][axis] = _center_bandwidth_filter[peak].apply(axis, value));
     192             :     }
     193             :     // write single log messages
     194             :     void log_noise_peak(uint8_t id, FrequencyPeak peak) const;
     195             :     // calculate the peak noise frequency
     196             :     void calculate_noise(bool calibrating, const EngineConfig& config);
     197             :     // calculate noise peaks based on energy and history
     198             :     uint8_t calculate_tracking_peaks(float& weighted_peak_freq_hz, bool calibrating, const EngineConfig& config);
     199             :     uint8_t calculate_tracking_peaks(float& weighted_center_freq_hz, const FrequencyData& freqs, const EngineConfig& config);
     200             :     // calculate noise peak frequency characteristics
     201             :     bool calculate_filtered_noise(FrequencyPeak target_peak, FrequencyPeak source_peak, const FrequencyData& freqs, const EngineConfig& config);
     202             :     void update_snr_values(const FrequencyData& freqs);
     203             :     // get the weighted frequency
     204             :     bool get_weighted_frequency(FrequencyPeak peak, float& weighted_peak_freq_hz, float& snr, const EngineConfig& config) const;
     205             :     // return the tracked noise peak
     206             :     FrequencyPeak get_tracked_noise_peak() const;
     207             :     // calculate the distance matrix between the current estimates and the current cycle
     208             :     void find_distance_matrix(DistanceMatrix& distance_matrix, const FrequencyData& freqs, const EngineConfig& config) const;
     209             :     // return the instantaneous peak that is closest to the target estimate peak
     210             :     FrequencyPeak find_closest_peak(const FrequencyPeak target, const DistanceMatrix& distance_matrix, uint8_t ignore = 0) const;
     211             :     // detected peak frequency weighted by energy
     212             :     float calculate_weighted_freq_hz(const Vector3f& energy, const Vector3f& freq) const;
     213             :     // update the estimation of the background noise energy
     214             :     void update_ref_energy(uint16_t max_bin);
     215             :     // test frequency detection for all of the allowable bins
     216             :     float self_test_bin_frequencies();
     217             :     // detect the provided frequency
     218             :     float self_test(float frequency, FloatBuffer& test_window);
     219             :     // whether to run analysis or not
     220   622690387 :     bool analysis_enabled() const { return _initialized && _analysis_enabled && _thread_created; };
     221             :     // whether analysis can be run again or not
     222             :     bool start_analysis();
     223             :     // return samples available in the gyro window
     224   535617685 :     uint16_t get_available_samples(uint8_t axis) {
     225   535617685 :         return _sample_mode == 0 ?_ins->get_raw_gyro_window(axis).available() : _downsampled_gyro_data[axis].available();
     226             :     }
     227             :     void update_parameters(bool force);
     228             :     // semaphore for access to shared FFT data
     229             :     HAL_Semaphore _sem;
     230             : 
     231             :     // data set from the FFT thread but accessible from the main thread protected by the semaphore
     232             :     struct EngineState {
     233             :         // energy of the detected peak frequency in dB
     234             :         Vector3f _center_freq_energy_db;
     235             :         // detected peak frequency
     236             :         Vector3f _center_freq_hz;
     237             :         // fit between first and second harmonics
     238             :         Vector3f _harmonic_fit;
     239             :         // bin of detected peak frequency
     240             :         Vector3ui _center_freq_bin;
     241             :         // fft engine health
     242             :         Vector3<uint8_t> _health;
     243             :         Vector3ul _health_ms;
     244             :         // fft engine output rate
     245             :         uint32_t _output_cycle_ms;
     246             :         // tracked frequency peak for the purposes of notching
     247             :         Vector3<uint8_t> _tracked_peak;
     248             :         // center frequency peak ignoring temporary energy changes / order switching
     249             :         Vector3<uint8_t> _center_peak;
     250             :         // signal to noise ratio of PSD at each of the detected centre frequencies
     251             :         Vector3f _center_freq_snr[FrequencyPeak::MAX_TRACKED_PEAKS];
     252             :         // filtered version of the peak frequency
     253             :         Vector3f _center_freq_hz_filtered[FrequencyPeak::MAX_TRACKED_PEAKS];
     254             :         // when we last calculated a value
     255             :         Vector3ul _last_output_us;
     256             :         // filtered energy of the detected peak frequency
     257             :         Vector3f _center_freq_energy_filtered[FrequencyPeak::MAX_TRACKED_PEAKS];
     258             :         // filtered detected peak width
     259             :         Vector3f _center_bandwidth_hz_filtered[FrequencyPeak::MAX_TRACKED_PEAKS];
     260             :         // axes that still require noise calibration
     261             :         uint8_t _noise_needs_calibration;
     262             :         // whether the analyzer is mid-cycle
     263             :         bool _analysis_started;
     264             :     };
     265             : 
     266             :     // Shared FFT engine state local to the FFT thread
     267             :     EngineState _thread_state;
     268             :     // Shared FFT engine state accessible by the main thread
     269             :     EngineState _global_state;
     270             : 
     271             :     // number of samples needed before a new frame can be processed
     272             :     uint16_t _samples_per_frame;
     273             :     // number of ms that a frame should take to process to sustain output rate
     274             :     uint16_t _frame_time_ms;
     275             :     // last cycle time
     276             :     uint32_t _output_cycle_micros;
     277             :     // downsampled gyro data circular buffer for frequency analysis
     278             :     FloatBuffer _downsampled_gyro_data[XYZ_AXIS_COUNT];
     279             :     // accumulator for sampled gyro data
     280             :     Vector3f _oversampled_gyro_accum;
     281             :     // count of oversamples
     282             :     uint16_t _oversampled_gyro_count;
     283             : 
     284             :     // state of the FFT engine
     285             :     AP_HAL::DSP::FFTWindowState* _state;
     286             :     // update state machine step information
     287             :     uint8_t _update_axis;
     288             :     // noise base of the gyros
     289             :     Vector3f* _ref_energy;
     290             :     // the number of cycles required to have a proper noise reference
     291             :     uint16_t _noise_cycles;
     292             :     // number of cycles over which to generate noise ensemble averages
     293             :     uint16_t _noise_calibration_cycles[XYZ_AXIS_COUNT];
     294             :     // current _sample_mode
     295             :     uint8_t _current_sample_mode;
     296             :     // harmonic multiplier for two highest peaks
     297             :     float _harmonic_multiplier;
     298             :     // number of tracked peaks
     299             :     uint8_t _tracked_peaks;
     300             :     // engine health in tracked peaks per axis
     301             :     Vector3<uint8_t> _health;
     302             :     // engine health on roll/pitch/yaw
     303             :     Vector3<uint8_t> _rpy_health;
     304             :     // averaged throttle output over averaging period
     305             :     float _avg_throttle_out;
     306             : 
     307             :     // smoothing filter on the output
     308             :     MedianLowPassFilter3dFloat _center_freq_filter[FrequencyPeak::MAX_TRACKED_PEAKS];
     309             :     // smoothing filter on the energy
     310             :     MedianLowPassFilter3dFloat _center_freq_energy_filter[FrequencyPeak::MAX_TRACKED_PEAKS];
     311             :     // smoothing filter on the bandwidth
     312             :     MedianLowPassFilter3dFloat _center_bandwidth_filter[FrequencyPeak::MAX_TRACKED_PEAKS];
     313             :     // smoothing filter on the frequency fit
     314             :     LowPassFilterConstDtFloat _harmonic_fit_filter[XYZ_AXIS_COUNT];
     315             : 
     316             :     // configured sampling rate
     317             :     uint16_t _fft_sampling_rate_hz;
     318             :     // number of cycles without a detected signal
     319             :     uint8_t _missed_cycles[XYZ_AXIS_COUNT][FrequencyPeak::MAX_TRACKED_PEAKS];
     320             :     // number of cycles where peaks have swapped places
     321             :     uint8_t _distorted_cycles[XYZ_AXIS_COUNT];
     322             :     // whether the analyzer initialized correctly
     323             :     bool _initialized;
     324             : 
     325             :     // whether the analyzer should be run
     326             :     bool _analysis_enabled ;
     327             :     // whether the update thread was created
     328             :     bool _thread_created ;
     329             :     // whether the pre-arm check has successfully completed
     330             :     bool _calibrated;
     331             :     // minimum frequency of the detection window
     332             :     AP_Int16 _fft_min_hz;
     333             :     // maximum frequency of the detection window
     334             :     AP_Int16 _fft_max_hz;
     335             :     // size of the FFT window
     336             :     AP_Int16 _window_size;
     337             :     // percentage overlap of FFT windows
     338             :     AP_Float _window_overlap;
     339             :     // overall enablement of the feature
     340             :     AP_Int8 _enable;
     341             :     // gyro rate sampling or cycle divider
     342             :     AP_Int8 _sample_mode;
     343             :     // learned throttle reference for the hover frequency
     344             :     AP_Float _throttle_ref;
     345             :     // learned hover filter frequency
     346             :     AP_Float _freq_hover_hz;
     347             :     // SNR Threshold
     348             :     AP_Float _snr_threshold_db;
     349             :     // attenuation to use for calculating the peak bandwidth at hover
     350             :     AP_Float _attenuation_power_db;
     351             :     // learned peak bandwidth at configured attenuation at hover
     352             :     AP_Float _bandwidth_hover_hz;
     353             :     // harmonic fit percentage
     354             :     AP_Int8 _harmonic_fit;
     355             :     // harmonic peak target
     356             :     AP_Int8 _harmonic_peak;
     357             :     // number of output frames to retain for averaging
     358             :     AP_Int8 _num_frames;
     359             :     // mask of IMUs to record gyro frames after the filter bank
     360             :     AP_Int32 _options;
     361             :     AP_InertialSensor* _ins;
     362             : #if DEBUG_FFT
     363             :     uint32_t _last_output_ms;
     364             :     EngineState _debug_state;
     365             :     float _debug_max_bin_freq;
     366             :     float _debug_max_freq_bin;
     367             :     uint16_t _debug_max_bin;
     368             :     float _debug_snr;
     369             : #endif
     370             : 
     371             :     static AP_GyroFFT *_singleton;
     372             : };
     373             : 
     374             : namespace AP {
     375             :     AP_GyroFFT *fft();
     376             : };
     377             : 
     378             : #endif // HAL_GYROFFT_ENABLED