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

AP_NavEKF3: use quat.to_euler(Vector3f&)

This commit is contained in:
Peter Barker 2023-04-15 23:50:36 +10:00 committed by Peter Barker
parent a3c5926040
commit ce8afaf57b
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -88,7 +88,7 @@ bool NavEKF3_core::getHeightControlLimit(float &height) const
// return the Euler roll, pitch and yaw angle in radians // return the Euler roll, pitch and yaw angle in radians
void NavEKF3_core::getEulerAngles(Vector3f &euler) const void NavEKF3_core::getEulerAngles(Vector3f &euler) const
{ {
outputDataNew.quat.to_euler(euler.x, euler.y, euler.z); outputDataNew.quat.to_euler(euler);
euler = euler - dal.get_trim(); euler = euler - dal.get_trim();
} }

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -2064,7 +2064,7 @@ void NavEKF3_core::setYawFromMag()
Matrix3F Tbn_zeroYaw; Matrix3F Tbn_zeroYaw;
if (order == rotationOrder::TAIT_BRYAN_321) { if (order == rotationOrder::TAIT_BRYAN_321) {
// rolled more than pitched so use 321 rotation order // rolled more than pitched so use 321 rotation order
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z); stateStruct.quat.to_euler(eulerAngles);
Tbn_zeroYaw.from_euler(eulerAngles.x, eulerAngles.y, 0.0f); Tbn_zeroYaw.from_euler(eulerAngles.x, eulerAngles.y, 0.0f);
} else if (order == rotationOrder::TAIT_BRYAN_312) { } else if (order == rotationOrder::TAIT_BRYAN_312) {
// pitched more than rolled so use 312 rotation order // pitched more than rolled so use 312 rotation order