Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_GyroFFT/AP_GyroFFT.h

354 lines
16 KiB
C
Raw Normal View History

AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
/*
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>
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
#include <AP_HAL/utility/RingBuffer.h>
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <Filter/LowPassFilter.h>
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
#include <Filter/FilterWithBuffer.h>
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
#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: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
typedef AP_HAL::DSP::FrequencyPeak FrequencyPeak;
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
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);
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// cycle through the FFT steps - runs in the FFT thread
uint16_t run_cycle();
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// capture gyro values at the appropriate update rate - runs at fast loop rate
void sample_gyros();
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// update the engine state - runs at 400Hz
void update();
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// 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
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
bool start_update_thread();
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// check at startup that standard frequencies can be detected
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len);
// make sure calibration is set done
bool prepare_for_arming();
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// 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
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
const Vector3f& get_noise_center_freq_hz() const { return get_noise_center_freq_hz(FrequencyPeak::CENTER); }
const Vector3f& get_noise_center_freq_hz(FrequencyPeak peak) const { return _global_state._center_freq_hz_filtered[peak]; }
AP_GyroFFT: slew FFT frequency output
2020-07-30 15:42:40 -03:00
// slew frequency values
float get_slewed_weighted_freq_hz(FrequencyPeak peak) const;
float get_slewed_noise_center_freq_hz(FrequencyPeak peak, uint8_t axis) const;
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// energy of the background noise at the detected center frequency
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
const Vector3f& get_noise_signal_to_noise_db() const { return _global_state._center_snr; }
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// detected peak frequency weighted by energy
AP_GyroFFT: allow all three peaks to be returned so that we can have three independent harmonic notches log dynamic harmonic notch per-peak move health check into update() and make accessors const use AP_Vehicle log function when necessary
2020-05-29 13:29:15 -03:00
float get_weighted_noise_center_freq_hz() const;
// all detected peak frequencies weighted by energy
uint8_t get_weighted_noise_center_frequencies_hz(uint8_t num_freqs, float* freqs) const;
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// detected peak frequency
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
const Vector3f& get_raw_noise_center_freq_hz() const { return _global_state._center_freq_hz; }
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// match between first and second harmonics
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
const Vector3f& get_raw_noise_harmonic_fit() const { return _global_state._harmonic_fit; }
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// energy of the detected peak frequency
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
const Vector3f& get_center_freq_energy() const { return get_center_freq_energy(FrequencyPeak::CENTER); }
const Vector3f& get_center_freq_energy(FrequencyPeak peak) const { return _global_state._center_freq_energy_filtered[peak]; }
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// index of the FFT bin containing the detected peak frequency
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
const Vector3<uint16_t>& get_center_freq_bin() const { return _global_state._center_freq_bin; }
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// detected peak bandwidth
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
const Vector3f& get_noise_center_bandwidth_hz() const { return get_noise_center_bandwidth_hz(FrequencyPeak::CENTER); }
const Vector3f& get_noise_center_bandwidth_hz(FrequencyPeak peak) const { return _global_state._center_bandwidth_hz_filtered[peak]; };
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// weighted detected peak bandwidth
AP_GyroFFT: allow all three peaks to be returned so that we can have three independent harmonic notches log dynamic harmonic notch per-peak move health check into update() and make accessors const use AP_Vehicle log function when necessary
2020-05-29 13:29:15 -03:00
float get_weighted_noise_center_bandwidth_hz() const;
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// log gyro fft messages
void write_log_messages();
static const struct AP_Param::GroupInfo var_info[];
static AP_GyroFFT *get_singleton() { return _singleton; }
private:
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// 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;
// smoothing filter that first takes the median from a sliding window and then
// applies a low pass filter to the result
class MedianLowPassFilter3dFloat {
public:
MedianLowPassFilter3dFloat() { }
float apply(uint8_t axis, float sample);
float get(uint8_t axis) const { return _lowpass_filter[axis].get(); }
void set_cutoff_frequency(float sample_freq, float cutoff_freq) {
for (uint8_t i = 0; i < XYZ_AXIS_COUNT; i++) {
_lowpass_filter[i].set_cutoff_frequency(sample_freq, cutoff_freq);
}
}
private:
LowPassFilterFloat _lowpass_filter[XYZ_AXIS_COUNT];
FilterWithBuffer<float,3> _median_filter[XYZ_AXIS_COUNT];
};
// structure for holding noise peak data while calculating swaps
class FrequencyData {
public:
FrequencyData(const AP_GyroFFT& gyrofft, const EngineConfig& config);
float get_weighted_frequency(FrequencyPeak i) const { return frequency[i]; }
float get_signal_to_noise(FrequencyPeak i) const { return snr[i]; }
bool is_valid(FrequencyPeak i) const { return valid[i]; }
private:
float frequency[FrequencyPeak::MAX_TRACKED_PEAKS];
float snr[FrequencyPeak::MAX_TRACKED_PEAKS];
bool valid[FrequencyPeak::MAX_TRACKED_PEAKS];
};
// distance matrix between filtered and instantaneous peaks
typedef float DistanceMatrix[FrequencyPeak::MAX_TRACKED_PEAKS][FrequencyPeak::MAX_TRACKED_PEAKS];
// thread-local accessors of filtered state
float get_tl_noise_center_freq_hz(FrequencyPeak peak, uint8_t axis) const { return _thread_state._center_freq_hz_filtered[peak][axis]; }
float get_tl_center_freq_energy(FrequencyPeak peak, uint8_t axis) const { return _thread_state._center_freq_energy_filtered[peak][axis]; }
float get_tl_noise_center_bandwidth_hz(FrequencyPeak peak, uint8_t axis) const { return _thread_state._center_bandwidth_hz_filtered[peak][axis]; };
// thread-local mutators of filtered state
float update_tl_noise_center_freq_hz(FrequencyPeak peak, uint8_t axis, float value) {
AP_GyroFFT: slew FFT frequency output
2020-07-30 15:42:40 -03:00
_thread_state._prev_center_freq_hz_filtered[peak][axis] = _thread_state._center_freq_hz_filtered[peak][axis];
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
return (_thread_state._center_freq_hz_filtered[peak][axis] = _center_freq_filter[peak].apply(axis, value));
}
float update_tl_center_freq_energy(FrequencyPeak peak, uint8_t axis, float value) {
return (_thread_state._center_freq_energy_filtered[peak][axis] = _center_freq_energy_filter[peak].apply(axis, value));
}
float update_tl_noise_center_bandwidth_hz(FrequencyPeak peak, uint8_t axis, float value) {
return (_thread_state._center_bandwidth_hz_filtered[peak][axis] = _center_bandwidth_filter[peak].apply(axis, value));
}
// write single log mesages
AP_GyroFFT: Add missing const in member functions Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>
2021-02-01 12:26:28 -04:00
void log_noise_peak(uint8_t id, FrequencyPeak peak, float notch_freq) const;
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// calculate the peak noise frequency
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
void calculate_noise(bool calibrating, const EngineConfig& config);
// calculate noise peaks based on energy and history
uint8_t calculate_tracking_peaks(float& weighted_peak_freq_hz, float& snr, bool calibrating, const EngineConfig& config);
// calculate noise peak frequency characteristics
bool calculate_filtered_noise(FrequencyPeak target_peak, FrequencyPeak source_peak, const FrequencyData& freqs, const EngineConfig& config);
// get the weighted frequency
bool get_weighted_frequency(FrequencyPeak peak, float& weighted_peak_freq_hz, float& snr, const EngineConfig& config) const;
// return the tracked noise peak
FrequencyPeak get_tracked_noise_peak() const;
// calculate the distance matrix between the current estimates and the current cycle
void find_distance_matrix(DistanceMatrix& distance_matrix, const FrequencyData& freqs, const EngineConfig& config) const;
// return the instantaneous peak that is closest to the target estimate peak
FrequencyPeak find_closest_peak(const FrequencyPeak target, const DistanceMatrix& distance_matrix, uint8_t ignore = 0) const;
// detected peak frequency weighted by energy
AP_GyroFFT: allow all three peaks to be returned so that we can have three independent harmonic notches log dynamic harmonic notch per-peak move health check into update() and make accessors const use AP_Vehicle log function when necessary
2020-05-29 13:29:15 -03:00
float calculate_weighted_freq_hz(const Vector3f& energy, const Vector3f& freq) const;
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// 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
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
float self_test(float frequency, FloatBuffer& test_window);
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// 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();
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// return samples available in the gyro window
uint16_t get_available_samples(uint8_t axis) {
return _sample_mode == 0 ?_ins->get_raw_gyro_window(axis).available() : _downsampled_gyro_data[axis].available();
}
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// semaphore for access to shared FFT data
HAL_Semaphore _sem;
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// data set from the FFT thread but accessible from the main thread protected by the semaphore
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
struct EngineState {
// 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;
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// bin of detected peak frequency
AP_GyroFFT: slew FFT frequency output
2020-07-30 15:42:40 -03:00
Vector3ui _center_freq_bin;
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// fft engine health
uint8_t _health;
AP_GyroFFT: slew FFT frequency output
2020-07-30 15:42:40 -03:00
Vector3ul _health_ms;
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// fft engine output rate
uint32_t _output_cycle_ms;
// tracked frequency peak
Vector3<uint8_t> _tracked_peak;
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// signal to noise ratio of PSD at the detected centre frequency
Vector3f _center_snr;
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// filtered version of the peak frequency
Vector3f _center_freq_hz_filtered[FrequencyPeak::MAX_TRACKED_PEAKS];
AP_GyroFFT: slew FFT frequency output
2020-07-30 15:42:40 -03:00
// previous filtered version of the peak frequency
Vector3f _prev_center_freq_hz_filtered[FrequencyPeak::MAX_TRACKED_PEAKS];
// when we last calculated a value
Vector3ul _last_output_us;
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// filtered energy of the detected peak frequency
Vector3f _center_freq_energy_filtered[FrequencyPeak::MAX_TRACKED_PEAKS];
// filtered detected peak width
Vector3f _center_bandwidth_hz_filtered[FrequencyPeak::MAX_TRACKED_PEAKS];
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// axes that still require noise calibration
uint8_t _noise_needs_calibration : 3;
// whether the analyzer is mid-cycle
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
bool _analysis_started;
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
};
// Shared FFT engine state local to the FFT thread
EngineState _thread_state;
// Shared FFT engine state accessible by the main thread
EngineState _global_state;
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// number of samples needed before a new frame can be processed
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
uint16_t _samples_per_frame;
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// number of ms that a frame should take to process to sustain output rate
uint16_t _frame_time_ms;
// last cycle time
uint32_t _output_cycle_micros;
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// downsampled gyro data circular buffer for frequency analysis
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
FloatBuffer _downsampled_gyro_data[XYZ_AXIS_COUNT];
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// accumulator for sampled gyro data
Vector3f _oversampled_gyro_accum;
// count of oversamples
uint16_t _oversampled_gyro_count;
// 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;
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// searched harmonics - inferred from harmonic notch harmonics
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
uint8_t _harmonics;
AP_GyroFFT: allow all three peaks to be returned so that we can have three independent harmonic notches log dynamic harmonic notch per-peak move health check into update() and make accessors const use AP_Vehicle log function when necessary
2020-05-29 13:29:15 -03:00
// engine health in tracked peaks
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
uint8_t _health;
AP_GyroFFT: allow all three peaks to be returned so that we can have three independent harmonic notches log dynamic harmonic notch per-peak move health check into update() and make accessors const use AP_Vehicle log function when necessary
2020-05-29 13:29:15 -03:00
// engine health on roll/pitch/yaw
Vector3<uint8_t> _rpy_health;
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// smoothing filter on the output
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
MedianLowPassFilter3dFloat _center_freq_filter[FrequencyPeak::MAX_TRACKED_PEAKS];
// smoothing filter on the energy
MedianLowPassFilter3dFloat _center_freq_energy_filter[FrequencyPeak::MAX_TRACKED_PEAKS];
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// smoothing filter on the bandwidth
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
MedianLowPassFilter3dFloat _center_bandwidth_filter[FrequencyPeak::MAX_TRACKED_PEAKS];
// smoothing filter on the frequency fit
LowPassFilterFloat _harmonic_fit_filter[XYZ_AXIS_COUNT];
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// configured sampling rate
uint16_t _fft_sampling_rate_hz;
// number of cycles without a detected signal
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
uint8_t _missed_cycles[XYZ_AXIS_COUNT][FrequencyPeak::MAX_TRACKED_PEAKS];
// number of cycles without a detected signal
uint8_t _distorted_cycles[XYZ_AXIS_COUNT];
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// whether the analyzer initialized correctly
bool _initialized;
// whether the analyzer should be run
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
bool _analysis_enabled ;
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// whether the update thread was created
AP_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
bool _thread_created ;
// whether the pre-arm check has successfully completed
bool _calibrated;
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
// 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_GyroFFT: filter energy, harmonic and amplitude fit. Make a better guess at harmonic matching add filtered second and third harmonics and log them make sure we use all of the gyro samples available on each axis rather than skipping separate gyro update from fft start to minimize time in fast loop add FFT_HMNC_PEAK to allow users to select which noise peak and which axis will be tracked. make sure the self-test runs once and display the results report self-test failure reason. make sure self-test runs for all windows. always give logging a chance to run at IO_PRIORITY add log message documentation make sure the engine still runs even when the arming check has been disabled record last FFT update time and cycle time and fallback to throttle estimate when update is too old delay for longer in FFT thread between cycles to cope on F4 try really hard to get a viable frequency estimate change range on MAXHZ/MINHZ to reflect that 50Hz is actually quite dangerous swap the center peak for one of the shoulders if there is temporarily a closer match with the frequency trend when FFT is disabled still log harmonic notch frequency use distance matrix to find most appropriate peak use a median filter to remove outliers before filtering discount peaks that are relatively too low in energy make sure harmonic fit is tracked for both potential targets convert all gyro buffers to ObjectBuffer<float> for lock-free access run all FFT steps inside the FFT thread calculate cycle time and loop delay correctly drop samples when the ring buffer is full
2020-02-24 17:56:14 -04:00
// harmonic fit percentage
AP_Int8 _harmonic_fit;
// harmonic peak target
AP_Int8 _harmonic_peak;
AP_GyroFFT: new FFT library for motor peak analysis using HAL DSP abstraction add dynamic gyro windows control inclusion based on HAL_WITH_DSP and GYROFFT_ENABLED target appropriate ARM cpus constrain window sizes to be achievable improve FFT signal accuracy through configurable window overlap and quinn's estimator calculate energy weighted center frequency add support for learning hover frequency and throttle reference calculate power spectrum rather than amplitude record noise as a per-bin power spectrum calculate true SNR per-bin and use that to determine there is a signal add user config for SNR signal level constrain frequency scanning to MAXHZ calculate and learn the peak bandwidth at the configured attenuation allow enabling/disabling dynamically through rc function MAXHZ should be below Nyquist Incorporate full range of MAXHZ to MINHZ update FFT analysis in a separate thread allocate memory in a specific region constrain window size by CPU class do not allocate any resources when not enabled Prevent self-check and analysis interfering with each other put configuration and initialization to AP_Vehicle add logging fix significant issues with threading, locking and sample buffer access use both calculated noise peaks together with the harmonic notch configuration to detemine which peak represents the fundamental harmonic that should be tracked record harmonic fit add CMSIS 5 libraries and headers allow larger FFT lengths on Linux and SITL
2019-08-15 18:12:20 -03:00
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