AP_Mount: report MAVLink gimbal angle to the GCS

libraries/AP_Gimbal/AP_Gimbal.cpp

@@ -178,6 +178,12 @@ void AP_Gimbal::update_target(Vector3f newTarget)
 
 }
 
+Vector3f AP_Gimbal::getGimbalEstimateEF()
+{
+    Quaternion quatEst;_ekf.getQuat(quatEst);Vector3f eulerEst;quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
+    return eulerEst;
+}
+
 uint8_t AP_Gimbal::isCopterFliped(){
     return fabs(_ahrs.roll)>1.0f || fabs(_ahrs.pitch)>1.0f;
 }

libraries/AP_Gimbal/AP_Gimbal.h

@@ -43,6 +43,8 @@ public:
     void    update_target(Vector3f newTarget);
     void    receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
 
+    Vector3f getGimbalEstimateEF();
+
     struct Measurament {
         float delta_time;
         Vector3f delta_angles;

libraries/AP_Mount/AP_Mount_MAVLink.cpp

@@ -87,7 +87,8 @@ void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode)
 // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
 void AP_Mount_MAVLink::status_msg(mavlink_channel_t chan)
 {
-    // do nothing - we rely on the mount sending the messages directly
+    Vector3f est = _gimbal.getGimbalEstimateEF();
+    mavlink_msg_mount_status_send(chan, 0, 0, degrees(est.y)*100, degrees(est.x)*100, degrees(est.z)*100);
 }
 
 /*