Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: implemented getRotationBodyToNED()

Browse Source
This commit is contained in:
Andrew Tridgell 2014-01-02 13:05:49 +11:00
parent 8c5cde4efd
commit 33673c954d
1 changed files with 7 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2110,4 +2110,11 @@ void NavEKF::calcEarthRateNED(Vector3f &omega, float latitude) const
omega.z = -earthRate*sinf(latitude); omega.z = -earthRate*sinf(latitude);
} }
void NavEKF::getRotationBodyToNED(Matrix3f &mat) const
{
Quaternion q(states[0], states[1], states[2], states[3]);
q.rotation_matrix(mat);
}
#endif // HAL_CPU_CLASS #endif // HAL_CPU_CLASS