/
gyroanalyse.c
375 lines (309 loc) · 16.8 KB
/
gyroanalyse.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software 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.
*
* Cleanflight and Betaflight are distributed in the hope that they
* 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 software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
/* original work by Rav
*
* 2018_07 updated by ctzsnooze to post filter, wider Q, different peak detection
* coding assistance and advice from DieHertz, Rav, eTracer
* test pilots icr4sh, UAV Tech, Flint723
*
* 2021_02 updated by KarateBrot: switched FFT with SDFT, multiple notches per axis
* test pilots: Sugar K, bizmar
*/
#include <math.h>
#include <stdint.h>
#include "platform.h"
#ifdef USE_GYRO_DATA_ANALYSE
#include "build/debug.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/sdft.h"
#include "common/time.h"
#include "common/utils.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/time.h"
#include "sensors/gyro.h"
#include "fc/core.h"
#include "gyroanalyse.h"
// SDFT_SAMPLE_SIZE defaults to 72 (common/sdft.h).
// We get 36 frequency bins from 72 consecutive data values, called SDFT_BIN_COUNT (common/sdft.h)
// Bin 0 is DC and can't be used.
// Only bins 1 to 35 are usable.
// A gyro sample is collected every PID loop.
// maxSampleCount recent gyro values are accumulated and averaged
// to ensure that 72 samples are collected at the right rate for the required SDFT bandwidth.
// For an 8k PID loop, at default 600hz max, 6 sequential gyro data points are averaged, SDFT runs 1333Hz.
// Upper limit of SDFT is half that frequency, eg 666Hz by default.
// At 8k, if user sets a max of 300Hz, int(8000/600) = 13, sdftSampleRateHz = 615Hz, range 307Hz.
// Note that lower max requires more samples to be averaged, increasing precision but taking longer to get enough samples.
// For Bosch at 3200Hz gyro, max of 600, int(3200/1200) = 2, sdftSampleRateHz = 1600, range to 800hz.
// For Bosch on XClass, better to set a max of 300, int(3200/600) = 5, sdftSampleRateHz = 640, range to 320Hz.
// When sampleCount reaches maxSampleCount, the averaged gyro value is put into the corresponding SDFT.
// At 8k, with 600Hz max, maxSampleCount = 6, this happens every 6 * 0.125us, or every 0.75ms.
// Hence to completely replace all 72 samples of the SDFT input buffer with clean new data takes 54ms.
// The SDFT code is split into steps. It takes 4 PID loops to calculate the SDFT, track peaks and update the filters for one axis.
// Since there are three axes, it takes 12 PID loops to completely update all axes.
// At 8k, any one axis gets updated at 8000 / 12 or 666hz or every 1.5ms
// In this time, 2 points in the SDFT buffer will have changed.
// At 4k, it takes twice as long to update an axis, i.e. each axis updates only every 3ms.
// Four points in the buffer will have changed in that time, and each point will be the average of three samples.
// Hence output jitter at 4k is about four times worse than at 8k. At 2k output jitter is quite bad.
// Each SDFT output bin has width sdftSampleRateHz/72, ie 18.5Hz per bin at 1333Hz.
// Usable bandwidth is half this, ie 666Hz if sdftSampleRateHz is 1333Hz, i.e. bin 1 is 18.5Hz, bin 2 is 37.0Hz etc.
#define DYN_NOTCH_SMOOTH_HZ 4
#define DYN_NOTCH_CALC_TICKS (XYZ_AXIS_COUNT * STEP_COUNT) // 3 axes and 4 steps per axis
#define DYN_NOTCH_OSD_MIN_THROTTLE 20
typedef enum {
STEP_WINDOW,
STEP_DETECT_PEAKS,
STEP_CALC_FREQUENCIES,
STEP_UPDATE_FILTERS,
STEP_COUNT
} step_e;
typedef struct peak_s {
uint8_t bin;
float value;
} peak_t;
static sdft_t FAST_DATA_ZERO_INIT sdft[XYZ_AXIS_COUNT];
static peak_t FAST_DATA_ZERO_INIT peaks[DYN_NOTCH_COUNT_MAX];
static float FAST_DATA_ZERO_INIT sdftData[SDFT_BIN_COUNT];
static uint16_t FAST_DATA_ZERO_INIT sdftSampleRateHz;
static float FAST_DATA_ZERO_INIT sdftResolutionHz;
static uint8_t FAST_DATA_ZERO_INIT sdftStartBin;
static uint8_t FAST_DATA_ZERO_INIT sdftEndBin;
static float FAST_DATA_ZERO_INIT sdftMeanSq;
static uint16_t FAST_DATA_ZERO_INIT dynNotchBandwidthHz;
static uint16_t FAST_DATA_ZERO_INIT dynNotchMinHz;
static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxHz;
static uint16_t FAST_DATA_ZERO_INIT dynNotchMaxFFT;
static float FAST_DATA_ZERO_INIT smoothFactor;
static uint8_t FAST_DATA_ZERO_INIT numSamples;
void gyroDataAnalyseInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
{
// initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
dynNotchBandwidthHz = gyroConfig()->dyn_notch_bandwidth_hz;
dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
dynNotchMaxHz = MAX(2 * dynNotchMinHz, gyroConfig()->dyn_notch_max_hz);
// gyroDataAnalyse() is running at targetLoopRateHz (which is PID loop rate aka. 1e6f/gyro.targetLooptimeUs)
const int32_t targetLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
numSamples = MAX(1, targetLoopRateHz / (2 * dynNotchMaxHz)); // 600hz, 8k looptime, 6.00
sdftSampleRateHz = targetLoopRateHz / numSamples;
// eg 8k, user max 600hz, int(8000/1200) = 6 (6.666), sdftSampleRateHz = 1333hz, range 666Hz
// eg 4k, user max 600hz, int(4000/1200) = 3 (3.333), sdftSampleRateHz = 1333hz, range 666Hz
// eg 2k, user max 600hz, int(2000/1200) = 1 (1.666) sdftSampleRateHz = 2000hz, range 1000Hz
// eg 2k, user max 400hz, int(2000/800) = 2 (2.5) sdftSampleRateHz = 1000hz, range 500Hz
// eg 1k, user max 600hz, int(1000/1200) = 1 (max(1,0.8333)) sdftSampleRateHz = 1000hz, range 500Hz
// the upper limit of DN is always going to be the Nyquist frequency (= sampleRate / 2)
sdftResolutionHz = (float)sdftSampleRateHz / SDFT_SAMPLE_SIZE; // 13.3hz per bin at 8k
sdftStartBin = MAX(2, lrintf(dynNotchMinHz / sdftResolutionHz + 0.5f)); // can't use bin 0 because it is DC.
sdftEndBin = MIN(SDFT_BIN_COUNT - 1, lrintf(dynNotchMaxHz / sdftResolutionHz + 0.5f)); // can't use more than SDFT_BIN_COUNT bins.
smoothFactor = pt1FilterGain(DYN_NOTCH_SMOOTH_HZ, DYN_NOTCH_CALC_TICKS / (float)targetLoopRateHz); // minimum PT1 k value
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
sdftInit(&sdft[axis], sdftStartBin, sdftEndBin, numSamples);
}
state->maxSampleCount = numSamples;
state->maxSampleCountRcp = 1.0f / state->maxSampleCount;
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
// any init value is fine, but evenly spreading centerFreqs across frequency range makes notch filters stick to peaks quicker
state->centerFreq[axis][p] = (p + 0.5f) * (dynNotchMaxHz - dynNotchMinHz) / (float)gyro.notchFilterDynCount + dynNotchMinHz;
}
}
}
// Collect gyro data, to be downsampled and analysed in gyroDataAnalyse() function
void gyroDataAnalysePush(gyroAnalyseState_t *state, const uint8_t axis, const float sample)
{
state->oversampledGyroAccumulator[axis] += sample;
}
static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state);
// Downsample and analyse gyro data
FAST_CODE void gyroDataAnalyse(gyroAnalyseState_t *state)
{
// samples should have been pushed by `gyroDataAnalysePush`
// if gyro sampling is > 1kHz, accumulate and average multiple gyro samples
if (state->sampleCount == state->maxSampleCount) {
state->sampleCount = 0;
// calculate mean value of accumulated samples
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
const float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
state->downsampledGyroData[axis] = sample;
if (axis == 0) {
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
}
state->oversampledGyroAccumulator[axis] = 0;
}
// We need DYN_NOTCH_CALC_TICKS ticks to update all axes with newly sampled value
// recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
// at 8kHz PID loop rate this means 8kHz / 4 / 3 = 666Hz => update every 1.5ms
// at 4kHz PID loop rate this means 4kHz / 4 / 3 = 333Hz => update every 3ms
state->updateTicks = DYN_NOTCH_CALC_TICKS;
}
// 2us @ F722
// SDFT processing in batches to synchronize with incoming downsampled data
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
sdftPushBatch(&sdft[axis], &state->downsampledGyroData[axis], &state->sampleCount);
}
state->sampleCount++;
// Find frequency peaks and update filters
if (state->updateTicks > 0) {
gyroDataAnalyseUpdate(state);
--state->updateTicks;
}
}
// Find frequency peaks and update filters
static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
{
uint32_t startTime = 0;
if (debugMode == (DEBUG_FFT_TIME)) {
startTime = micros();
}
DEBUG_SET(DEBUG_FFT_TIME, 0, state->updateStep);
switch (state->updateStep) {
case STEP_WINDOW: // 6us @ F722
{
sdftWinSq(&sdft[state->updateAxis], sdftData);
// Calculate mean square over frequency range (= average power of vibrations)
sdftMeanSq = 0.0f;
for (uint8_t bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) { // don't use startBin or endBin because they are not windowed properly
sdftMeanSq += sdftData[bin]; // sdftData is already squared (see sdftWinSq)
}
sdftMeanSq /= sdftEndBin - sdftStartBin - 1;
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break;
}
case STEP_DETECT_PEAKS: // 6us @ F722
{
// Get memory ready for new peak data on current axis
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
peaks[p].bin = 0;
peaks[p].value = 0.0f;
}
// Search for N biggest peaks in frequency spectrum
for (uint8_t bin = (sdftStartBin + 1); bin < sdftEndBin; bin++) {
// Check if bin is peak
if ((sdftData[bin] > sdftData[bin - 1]) && (sdftData[bin] > sdftData[bin + 1])) {
// Check if peak is big enough to be one of N biggest peaks.
// If so, insert peak and sort peaks in descending height order
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
if (sdftData[bin] > peaks[p].value) {
for (uint8_t k = gyro.notchFilterDynCount - 1; k > p; k--) {
peaks[k] = peaks[k - 1];
}
peaks[p].bin = bin;
peaks[p].value = sdftData[bin];
break;
}
}
bin++; // If bin is peak, next bin can't be peak => jump it
}
}
// Sort N biggest peaks in ascending bin order (example: 3, 8, 25, 0, 0, ..., 0)
for (uint8_t p = gyro.notchFilterDynCount - 1; p > 0; p--) {
for (uint8_t k = 0; k < p; k++) {
// Swap peaks but ignore swapping void peaks (bin = 0). This leaves
// void peaks at the end of peaks array without moving them
if (peaks[k].bin > peaks[k + 1].bin && peaks[k + 1].bin != 0) {
peak_t temp = peaks[k];
peaks[k] = peaks[k + 1];
peaks[k + 1] = temp;
}
}
}
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break;
}
case STEP_CALC_FREQUENCIES: // 4us @ F722
{
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
// Only update state->centerFreq if there is a peak (ignore void peaks) and if peak is above noise floor
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) {
// accumulate sdftSum and sdftWeightedSum from peak bin, and shoulder bins either side of peak
float squaredData = peaks[p].value; // peak data already squared (see sdftWinSq)
float sdftSum = squaredData;
float sdftWeightedSum = squaredData * peaks[p].bin;
// accumulate upper shoulder unless it would be sdftEndBin
uint8_t shoulderBin = peaks[p].bin + 1;
if (shoulderBin < sdftEndBin) {
squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq)
sdftSum += squaredData;
sdftWeightedSum += squaredData * shoulderBin;
}
// accumulate lower shoulder unless lower shoulder would be bin 0 (DC)
if (peaks[p].bin > 1) {
shoulderBin = peaks[p].bin - 1;
squaredData = sdftData[shoulderBin]; // sdftData already squared (see sdftWinSq)
sdftSum += squaredData;
sdftWeightedSum += squaredData * shoulderBin;
}
// get centerFreq in Hz from weighted bins
float centerFreq = dynNotchMaxHz;
float sdftMeanBin = 0;
if (sdftSum > 0) {
sdftMeanBin = (sdftWeightedSum / sdftSum);
centerFreq = sdftMeanBin * sdftResolutionHz;
centerFreq = constrainf(centerFreq, dynNotchMinHz, dynNotchMaxHz);
// In theory, the index points to the centre frequency of the bin.
// at 1333hz, bin widths are 13.3Hz, so bin 2 (26.7Hz) has the range 20Hz to 33.3Hz
// Rav feels that maybe centerFreq = (sdftMeanBin + 0.5) * sdftResolutionHz is better
// empirical checking shows that not adding 0.5 works better
// PT1 style dynamic smoothing moves rapidly towards big peaks and slowly away, up to 8x faster
// DYN_NOTCH_SMOOTH_HZ = 4 & dynamicFactor = 1 .. 8 => PT1 -3dB cutoff frequency = 4Hz .. 41Hz
const float dynamicFactor = constrainf(peaks[p].value / sdftMeanSq, 1.0f, 8.0f);
state->centerFreq[state->updateAxis][p] += smoothFactor * dynamicFactor * (centerFreq - state->centerFreq[state->updateAxis][p]);
}
}
}
if(calculateThrottlePercentAbs() > DYN_NOTCH_OSD_MIN_THROTTLE) {
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis][p]);
}
}
if (state->updateAxis == gyro.gyroDebugAxis) {
for (uint8_t p = 0; p < gyro.notchFilterDynCount && p < 3; p++) {
DEBUG_SET(DEBUG_FFT_FREQ, p, lrintf(state->centerFreq[state->updateAxis][p]));
}
DEBUG_SET(DEBUG_DYN_LPF, 1, lrintf(state->centerFreq[state->updateAxis][0]));
}
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break;
}
case STEP_UPDATE_FILTERS: // 7us @ F722
{
for (uint8_t p = 0; p < gyro.notchFilterDynCount; p++) {
// Only update notch filter coefficients if the corresponding peak got its center frequency updated in the previous step
if (peaks[p].bin != 0 && peaks[p].value > sdftMeanSq) {
// Choose notch Q in such a way that notch bandwidth stays constant (improves prop wash handling)
float dynamicQ = state->centerFreq[state->updateAxis][p] / (float)dynNotchBandwidthHz;
dynamicQ = constrainf(dynamicQ, 2.0f, 10.0f);
biquadFilterUpdate(&gyro.notchFilterDyn[state->updateAxis][p], state->centerFreq[state->updateAxis][p], gyro.targetLooptime, dynamicQ, FILTER_NOTCH);
}
}
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
}
}
state->updateStep = (state->updateStep + 1) % STEP_COUNT;
}
uint16_t getMaxFFT(void) {
return dynNotchMaxFFT;
}
void resetMaxFFT(void) {
dynNotchMaxFFT = 0;
}
#endif // USE_GYRO_DATA_ANALYSE