Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: add getQuaternionBodyToNED

This commit is contained in:
Mark Whitehorn 2019-03-20 11:45:18 -06:00 committed by Randy Mackay
parent b32ccbfb33
commit 6b70181837
2 changed files with 15 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -1042,6 +1042,17 @@ void NavEKF2::getRotationBodyToNED(Matrix3f &mat) const
}
// return the quaternions defining the rotation from NED to XYZ (body) axes
void NavEKF2::getQuaternionBodyToNED(int8_t instance, Quaternion &quat) const
{
if (instance < 0 || instance >= num_cores) instance = primary;
if (core) {
Matrix3f mat;
core[instance].getRotationBodyToNED(mat);
quat.from_rotation_matrix(mat);
}
}
// return the quaternions defining the rotation from NED to XYZ (autopilot) axes
void NavEKF2::getQuaternion(int8_t instance, Quaternion &quat) const
{
if (instance < 0 || instance >= num_cores) instance = primary;

5
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -187,7 +187,10 @@ public:
// return the transformation matrix from XYZ (body) to NED axes
void getRotationBodyToNED(Matrix3f &mat) const;
// return the quaternions defining the rotation from NED to XYZ (body) axes
// return the transformation matrix from XYZ (body) to NED axes
void getQuaternionBodyToNED(int8_t instance, Quaternion &quat) const;
// return the quaternions defining the rotation from NED to autopilot axes
void getQuaternion(int8_t instance, Quaternion &quat) const;
// return the innovations for the specified instance