aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mount: In Siyi, fix calculation of attitude quaternion

Browse Source 
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).
This commit is contained in:
Nick Exton 2023-09-08 13:54:25 +10:00 committed by Peter Barker
parent f2f7f7de3b
commit cc25f2915a
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Mount/AP_Mount_Siyi.cpp
View File

@ -181,7 +181,8 @@ bool AP_Mount_Siyi::healthy() const
// get attitude as a quaternion. returns true on success // get attitude as a quaternion. returns true on success
bool AP_Mount_Siyi::get_attitude_quaternion(Quaternion& att_quat) 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; return true;
} }