From 9c2f1ce869240dc82d775a586256ee252be62a5f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 27 Feb 2015 16:11:02 +1100 Subject: [PATCH] AP_Mount: Fix bug in mavlink gimbal attitude control --- libraries/AP_Mount/AP_Mount_MAVLink.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp index 8e10371e1aa..160bdb7f5ba 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_MAVLink.cpp @@ -194,17 +194,17 @@ Vector3f AP_Mount_MAVLink::gimbal_update_control1(const Vector3f &ef_target_eule //divide the demanded quaternion by the estimated to get the error Quaternion quatErr = quatDem / quatEst; - // convert the quaternion to an angle error vector + // convert the quaternion to an angle error vector using a first order approximation Vector3f deltaAngErr; - float scaler = 1.0f-quatErr[0]*quatErr[0]; - if (scaler > 1e-12) { - scaler = 1.0f/sqrtf(scaler); - deltaAngErr.x = quatErr[1] * scaler; - deltaAngErr.y = quatErr[2] * scaler; - deltaAngErr.z = quatErr[3] * scaler; + float scaler; + if (quatErr[0] >= 0.0f) { + scaler = 2.0f; } else { - deltaAngErr.zero(); + scaler = -2.0f; } + deltaAngErr.x = quatErr[1] * scaler; + deltaAngErr.y = quatErr[2] * scaler; + deltaAngErr.z = quatErr[3] * scaler; // multiply the angle error vector by a gain to calculate a demanded gimbal rate Vector3f rateDemand = deltaAngErr * 1.0f;