Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: added euler angle functions

Browse Source
This commit is contained in:
Andrew Tridgell 2013-12-29 23:20:53 +11:00
parent 977ad4bbf6
commit ffce1f64cc
2 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1868,13 +1868,19 @@ void NavEKF::eul2quat(float quat[4], float eul[3])
quat[3] = u1*u2*u6-u4*u5*u3;
}
void NavEKF::quat2eul(float y[3], float u[4])
void NavEKF::quat2eul(Vector3f &y, float u[4])
{
y[0] = atan2f((2*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
y[1] = -asinf(2*(u[1]*u[3]-u[0]*u[2]));
y[2] = atan2f((2*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
}
void NavEKF::getEulerAngles(Vector3f &euler)
{
float q[4] = { states[0], states[1], states[2], states[3] };
quat2eul(euler, q);
}
void NavEKF::calcposNE(float lat, float lon)
{
posNE[0] = RADIUS_OF_EARTH * (lat - latRef);

2
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -101,7 +101,7 @@ private:
void eul2quat(float quat[4], float eul[3]);
void quat2eul(float eul[3],float quat[4]);
void quat2eul(Vector3f &eul, float quat[4]);
void calcvelNED(float velNED[3], float gpsCourse, float gpsGndSpd, float gpsVelD);