rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Take magnitude of filtered gravity vector into account.

Browse Source
This commit is contained in:
Erik Gustavsson 2012-11-02 09:22:18 +01:00
parent f63db712d0
commit 677f921b15
1 changed files with 8 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
flight/Modules/Attitude/attitude.c
View File

@ -469,9 +469,14 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
if(accel_mag < 1.0e-3f)
return;
accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag;
// Account for filtered gravity vector magnitude
float grot_mag = sqrtf(grot[0]*grot[0] + grot[1]*grot[1] + grot[2]*grot[2]);
if(grot_mag < 1.0e-3f)
return;
accel_err[0] /= (accel_mag*grot_mag);
accel_err[1] /= (accel_mag*grot_mag);
accel_err[2] /= (accel_mag*grot_mag);
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_correct_int[0] += accel_err[0] * accelKi;