From df99941fc8d625a05159de44f31f55c9aba1497d Mon Sep 17 00:00:00 2001
From: Grant Morphett <grant@gmorph.com>
Date: Fri, 26 Aug 2016 11:28:49 +1000
Subject: [PATCH] Rover: Added support for COMMAND_INT

And also made DO_SET_ROI available as a COMMAND_INT as this gives us
cm accuracy rather then 1.5m when a float is used.
---
 APMrover2/GCS_Mavlink.cpp  | 48 ++++++++++++++++++++++++++++++++++++++
 APMrover2/capabilities.cpp |  1 +
 2 files changed, 49 insertions(+)

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index ba9dde1e77..48f308d6b7 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -822,6 +822,54 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             break;
         }
 
+    case MAVLINK_MSG_ID_COMMAND_INT: {
+        // decode packet
+        mavlink_command_int_t packet;
+        mavlink_msg_command_int_decode(msg, &packet);
+        uint8_t result = MAV_RESULT_UNSUPPORTED;
+
+        switch(packet.command) {
+#if MOUNT == ENABLED
+        case MAV_CMD_DO_SET_ROI: {
+            // param1 : /* Region of interest mode (not used)*/
+            // param2 : /* MISSION index/ target ID (not used)*/
+            // param3 : /* ROI index (not used)*/
+            // param4 : /* empty */
+            // x : lat
+            // y : lon
+            // z : alt
+            // sanity check location
+            if (!check_latlng(packet.x, packet.y)) {
+                break;
+            }
+            Location roi_loc;
+            roi_loc.lat = packet.x;
+            roi_loc.lng = packet.y;
+            roi_loc.alt = (int32_t)(packet.z * 100.0f);
+            if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) {
+                // switch off the camera tracking if enabled
+                if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
+                    rover.camera_mount.set_mode_to_default();
+                }
+            } else {
+                // send the command to the camera mount
+                rover.camera_mount.set_roi_target(roi_loc);
+            }
+            result = MAV_RESULT_ACCEPTED;
+            break;
+        }
+#endif
+
+        default:
+            result = MAV_RESULT_UNSUPPORTED;
+            break;
+        }
+
+        // send ACK or NAK
+        mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result);
+        break;
+    }
+
     case MAVLINK_MSG_ID_COMMAND_LONG:
         {
             // decode
diff --git a/APMrover2/capabilities.cpp b/APMrover2/capabilities.cpp
index d3d0d52e54..9b4917a6f5 100644
--- a/APMrover2/capabilities.cpp
+++ b/APMrover2/capabilities.cpp
@@ -7,6 +7,7 @@ void Rover::init_capabilities(void)
     hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
                                MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT |
                                MAV_PROTOCOL_CAPABILITY_MISSION_INT |
+                               MAV_PROTOCOL_CAPABILITY_COMMAND_INT |
                                MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED |
                                MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT);
 }