Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Gimbal: use new quaternion math to handle gimbal lock issue

Browse Source
This commit is contained in:
Arthur Benemann 2015-04-02 12:27:43 -07:00 committed by Randy Mackay
parent 255c5b3025
commit 03974c93bb
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Gimbal/AP_Gimbal.cpp
View File

@ -113,13 +113,13 @@ Vector3f AP_Gimbal::getGimbalRateDemVecTilt(Quaternion quatEst)
{
// Calculate the gimbal 321 Euler angle estimates relative to earth frame
Vector3f eulerEst;
quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
quatEst.to_vector312(eulerEst.x, eulerEst.y, eulerEst.z);
// Calculate a demanded quaternion using the demanded roll and pitch and estimated yaw (yaw is slaved to the vehicle)
Quaternion quatDem;
quatDem.from_euler(_angle_ef_target_rad.x,
_angle_ef_target_rad.y,
eulerEst.z);
quatDem.from_vector312( _angle_ef_target_rad.x,
_angle_ef_target_rad.y,
eulerEst.z);
//divide the demanded quaternion by the estimated to get the error
Quaternion quatErr = quatDem / quatEst;