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

enable frequency analysis and automatic, dynamic changing of notch filter frequencies

Browse source 
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
This commit is contained in:
rav 2017-05-11 16:10:29 +02:00
parent f55077673d
commit d9909b91d3
39 changed files with 449 additions and 70 deletions
Download patch file Download diff file Expand all files Collapse all files

45
src/main/sensors/gyro.c
View file

@ -102,6 +102,9 @@ static gyroSensor_t gyroSensor0;
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
static filterApplyFnPtr notchFilterDynApplyFn;
biquadFilter_t *notchFilterDyn[3];
#define DEBUG_GYRO_CALIBRATION 3
#ifdef STM32F10X
@ -367,7 +370,7 @@ void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz)
if (lpfHz && lpfHz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known
switch (gyroConfig()->gyro_soft_lpf_type) {
case FILTER_BIQUAD:
gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2;
for (int axis = 0; axis < 3; axis++) {
gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->softLpfFilter.gyroFilterLpfState[axis];
biquadFilterInitLPF(&gyroSensor->softLpfFilter.gyroFilterLpfState[axis], lpfHz, gyro.targetLooptime);
@ -397,7 +400,7 @@ void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
gyroSensor->notchFilter1ApplyFn = nullFilterApply;
const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
if (notchHz && notchHz <= gyroFrequencyNyquist) {
gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroSensor->notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
@ -411,7 +414,7 @@ void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
gyroSensor->notchFilter2ApplyFn = nullFilterApply;
const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
if (notchHz && notchHz <= gyroFrequencyNyquist) {
gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2;
const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroSensor->notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
@ -426,9 +429,23 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
}
void gyroInitFilterDynamicNotch()
{
static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT];
notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApply; //must be this function, do not change
const float notchQ = filterGetNotchQ(400, 390); //just any init value
for (int axis = 0; axis < 3; axis++) {
notchFilterDyn[axis] = &gyroFilterNotch[axis];
biquadFilterInit(notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);
}
}
void gyroInitFilters(void)
{
gyroInitSensorFilters(&gyroSensor0);
gyroInitFilterDynamicNotch();
}
bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
@ -506,6 +523,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
void gyroUpdateSensor(gyroSensor_t *gyroSensor)
{
if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
return;
}
gyroSensor->gyroDev.dataReady = false;
@ -531,14 +549,27 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
// scale gyro output to degrees per second
float gyroADCf = (float)gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
#ifdef USE_GYRO_DATA_ANALYSE
// Apply Dynamic Notch filtering
if (axis == 0)
DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
if (isDynamicFilterActive())
gyroADCf = notchFilterDynApplyFn(notchFilterDyn[axis], gyroADCf);
if (axis == 0)
DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
#endif
// Apply Static Notch filtering
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
gyroADCf = gyroSensor->notchFilter1ApplyFn(&gyroSensor->notchFilter1[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf);
// Apply LPF
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
// Apply Notch filtering
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
gyroADCf = gyroSensor->notchFilter1ApplyFn(&gyroSensor->notchFilter1[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf;
}