rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Change gyro filter / debug order

This commit is contained in:
borisbstyle 2016-09-09 09:55:46 +02:00
parent 389a27e3f1
commit cab7b562d8
1 changed files with 13 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/main/sensors/gyro.c
View File

@ -158,7 +158,6 @@ void gyroUpdate(void)
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis];
gyroADC[axis] = gyroADCRaw[axis];
}
@ -172,21 +171,21 @@ void gyroUpdate(void)
if (gyroSoftLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; 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);
}
if (debugMode == DEBUG_GYRO)
debug[axis] = gyroADC[axis];
if (gyroSoftLpfType == FILTER_BIQUAD)
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]);
else
gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt);
if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]);
if (gyroSoftNotchHz)
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch[axis], gyroADCf[axis]);
if (gyroSoftLpfType == FILTER_BIQUAD) {
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);
} else {
gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], sample, gyroSoftLpfHz, gyroDt);
}
gyroADC[axis] = lrintf(gyroADCf[axis]);
}
} else {