AP_Mount: added an alternative tilt-only gimbal control method

this adds some nice control characteristics based on work by Paul and
Arthur, but is tilt only

libraries/AP_Mount/AP_Mount_MAVLink.cpp

@@ -5,6 +5,7 @@
 #include <GCS_MAVLink.h>
 
 #define MOUNT_DEBUG 0
+#define TILT_CONTROL_ONLY 0
 
 #if MOUNT_DEBUG
 #include <stdio.h>
@@ -13,7 +14,12 @@
 AP_Mount_MAVLink::AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) :
     AP_Mount_Backend(frontend, state, instance),
     _initialised(false),
-    _ekf(frontend._ahrs)
+    _ekf(frontend._ahrs),
+    K_gimbalRate(0.1f),
+    angRateLimit(0.5f),
+    yawRateFiltPole(10.0f),
+    yawErrorLimit(0.1f),
+    vehicleYawRateFilt(0)
 {}
 
 // init - performs any required initialisation for this instance
@@ -108,45 +114,25 @@ void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_mess
     Vector3f joint_angles(_gimbal_report.joint_roll,
                           _gimbal_report.joint_pitch,
                           _gimbal_report.joint_yaw);
+
     _ekf.RunEKF(_gimbal_report.delta_time, delta_angles, delta_velocity, joint_angles);
 
-    // get the gyro bias data
+    /*
-    Vector3f gyroBias;
+      we have two different gimbal control algorithms. One does tilt
-    _ekf.getGyroBias(gyroBias);
+      control only, but has better control characteristics. The other
+      does roll/tilt/yaw, but has worset control characteristics
+     */
+#if TILT_CONTROL_ONLY
+    Vector3f rateDemand = gimbal_update_control2(_angle_ef_target_rad, 
+                                                 _gimbal_report.delta_time, delta_angles, delta_velocity, joint_angles);
+#else
+    Vector3f rateDemand = gimbal_update_control1(_angle_ef_target_rad, 
+                                                 _gimbal_report.delta_time, delta_angles, delta_velocity, joint_angles);
+#endif
 
-    // get the gimbal estimated quaternion
+    // for now send a zero gyro bias update and incorporate into the
-    Quaternion quatEst;
+    // demanded rates
-    _ekf.getQuat(quatEst);
+    Vector3f gyroBias(0,0,0);
-
-    // set the demanded quaternion - tilt down with a roll and yaw of zero
-    Quaternion quatDem;
-    quatDem.from_euler(_angle_ef_target_rad.x,
-                       _angle_ef_target_rad.y,
-                       _angle_ef_target_rad.z);
-
-    //divide the demanded quaternion by the estimated to get the error
-    Quaternion quatErr = quatDem / quatEst;
-
-    // convert the quaternion to an angle error vector
-    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;
-    } else {
-        deltaAngErr.zero();
-    }
-
-    // multiply the angle error vector by a gain to calculate a demanded gimbal rate
-    Vector3f rateDemand = deltaAngErr * 1.0f;
-
-    // Constrain the demanded rate to a length of 0.5 rad /sec
-    float length = rateDemand.length();
-    if (length > 0.5f) {
-        rateDemand = rateDemand * (0.5f / length);
-    }
 
     // send the gimbal control message
     mavlink_msg_gimbal_control_send(chan, 
@@ -184,4 +170,184 @@ void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan)
 #endif
 }
 
+/*
+  calculate demanded rates for the gimbal
+ */
+Vector3f AP_Mount_MAVLink::gimbal_update_control1(const Vector3f &ef_target_euler_rad, 
+                                                  float delta_time, 
+                                                  const Vector3f &delta_angles,
+                                                  const Vector3f &delta_velocity,
+                                                  const Vector3f &joint_angles)
+{
+    // get the gyro bias data
+    Vector3f gyroBias;
+    _ekf.getGyroBias(gyroBias);
+    
+    // get the gimbal estimated quaternion
+    Quaternion quatEst;
+    _ekf.getQuat(quatEst);
+
+    // set the demanded quaternion - tilt down with a roll and yaw of zero
+    Quaternion quatDem;
+    quatDem.from_euler(ef_target_euler_rad.x, ef_target_euler_rad.y, ef_target_euler_rad.z);
+
+    //divide the demanded quaternion by the estimated to get the error
+    Quaternion quatErr = quatDem / quatEst;
+
+    // convert the quaternion to an angle error vector
+    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;
+    } else {
+        deltaAngErr.zero();
+    }
+ 
+    // multiply the angle error vector by a gain to calculate a demanded gimbal rate
+    Vector3f rateDemand = deltaAngErr * 1.0f;
+
+    // Constrain the demanded rate to a length of 0.5 rad /sec
+    float length = rateDemand.length();
+    if (length > 0.5f) {
+        rateDemand = rateDemand * (0.5f / length);
+    }
+
+    return rateDemand;
+}
+
+
+// convert the quaternion to rotation vector
+Vector3f AP_Mount_MAVLink::quaternion_to_vector(const Quaternion &quat)
+{
+    Vector3f vector;
+    float scaler = 1.0f-quat[0]*quat[0];
+    if (scaler > 1e-12f) {
+        scaler = 1.0f/sqrtf(scaler);
+        if (quat[0] < 0.0f) {
+            scaler *= -1.0f;
+        }
+        vector.x = quat[1] * scaler;
+        vector.y = quat[2] * scaler;
+        vector.z = quat[3] * scaler;
+    } else {
+        vector.zero();
+    }
+    return vector;
+}
+
+// Define rotation matrix using a 312 rotation sequence vector
+Matrix3f AP_Mount_MAVLink::vector312_to_rotation_matrix(const Vector3f &vector)
+{
+    Matrix3f matrix;
+    float cosPhi = cosf(vector.x);
+    float cosTheta = cosf(vector.y);
+    float sinPhi = sinf(vector.x);
+    float sinTheta = sinf(vector.y);
+    float sinPsi = sinf(vector.z);
+    float cosPsi = cosf(vector.z);
+    matrix[0][0] = cosTheta*cosPsi-sinPsi*sinPhi*sinTheta;
+    matrix[1][0] = -sinPsi*cosPhi;
+    matrix[2][0] = cosPsi*sinTheta+cosTheta*sinPsi*sinPhi;
+    matrix[0][1] = cosTheta*sinPsi+cosPsi*sinPhi*sinTheta;
+    matrix[1][1] = cosPsi*cosPhi;
+    matrix[2][1] = sinPsi*sinTheta-cosTheta*cosPsi*sinPhi;
+    matrix[0][2] = -sinTheta*cosPhi;
+    matrix[1][2] = sinPhi;
+    matrix[2][2] = cosTheta*cosPhi;
+    return matrix;
+}
+
+
+/*
+  calculate the demanded rates for the mount, running the controller
+ */
+Vector3f AP_Mount_MAVLink::gimbal_update_control2(const Vector3f &ef_target_euler_rad, 
+                                                  float delta_time, 
+                                                  const Vector3f &delta_angles,
+                                                  const Vector3f &delta_velocity,
+                                                  const Vector3f &joint_angles)
+{
+    // get the gimbal quaternion estimate
+    Quaternion quatEst;
+    _ekf.getQuat(quatEst);
+ 
+    // Add the control rate vectors
+    Vector3f gimbalRateDemVec = 
+        getGimbalRateDemVecYaw(ef_target_euler_rad, delta_time, quatEst, joint_angles) + 
+        getGimbalRateDemVecTilt(ef_target_euler_rad, quatEst) + 
+        getGimbalRateDemVecForward(ef_target_euler_rad, delta_time, quatEst);
+
+    Vector3f gyroBias;
+    _ekf.getGyroBias(gyroBias);
+
+    gimbalRateDemVec += gyroBias;
+    return gimbalRateDemVec;
+}
+
+Vector3f AP_Mount_MAVLink::getGimbalRateDemVecYaw(const Vector3f &ef_target_euler_rad, float delta_time, const Quaternion &quatEst, const Vector3f &joint_angles)
+{
+    // Define rotation from vehicle to gimbal using a 312 rotation sequence
+    Matrix3f Tvg = vector312_to_rotation_matrix(joint_angles);
+
+    // multiply the yaw joint angle by a gain to calculate a
+    // demanded vehicle frame relative rate vector required to
+    // keep the yaw joint centred
+    Vector3f gimbalRateDemVecYaw(0, 0, - K_gimbalRate * joint_angles.z);
+
+    // Get filtered vehicle turn rate in earth frame
+    vehicleYawRateFilt = (1.0f - yawRateFiltPole * delta_time) * vehicleYawRateFilt + yawRateFiltPole * delta_time * _frontend._ahrs.get_yaw_rate_earth();
+    Vector3f vehicle_rate_ef(0,0,vehicleYawRateFilt);
+    
+    // calculate the maximum steady state rate error corresponding to the maximum permitted yaw angle error
+    float maxRate = K_gimbalRate * yawErrorLimit;
+    float vehicle_rate_mag_ef = vehicle_rate_ef.length();
+    float excess_rate_correction = fabs(vehicle_rate_mag_ef) - maxRate; 
+    if (vehicle_rate_mag_ef > maxRate) {
+        if (vehicle_rate_ef.z>0.0f) {
+            gimbalRateDemVecYaw += _frontend._ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);    
+        } else {
+            gimbalRateDemVecYaw -= _frontend._ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);    
+        }            
+    }        
+    
+    // rotate into gimbal frame to calculate the gimbal rate vector required to keep the yaw gimbal centred
+    gimbalRateDemVecYaw = Tvg * gimbalRateDemVecYaw;
+    return gimbalRateDemVecYaw;
+}
+
+Vector3f AP_Mount_MAVLink::getGimbalRateDemVecTilt(const Vector3f &ef_target_euler_rad, const Quaternion &quatEst)
+{
+    // Calculate the gimbal 321 Euler angle estimates relative to earth frame
+    Vector3f eulerEst;
+    quatEst.to_euler(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;
+    //TODO receive target from AP_Mount
+    quatDem.from_euler(0, ef_target_euler_rad.y, eulerEst.z);
+
+    //divide the demanded quaternion by the estimated to get the error
+    Quaternion quatErr = quatDem / quatEst;
+
+    // multiply the angle error vector by a gain to calculate a demanded gimbal rate required to control tilt
+    Vector3f gimbalRateDemVecTilt = quaternion_to_vector(quatErr) * K_gimbalRate;
+    return gimbalRateDemVecTilt;
+}
+
+Vector3f AP_Mount_MAVLink::getGimbalRateDemVecForward(const Vector3f &ef_target_euler_rad, float delta_time, const Quaternion &quatEst)
+{
+    // calculate the delta rotation from the last to the current demand where the demand does not incorporate the copters yaw rotation
+    Quaternion quatDemForward;
+    quatDemForward.from_euler(0, ef_target_euler_rad.y, 0);
+    Quaternion deltaQuat = quatDemForward / lastQuatDem;
+    lastQuatDem = quatDemForward;
+    
+    // convert to a rotation vector and divide by delta time to obtain a forward path rate demand
+    Vector3f gimbalRateDemVecForward = quaternion_to_vector(deltaQuat) * (1.0f / delta_time);
+    return gimbalRateDemVecForward;
+}
+
 #endif // AP_AHRS_NAVEKF_AVAILABLE

libraries/AP_Mount/AP_Mount_MAVLink.h

@@ -56,7 +56,41 @@ private:
 
     // keep last gimbal report
     mavlink_gimbal_report_t _gimbal_report;
+
+    float vehicleYawRateFilt;
+    const float K_gimbalRate;
+    const float angRateLimit;
+
+    // circular frequency (rad/sec) constant of filter applied to forward path vehicle yaw rate
+    // this frequency must not be larger than the update rate (Hz).
+    // reducing it makes the gimbal yaw less responsive to vehicle yaw
+    // increasing it makes the gimbal yawe more responsive to vehicle yaw
+    const float yawRateFiltPole;
+
+    // amount of yaw angle that we permit the gimbal to lag the vehicle when operating in slave mode
+    // reducing this makes the gimbal respond more to vehicle yaw disturbances
+    const float yawErrorLimit;
+
+    // quaternion demanded at the previous time step
+    Quaternion lastQuatDem;
+
+    Vector3f quaternion_to_vector(const Quaternion &quat);
+    Matrix3f vector312_to_rotation_matrix(const Vector3f &vector);
+    Vector3f gimbal_update_control1(const Vector3f &ef_target_euler_rad,
+                                    float delta_time, 
+                                    const Vector3f &delta_angles,
+                                    const Vector3f &delta_velocity,
+                                    const Vector3f &joint_angles);
+    Vector3f gimbal_update_control2(const Vector3f &ef_target_euler_rad,
+                                    float delta_time, 
+                                    const Vector3f &delta_angles,
+                                    const Vector3f &delta_velocity,
+                                    const Vector3f &joint_angles);
+    Vector3f getGimbalRateDemVecYaw(const Vector3f &ef_target_euler_rad, float delta_time, const Quaternion &quatEst, const Vector3f &joint_angles);
+    Vector3f getGimbalRateDemVecTilt(const Vector3f &ef_target_euler_rad, const Quaternion &quatEst);
+    Vector3f getGimbalRateDemVecForward(const Vector3f &ef_target_euler_rad, float delta_time, const Quaternion &quatEst);
 };
+
 #endif // AP_AHRS_NAVEKF_AVAILABLE
 
 #endif // __AP_MOUNT_MAVLINK_H__