Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

Improve peak detection

Browse source
This commit is contained in:
ctzsnooze 2018-08-15 17:20:12 +10:00
parent 14c90bf10b
commit c6a2ba18ef
1 changed files with 25 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

38
src/main/sensors/gyroanalyse.c
View file

@ -263,40 +263,46 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
float fftWeightedSum = 0; float fftWeightedSum = 0;
float dataAvg = 0; float dataAvg = 0;
float dataMax = 0; float dataMax = 0;
float dynFiltThreshold = 0; float dataThreshold;
bool fftIncreasing = false; bool fftPeakDetected = false;
bool fftPeakFinished = false; bool fftPeakFinished = false;
//get simple average and max of bin amplitudes //get average and max of bin amplitudes once they start increasing
for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) {
if (fftPeakDetected || (state->fftData[i] > state->fftData[i - 1])) {
dataAvg += state->fftData[i]; dataAvg += state->fftData[i];
fftPeakDetected = true;
if (state->fftData[i] > dataMax) { if (state->fftData[i] > dataMax) {
dataMax = state->fftData[i]; dataMax = state->fftData[i];
} }
} }
// lower Max value to catch first peak close to max }
dataMax = 0.8f * dataMax;
dataAvg = dataAvg / FFT_BIN_COUNT; dataAvg = dataAvg / FFT_BIN_COUNT;
// automatically set peak detection threshold at half difference between peak and average
dynFiltThreshold = 0.5f * (dataMax / dataAvg); //peak, once increasing, must be more than 80% above average and 1.4 times average
dataThreshold = MAX(1.4f * dataAvg, (0.8f * dataMax + 0.2f * dataAvg));
// iterate over fft data and calculate weighted indices // iterate over fft data and calculate weighted indices
fftPeakDetected = false;
for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) { for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) {
const float data = state->fftData[i]; const float data = state->fftData[i];
const float prevData = state->fftData[i - 1]; const float dataPrev = state->fftData[i - 1];
// disregard fft bins after first peak // include bins only after first peak detected
if (!fftPeakFinished) { if (!fftPeakFinished) {
// include bins around the first bin that exceeds 80% max bin height and increased compared to previous bin // peak must exceed thresholds and come after an increase in bin height
if (fftIncreasing || ((data > prevData * dynFiltThreshold) && (data > dataMax))) { if (fftPeakDetected || (data > dataPrev && data > dataThreshold)) {
// add current bin
float cubedData = data * data * data; float cubedData = data * data * data;
// add previous bin // indicate peak detected
if (!fftIncreasing) { if (!fftPeakDetected) {
cubedData += prevData * prevData * prevData; fftPeakDetected = true;
fftIncreasing = true; // accumulate previous bin
cubedData += dataPrev * dataPrev * dataPrev;
} }
// peak over when incoming bin falls below average // terminate when peak ends ie data falls below average
if (data < dataAvg) { if (data < dataAvg) {
fftPeakFinished = true; fftPeakFinished = true;
} }
//calculate sums
fftSum += cubedData; fftSum += cubedData;
// calculate weighted index starting at 1, not 0 // calculate weighted index starting at 1, not 0
fftWeightedSum += cubedData * (i + 1); fftWeightedSum += cubedData * (i + 1);