1. User can set sampling rate to suit expected range of frequencies:
- HIGH suits 4" or smaller and 6S 5"
- MEDIUM suits classic 5" 4S
- LOW is for 6" or greater
Limits automatically scaled:
HIGH : 133/166 to 1000Hz, MEDIUM : 89/111 to 666Hz, LOW : 67/83 to 500Hz
2. Bandpass entirely eliminated, not needed.
3. True peak detection method, favouring first peak to exceed 80% of maximum bin height; ignore or threshold values not required.
Improves on earlier 3.5RC1 dynamic filter with:
- better FFT tracking performance overall, even with a narrower default notch width
- can reach 850Hz for high kV 2.5-3" and 6S quads
- works better with 32k gyros
- can be applied pre- (location 1) and post- (location 2) static filters. Pre-static filter location works best, but post-static may work well in 32k modes or with PT1 option
- option to use a PT1 filter rather than the classical notch filter, perhaps useful for quads with a lot of noise above their peak.
- ability to totally bypass the pre-FFT bandpass filter, by setting Q=0, maximising the range of responsiveness
- "ignore" value, absolute FFT bin amplitude below which the bin will be excluded from consideration. May be tweaked to optimise peak detection; primarily for advanced tuners. If too high, only the very peaks will be included, if too low, noise may clutter peak detection.
- "threshold" value for peak detection, which, when divided by 10, is the multiple by which one bin must exceed the previous one for peak detection to commence. If too low, FFT detection becomes more random, if too high, no peaks are detected.
Revise and simplify initialization logic and avoid unneeded code if USE_DUAL_GYRO is not defined.
Corrected double constant to float.
Split centerFreq filter apply and constrain into separate statements.
For gyro loops from 2K to < 4K scale the FFT_SAMPLING_RATE_HZ to ensure we get 3 samples.
Also constrain the `centerFreq` result from the lowpass to ensure it doesn't overshoot beyond the min/max notch center cutoff.
Improves performance of the dynamic notch filter, increasing peak accuracy over a wider band of frequencies, and generally using a narrower, higher notch.
Details:
- FFT now operates on gyro data *after* gyro notches and lowpasses
- FFT bandpass Q changed from 0.707 to 0.05, to 'open up' the FFT to a greater range of incoming frequencies
- FFT centre output now ranges from about 130 to 666Hz.
- ignore the lowest couple of FFT bins going into centre frequency calculation
- analyse FFT bins from low to high, keep ignoring bins until a bin is found that exceeds its previous bin by a factor of 2; then start examining bins from the bin before that (stops the FFT from being biased low, or going to the lowest value if there is no notch at all).
- if no bin exceeds previous by more than 2 times, ie no obvious peak, smoothly go to maximum allowed notch frequency to avoid delay (might be better to bypass filter altogether?)
- dominant bin emphasised by cubing bin height before calculating mean
- maximum cutoff frequency is half the highest allowable centre frequency
- default notch width is +/-25% of centre, narrower than before most of the time
- code tidied up
- thanks to rav, Flint, UAV Tech, icr4sh, diehertz and everyone else who helped with this.
* * FAST_RAM-ing variables used to compute FFT
* Eradicated global static variables in favour of define
* FFT_WINDOW_SIZE / 2 replaced with FFT_BIN_COUNT
* Limit call count of filters update to necessary minimum on 32k and 16k gyro sampling rate
* Dynamic filter recalculation freq. is at least FFT_SAMPLING_RATE + update time
* Moved global variables used in local scope only to local scope
* * Based on diehertzs review I removed all 0 initializations of global variables
* * Fixed calculation of update frequency for center frequency filter, thx rav-rav for pointing the problem
* * Silenced the warning signed vs unsigned comparison
* * Replaced magick values 3*4 and 12 with preprocessor macro as requested by DieHertz
* * Replaced hardcoded axis count with proper preprocessor macro
change F3 from CM1 to CM4
add debug flags for FFT
add bandpass filter
add different filtering apply function
add feature DYNAMIC_FILTER
replace mode GTUNE with DYNAMIC FILTER
move gyro frequency analysis into gyro loop instead of own task
