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

Cleanup // Fix some filter inits // More space Naze target

Browse source
This commit is contained in:
borisbstyle 2016-10-09 00:45:02 +02:00
parent 81623d4ac7
commit 70cca63875
6 changed files with 50 additions and 47 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -51,8 +51,8 @@ static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_A
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
static denoisingState_t gyroDenoiseState[XYZ_AXIS_COUNT];
static uint8_t gyroSoftLpfType;
static uint16_t gyroSoftNotchHz_1, gyroSoftNotchHz_2;
static float gyroSoftNotchQ_1, gyroSoftNotchQ_2;
static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2;
static float gyroSoftNotchQ1, gyroSoftNotchQ2;
static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0;
static float gyroDt;
@ -67,11 +67,11 @@ void gyroUseConfig(const gyroConfig_t *gyroConfigToUse,
{
gyroConfig = gyroConfigToUse;
gyroSoftLpfHz = gyro_soft_lpf_hz;
gyroSoftNotchHz_1 = gyro_soft_notch_hz_1;
gyroSoftNotchHz_2 = gyro_soft_notch_hz_2;
gyroSoftNotchHz1 = gyro_soft_notch_hz_1;
gyroSoftNotchHz2 = gyro_soft_notch_hz_2;
gyroSoftLpfType = gyro_soft_lpf_type;
gyroSoftNotchQ_1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1);
gyroSoftNotchQ_2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2);
gyroSoftNotchQ1 = filterGetNotchQ(gyro_soft_notch_hz_1, gyro_soft_notch_cutoff_1);
gyroSoftNotchQ2 = filterGetNotchQ(gyro_soft_notch_hz_2, gyro_soft_notch_cutoff_2);
}
void gyroInit(void)
@ -80,15 +80,17 @@ void gyroInit(void)
for (int axis = 0; axis < 3; axis++) {
if (gyroSoftLpfType == FILTER_BIQUAD)
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
else
else if (gyroSoftLpfType == FILTER_PT1)
gyroDt = (float) gyro.targetLooptime * 0.000001f;
else
initDenoisingFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
}
}
if ((gyroSoftNotchHz_1 || gyroSoftNotchHz_2) && gyro.targetLooptime) {
if ((gyroSoftNotchHz1 || gyroSoftNotchHz2) && gyro.targetLooptime) {
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz_1, gyro.targetLooptime, gyroSoftNotchQ_1, FILTER_NOTCH);
biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz_2, gyro.targetLooptime, gyroSoftNotchQ_2, FILTER_NOTCH);
biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
}
}
}
@ -201,10 +203,10 @@ void gyroUpdate(void)
if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]);
if (gyroSoftNotchHz_1)
if (gyroSoftNotchHz1)
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]);
if (gyroSoftNotchHz_2)
if (gyroSoftNotchHz2)
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);