From 03974c93bb7c3b47718d5fe412e5207903e810c8 Mon Sep 17 00:00:00 2001 From: Arthur Benemann Date: Thu, 2 Apr 2015 12:27:43 -0700 Subject: [PATCH] AP_Gimbal: use new quaternion math to handle gimbal lock issue --- libraries/AP_Gimbal/AP_Gimbal.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Gimbal/AP_Gimbal.cpp b/libraries/AP_Gimbal/AP_Gimbal.cpp index 4aa009c4b3..68f845ec34 100644 --- a/libraries/AP_Gimbal/AP_Gimbal.cpp +++ b/libraries/AP_Gimbal/AP_Gimbal.cpp @@ -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;