Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: add getQuaternionBodyToNED

Browse Source
This commit is contained in:
Mark Whitehorn 2019-03-20 11:46:23 -06:00 committed by Randy Mackay
parent 6b70181837
commit 3c432cf25c
2 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1067,6 +1067,17 @@ void NavEKF3::getRotationBodyToNED(Matrix3f &mat) const
}
// return the quaternions defining the rotation from NED to XYZ (body) axes
void NavEKF3::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 NavEKF3::getQuaternion(int8_t instance, Quaternion &quat) const
{
if (instance < 0 || instance >= num_cores) instance = primary;

3
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

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