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

set lower filter cutoff instead of Q

Browse source 
fix error in gyro pt1 filter
This commit is contained in:
rav 2016-08-03 23:45:52 +02:00
parent 258e05e9c5
commit 0e905e01d3
10 changed files with 21 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -55,16 +55,13 @@ static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0;
static float gyroDt;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_bw, uint8_t gyro_soft_lpf_type)
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint16_t gyro_soft_notch_cutoff, uint8_t gyro_soft_lpf_type)
{
gyroConfig = gyroConfigToUse;
gyroSoftLpfHz = gyro_soft_lpf_hz;
gyroSoftNotchHz = gyro_soft_notch_hz;
gyroSoftNotchQ = gyro_soft_notch_bw;
gyroSoftLpfType = gyro_soft_lpf_type;
float octaves = log2f(((float) gyro_soft_notch_hz + gyro_soft_notch_bw / 2) / ((float) gyro_soft_notch_hz - gyro_soft_notch_bw / 2));
gyroSoftNotchQ = sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
gyroSoftNotchQ = filterGetNotchQ(gyro_soft_notch_hz, gyro_soft_notch_cutoff);
}
void gyroInit(void)
@ -75,7 +72,7 @@ void gyroInit(void)
if (gyroSoftLpfType == FILTER_BIQUAD)
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
else
gyroDt = gyro.targetLooptime / 1000.0f;
gyroDt = (float) gyro.targetLooptime * 0.000001f;
}
}
}