From 97687f04afd9ef2c11cc59fb33080cb8414ee5a5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 26 May 2015 08:30:30 +1000
Subject: [PATCH] Plane: enable messages for MAVLink gimbal support

---
 ArduPlane/GCS_Mavlink.cpp    | 17 ++++++++++++++++-
 ArduPlane/commands_logic.cpp |  8 +++++++-
 2 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index f64f115cb3..3e45450eac 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -703,11 +703,17 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
 #endif
         break;
 
+    case MSG_GIMBAL_REPORT:
+#if MOUNT == ENABLED
+        CHECK_PAYLOAD_SIZE(GIMBAL_REPORT);
+        plane.camera_mount.send_gimbal_report(chan);
+#endif
+        break;
+
     case MSG_RETRY_DEFERRED:
         break; // just here to prevent a warning
 
     case MSG_LIMITS_STATUS:
-    case MSG_GIMBAL_REPORT:
         // unused
         break;
     }
@@ -940,6 +946,7 @@ GCS_MAVLINK::data_stream_send(void)
         send_message(MSG_MOUNT_STATUS);
         send_message(MSG_OPTICAL_FLOW);
         send_message(MSG_EKF_STATUS_REPORT);
+        send_message(MSG_GIMBAL_REPORT);
     }
 }
 
@@ -1454,6 +1461,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         break;
     }
 
+    case MAVLINK_MSG_ID_GIMBAL_REPORT:
+    {
+#if MOUNT == ENABLED
+        handle_gimbal_report(plane.camera_mount, msg);
+#endif
+        break;
+    }
+
     case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
     {
         // allow override of RC channel values for HIL
diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
index 438e57700d..c3f6a123cd 100644
--- a/ArduPlane/commands_logic.cpp
+++ b/ArduPlane/commands_logic.cpp
@@ -175,6 +175,13 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
             camera_mount.set_roi_target(cmd.content.location);
         }
         break;
+
+    case MAV_CMD_DO_MOUNT_CONTROL:          // 205
+        // point the camera to a specified angle
+        camera_mount.set_angle_targets(cmd.content.mount_control.roll, 
+                                       cmd.content.mount_control.pitch, 
+                                       cmd.content.mount_control.yaw);
+        break;
 #endif
     }
 
@@ -247,7 +254,6 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
     case MAV_CMD_NAV_ROI:
     case MAV_CMD_DO_MOUNT_CONFIGURE:
-    case MAV_CMD_DO_MOUNT_CONTROL:
     case MAV_CMD_DO_INVERTED_FLIGHT:
     case MAV_CMD_DO_LAND_START:
     case MAV_CMD_DO_FENCE_ENABLE: