Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 04:45:24 +03:00
Code Issues Wiki Activity

Tidied gyro filter code

Browse source
This commit is contained in:
Martin Budden 2017-12-30 00:12:49 +00:00
parent 5a448ea9b0
commit 3125ae252b
5 changed files with 36 additions and 42 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -29,21 +29,12 @@
#include "common/time.h"
#include "common/utils.h"
#include "config/feature.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/time.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "sensors/gyro.h"
#include "sensors/gyroanalyse.h"
#include "common/filter.h"
// The FFT splits the frequency domain into an number of bins
// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each with a width 31.25Hz
// Eg [0,31), [31,62), [62, 93) etc
@ -136,20 +127,11 @@ const gyroFftData_t *gyroFftData(int axis)
return &fftResult[axis];
}
bool isDynamicFilterActive(void)
{
return feature(FEATURE_DYNAMIC_FILTER);
}
/*
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
*/
void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn)
{
if (!isDynamicFilterActive()) {
return;
}
// if gyro sampling is > 1kHz, accumulate multiple samples
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
fftAcc[axis] += gyroDev->gyroADC[axis];