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

Acc notch filter hardcoded to 75Hz

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2017-03-10 22:52:17 +01:00
parent 1ee72eacd6
commit a19cdefd11
1 changed files with 11 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -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 biquadFilter_t accFilter[XYZ_AXIS_COUNT];
static void *accNotchFilter[XYZ_AXIS_COUNT];
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0); PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
@ -442,6 +444,7 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f
void accUpdate(void) void accUpdate(void)
{ {
float temp;
if (!acc.dev.read(&acc.dev)) { if (!acc.dev.read(&acc.dev)) {
return; return;
} }
@ -452,7 +455,8 @@ void accUpdate(void)
if (accelerometerConfig()->acc_lpf_hz) { if (accelerometerConfig()->acc_lpf_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { 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) void accInitFilters(void)
{ {
static biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT];
if (acc.accTargetLooptime && accelerometerConfig()->acc_lpf_hz) { if (acc.accTargetLooptime && accelerometerConfig()->acc_lpf_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accelerometerConfig()->acc_lpf_hz, acc.accTargetLooptime); biquadFilterInitLPF(&accFilter[axis], accelerometerConfig()->acc_lpf_hz, acc.accTargetLooptime);
}
accNotchFilter[axis] = &accFilterNotch[axis];
biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, 75, 30);
}
} }
} }