rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-21 13:28:58 +01:00
Code Issues Releases Activity

Run rotated gravity vector through smoothing filter to match phase with filtered accelerometer data.

Browse Source
This commit is contained in:
Erik Gustavsson 2012-11-01 12:14:33 +01:00
parent c207afbfef
commit b7bdf9861d
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -444,7 +444,7 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
float * gyros = &gyrosData->x;
float * accels = &accelsData->x;
float grot[3];
static float grot[3];
float accel_err[3];
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
@ -455,9 +455,9 @@ static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
accels_filtered[2] = accels_filtered[2] * accel_alpha + accels[2] * (1 - accel_alpha);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
grot[0] = grot[0] * accel_alpha - (2 * (q[1] * q[3] - q[0] * q[2])) * (1 - accel_alpha);
grot[1] = grot[1] * accel_alpha - (2 * (q[2] * q[3] + q[0] * q[1])) * (1 - accel_alpha);
grot[2] = grot[2] * accel_alpha - (q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]) * (1 - accel_alpha);
CrossProduct((const float *) accels_filtered, (const float *) grot, accel_err);
// Account for accel magnitude