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

Merge pull request #684 from rav-rav/notch

Browse source 
Smoother filtering (additional notch filter)
This commit is contained in:
borisbstyle 2016-07-16 10:30:47 +02:00 • committed by GitHub
commit 4fff6c89bf
12 changed files with 71 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -27,6 +27,7 @@
#include "common/filter.h"
#include "drivers/sensor.h"
#include "drivers/system.h"
#include "drivers/accgyro.h"
#include "io/beeper.h"
@ -44,21 +45,27 @@ float gyroADCf[XYZ_AXIS_COUNT];
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
static const gyroConfig_t *gyroConfig;
static biquadFilter_t gyroFilter[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT];
static uint16_t gyroSoftNotchHz;
static uint8_t gyroSoftNotchQ;
static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz)
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q)
{
gyroConfig = gyroConfigToUse;
gyroSoftLpfHz = gyro_soft_lpf_hz;
gyroSoftNotchHz = gyro_soft_notch_hz;
gyroSoftNotchQ = gyro_soft_notch_q;
}
void gyroInit(void)
{
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilter[axis], gyroSoftLpfHz, gyro.targetLooptime);
biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH);
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
}
}
}
@ -158,7 +165,17 @@ void gyroUpdate(void)
if (gyroSoftLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADCf[axis] = biquadFilterApply(&gyroFilter[axis], (float)gyroADC[axis]);
float sample = (float) gyroADC[axis];
if (gyroSoftNotchHz) {
sample = biquadFilterApply(&gyroFilterNotch[axis], sample);
}
if (debugMode == DEBUG_NOTCH && axis < 2){
debug[axis*2 + 0] = gyroADC[axis];
debug[axis*2 + 1] = lrintf(sample);
}
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);;
gyroADC[axis] = lrintf(gyroADCf[axis]);
}
} else {