From cc25f2915a44f8f818c7a65def53ee7e55b60b13 Mon Sep 17 00:00:00 2001 From: Nick Exton Date: Fri, 8 Sep 2023 13:54:25 +1000 Subject: [PATCH] AP_Mount: In Siyi, fix calculation of attitude quaternion Order of rotation for Siyi gimbals is (yaw, roll, pitch), which is 312 order. Quaternion::from_euler() function assumes a 321 order of rotation (yaw, pitch, roll). --- libraries/AP_Mount/AP_Mount_Siyi.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 37caa578c9..cf5ee69fa8 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -181,7 +181,8 @@ bool AP_Mount_Siyi::healthy() const // get attitude as a quaternion. returns true on success bool AP_Mount_Siyi::get_attitude_quaternion(Quaternion& att_quat) { - att_quat.from_euler(_current_angle_rad.x, _current_angle_rad.y, _current_angle_rad.z); + // Order of rotation for Siyi gimbals is (yaw, roll, pitch), which is 312 order + att_quat.from_vector312(_current_angle_rad.x, _current_angle_rad.y, _current_angle_rad.z); return true; }