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

Filter Cleanup CF coding style// Remove Old pt1 for acc etc // F1 slower acc update

Browse source
This commit is contained in:
borisbstyle 2016-01-17 22:33:30 +01:00
parent e15ee54513
commit a105af1225
11 changed files with 52 additions and 64 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/main/flight/imu.c
View file

@ -377,7 +377,8 @@ static bool isMagnetometerHealthy(void)
static void imuCalculateEstimatedAttitude(void)
{
static filterStatePt1_t accLPFState[3];
static biquad_t accLPFState[3];
static bool accStateIsSet;
static uint32_t previousIMUUpdateTime;
float rawYawError = 0;
int32_t axis;
@ -391,8 +392,13 @@ static void imuCalculateEstimatedAttitude(void)
// Smooth and use only valid accelerometer readings
for (axis = 0; axis < 3; axis++) {
if (imuRuntimeConfig->acc_cut_hz > 0) {
accSmooth[axis] = filterApplyPt1(accADC[axis], &accLPFState[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6f);
if (imuRuntimeConfig->acc_cut_hz) {
if (accStateIsSet) {
accSmooth[axis] = lrintf(applyBiQuadFilter((float) accADC[axis], &accLPFState[axis]));
} else {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(imuRuntimeConfig->acc_cut_hz, &accLPFState[axis], 1000);
accStateIsSet = true;
}
} else {
accSmooth[axis] = accADC[axis];
}