rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

I knew I had forgotten something (cos -> cosf ...)

Browse Source
This commit is contained in:
Corvus Corax 2013-05-01 20:40:50 +02:00
parent ff5fd9c7c9
commit 467b22f294
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
flight/modules/Attitude/revolution/attitude.c
View File

@ -331,14 +331,14 @@ static int32_t updateAttitudeComplementary(bool first_run)
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level // so pseudo "north" vector can be estimated even if the board is not level
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z); attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z);
float zn = cos(attitudeActual.Roll) * magData.z + sin(attitudeActual.Roll) * magData.y; float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y;
float yn = cos(attitudeActual.Roll) * magData.y - sin(attitudeActual.Roll) * magData.z; float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z;
// rotate accels z vector according to roll // rotate accels z vector according to roll
float azn = cos(attitudeActual.Roll) * accelsData.z + sin(attitudeActual.Roll) * accelsData.y; float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y;
attitudeActual.Pitch = atan2f(accelsData.x, -azn); attitudeActual.Pitch = atan2f(accelsData.x, -azn);
float xn = cos(attitudeActual.Pitch) * magData.x + sin(attitudeActual.Pitch) * zn; float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn;
attitudeActual.Yaw = atan2f(-yn, xn); attitudeActual.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack // TODO: This is still a hack
@ -808,14 +808,14 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level // so pseudo "north" vector can be estimated even if the board is not level
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z); attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z);
float zn = cos(attitudeActual.Roll) * magData.z + sin(attitudeActual.Roll) * magData.y; float zn = cosf(attitudeActual.Roll) * magData.z + sinf(attitudeActual.Roll) * magData.y;
float yn = cos(attitudeActual.Roll) * magData.y - sin(attitudeActual.Roll) * magData.z; float yn = cosf(attitudeActual.Roll) * magData.y - sinf(attitudeActual.Roll) * magData.z;
// rotate accels z vector according to roll // rotate accels z vector according to roll
float azn = cos(attitudeActual.Roll) * accelsData.z + sin(attitudeActual.Roll) * accelsData.y; float azn = cosf(attitudeActual.Roll) * accelsData.z + sinf(attitudeActual.Roll) * accelsData.y;
attitudeActual.Pitch = atan2f(accelsData.x, -azn); attitudeActual.Pitch = atan2f(accelsData.x, -azn);
float xn = cos(attitudeActual.Pitch) * magData.x + sin(attitudeActual.Pitch) * zn; float xn = cosf(attitudeActual.Pitch) * magData.x + sinf(attitudeActual.Pitch) * zn;
attitudeActual.Yaw = atan2f(-yn, xn); attitudeActual.Yaw = atan2f(-yn, xn);
// TODO: This is still a hack // TODO: This is still a hack