AP_Gimbal: update getGimbalRateDemVecTilt to use quaternions

libraries/AP_Gimbal/AP_Gimbal.cpp

@@ -113,13 +113,30 @@ Vector3f AP_Gimbal::getGimbalRateDemVecTilt(Quaternion quatEst)
         Vector3f eulerEst;
         quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
 
-        //TODO receive target from AP_Mount
-        Vector3f vectorError;
-        vectorError.x = eulerEst.x;
-        vectorError.y = eulerEst.y - _angle_ef_target_rad.y;
-        vectorError.z = 0;
-
-        Vector3f gimbalRateDemVecTilt = - vectorError * K_gimbalRate;
+        // 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);
+
+        //divide the demanded quaternion by the estimated to get the error
+        Quaternion quatErr = quatDem / quatEst;
+
+        // Convert to a delta rotation using a small angle appoximation
+        quatErr.normalize();
+        Vector3f deltaAngErr;
+        float scaler;
+        if (quatErr[0] >= 0.0f) {
+            scaler = 2.0f;
+        } else {
+            scaler = -2.0f;
+        }
+        deltaAngErr.x = scaler * quatErr[1];
+        deltaAngErr.y = scaler * quatErr[2];
+        deltaAngErr.z = scaler * quatErr[3];
+
+        // multiply the angle error vector by a gain to calculate a demanded gimbal rate required to control tilt
+        Vector3f gimbalRateDemVecTilt = deltaAngErr * K_gimbalRate;
         return gimbalRateDemVecTilt;
 }