mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-30 20:48:33 -04:00
AP_NavEKF3: add getQuaternionBodyToNED
This commit is contained in:
parent
6b70181837
commit
3c432cf25c
@ -1067,6 +1067,17 @@ void NavEKF3::getRotationBodyToNED(Matrix3f &mat) const
|
|||||||
}
|
}
|
||||||
|
|
||||||
// return the quaternions defining the rotation from NED to XYZ (body) axes
|
// 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
|
void NavEKF3::getQuaternion(int8_t instance, Quaternion &quat) const
|
||||||
{
|
{
|
||||||
if (instance < 0 || instance >= num_cores) instance = primary;
|
if (instance < 0 || instance >= num_cores) instance = primary;
|
||||||
|
@ -179,6 +179,9 @@ public:
|
|||||||
void getRotationBodyToNED(Matrix3f &mat) const;
|
void getRotationBodyToNED(Matrix3f &mat) const;
|
||||||
|
|
||||||
// return the quaternions defining the rotation from NED to XYZ (body) axes
|
// 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;
|
void getQuaternion(int8_t instance, Quaternion &quat) const;
|
||||||
|
|
||||||
// return the innovations for the specified instance
|
// return the innovations for the specified instance
|
||||||
|
Loading…
Reference in New Issue
Block a user