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

Merge branch 'master' into development

Browse source
This commit is contained in:
borisbstyle 2016-09-11 01:16:53 +02:00
commit 05908aad4d
36 changed files with 957 additions and 183 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -159,7 +159,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];
}
@ -173,21 +172,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 {