Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

notch filter method pointer

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2017-03-11 19:18:37 +01:00
parent 888fd3f76c
commit 6e1988500d
1 changed files with 4 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/sensors/acceleration.c
View file

@ -71,6 +71,7 @@ static uint16_t calibratingA = 0; // the calibration is done is the main lo
static biquadFilter_t accFilter[XYZ_AXIS_COUNT]; static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
#ifdef USE_ACC_NOTCH #ifdef USE_ACC_NOTCH
static filterApplyFnPtr accNotchFilterApplyFn;
static void *accNotchFilter[XYZ_AXIS_COUNT]; static void *accNotchFilter[XYZ_AXIS_COUNT];
#endif #endif
@ -465,7 +466,7 @@ void accUpdate(void)
#ifdef USE_ACC_NOTCH #ifdef USE_ACC_NOTCH
if (accelerometerConfig()->acc_notch_hz) { if (accelerometerConfig()->acc_notch_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADC[axis] = lrintf(biquadFilterApply(accNotchFilter[axis], (float)acc.accADC[axis])); acc.accADC[axis] = lrintf(accNotchFilterApplyFn(accNotchFilter[axis], (float)acc.accADC[axis]));
} }
} }
#endif #endif
@ -500,8 +501,10 @@ void accInitFilters(void)
#ifdef USE_ACC_NOTCH #ifdef USE_ACC_NOTCH
static biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT]; static biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT];
accNotchFilterApplyFn = nullFilterApply;
if (acc.accTargetLooptime && accelerometerConfig()->acc_notch_hz) { if (acc.accTargetLooptime && accelerometerConfig()->acc_notch_hz) {
accNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accNotchFilter[axis] = &accFilterNotch[axis]; accNotchFilter[axis] = &accFilterNotch[axis];
biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, accelerometerConfig()->acc_notch_hz, accelerometerConfig()->acc_notch_cutoff); biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, accelerometerConfig()->acc_notch_hz, accelerometerConfig()->acc_notch_cutoff);