From a19cdefd11ceaa30b36da171969a55ed79a41ad7 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 10 Mar 2017 22:52:17 +0100 Subject: [PATCH] Acc notch filter hardcoded to 75Hz --- src/main/sensors/acceleration.c | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 96523d6433..667eaf918a 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -70,6 +70,8 @@ static uint16_t calibratingA = 0; // the calibration is done is the main lo static biquadFilter_t accFilter[XYZ_AXIS_COUNT]; +static void *accNotchFilter[XYZ_AXIS_COUNT]; + PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0); void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) @@ -442,6 +444,7 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f void accUpdate(void) { + float temp; if (!acc.dev.read(&acc.dev)) { return; } @@ -452,7 +455,8 @@ void accUpdate(void) if (accelerometerConfig()->acc_lpf_hz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - acc.accADC[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)acc.accADC[axis])); + temp = biquadFilterApply(&accFilter[axis], (float)acc.accADC[axis]); + acc.accADC[axis] = lrintf(biquadFilterApply(accNotchFilter[axis], temp)); } } @@ -478,10 +482,15 @@ void accSetCalibrationValues(void) void accInitFilters(void) { + static biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT]; + if (acc.accTargetLooptime && accelerometerConfig()->acc_lpf_hz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { biquadFilterInitLPF(&accFilter[axis], accelerometerConfig()->acc_lpf_hz, acc.accTargetLooptime); - } + + accNotchFilter[axis] = &accFilterNotch[axis]; + biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, 75, 30); + } } }