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

Apply accz pt1 element fixes. 

See:
f7132b9d33211a55b8bd2c92f14da59f35b715a4
b1f58bc01fcf1632b3c3947a5ed0e807c2763a30
5832e2f67fcd2ee93044b300fda37a420c595813
a5961aeda657225fa843a115a94229313db4b2db
This commit is contained in:
Dominic Clifton 2014-08-01 21:10:26 +01:00
parent 9a9ff9b1ad
commit 38cbe28e0f
1 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/main/flight/imu.c
View File

@ -214,10 +214,14 @@ static const float fc_acc = 0.5f / (M_PI * F_CUT_ACCZ);
void acc_calc(uint32_t deltaT)
{
static int32_t accZoffset = 0;
static float accz_smooth;
static float accz_smooth = 0;
float dT;
fp_angles_t rpy;
t_fp_vector accel_ned;
// deltaT is measured in us ticks
dT = (float)deltaT * 1e-6f;
// the accel values have to be rotated into the earth frame
rpy.angles.roll = -(float)anglerad[AI_ROLL];
rpy.angles.pitch = -(float)anglerad[AI_PITCH];
@ -238,7 +242,7 @@ void acc_calc(uint32_t deltaT)
} else
accel_ned.V.Z -= acc_1G;
accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy);