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

Moved softfilter state items into a union

Browse source
This commit is contained in:
Martin Budden 2017-05-08 07:50:08 +01:00
parent 4be1e31d01
commit 6f5c1e9aca
1 changed files with 13 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -78,14 +78,18 @@ typedef struct gyroCalibration_s {
uint16_t calibratingG; uint16_t calibratingG;
} gyroCalibration_t; } gyroCalibration_t;
typedef union gyroSoftFilter_u {
biquadFilter_t gyroFilterLpfState[XYZ_AXIS_COUNT];
pt1Filter_t gyroFilterPt1State[XYZ_AXIS_COUNT];
firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
} gyroSoftLpfFilter_t;
typedef struct gyroSensor_s { typedef struct gyroSensor_s {
gyroDev_t gyroDev; gyroDev_t gyroDev;
gyroCalibration_t gyroCalibration; gyroCalibration_t gyroCalibration;
// gyro soft filter // gyro soft filter
filterApplyFnPtr softLpfFilterApplyFn; filterApplyFnPtr softLpfFilterApplyFn;
biquadFilter_t gyroFilterLPFState[XYZ_AXIS_COUNT]; gyroSoftLpfFilter_t softLpfFilter;
pt1Filter_t gyroFilterPt1State[XYZ_AXIS_COUNT];
firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
void *softLpfFilterPtr[XYZ_AXIS_COUNT]; void *softLpfFilterPtr[XYZ_AXIS_COUNT];
// notch filters // notch filters
filterApplyFnPtr notchFilter1ApplyFn; filterApplyFnPtr notchFilter1ApplyFn;
@ -365,23 +369,23 @@ void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz)
case FILTER_BIQUAD: case FILTER_BIQUAD:
gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->gyroFilterLPFState[axis]; gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->softLpfFilter.gyroFilterLpfState[axis];
biquadFilterInitLPF(gyroSensor->softLpfFilterPtr[axis], lpfHz, gyro.targetLooptime); biquadFilterInitLPF(&gyroSensor->softLpfFilter.gyroFilterLpfState[axis], lpfHz, gyro.targetLooptime);
} }
break; break;
case FILTER_PT1: case FILTER_PT1:
gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
const float gyroDt = (float) gyro.targetLooptime * 0.000001f; const float gyroDt = (float) gyro.targetLooptime * 0.000001f;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->gyroFilterPt1State[axis]; gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->softLpfFilter.gyroFilterPt1State[axis];
pt1FilterInit(gyroSensor->softLpfFilterPtr[axis], lpfHz, gyroDt); pt1FilterInit(&gyroSensor->softLpfFilter.gyroFilterPt1State[axis], lpfHz, gyroDt);
} }
break; break;
default: default:
gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->gyroDenoiseState[axis]; gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->softLpfFilter.gyroDenoiseState[axis];
firFilterDenoiseInit(gyroSensor->softLpfFilterPtr[axis], lpfHz, gyro.targetLooptime); firFilterDenoiseInit(&gyroSensor->softLpfFilter.gyroDenoiseState[axis], lpfHz, gyro.targetLooptime);
} }
break; break;
} }