From 27b444f4e8376774887b433ce3e0ae62ceafba5c Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Sun, 26 Aug 2018 22:01:06 +1000
Subject: [PATCH] Plane: move handling of gimbal messages up

Plane: move handling of mav_cmd_do_mount_control up

Plane: move handling of deprecated mount messages up

Plane: move handling of command_do_set_roi up

Plane: mount no longer takes ahrs in constructor
---
 ArduPlane/GCS_Mavlink.cpp | 67 ---------------------------------------
 ArduPlane/Plane.h         |  2 +-
 2 files changed, 1 insertion(+), 68 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index d717a01950..b14b18c700 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -470,13 +470,6 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
         plane.send_wind(chan);
         break;
 
-    case MSG_MOUNT_STATUS:
-#if MOUNT == ENABLED
-        CHECK_PAYLOAD_SIZE(MOUNT_STATUS);
-        plane.camera_mount.status_msg(chan);
-#endif // MOUNT == ENABLED
-        break;
-
     case MSG_OPTICAL_FLOW:
 #if OPTFLOW == ENABLED
         if (plane.optflow.enabled()) {        
@@ -486,13 +479,6 @@ bool GCS_MAVLINK_Plane::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_PID_TUNING:
         if (plane.control_mode != MANUAL) {
             CHECK_PAYLOAD_SIZE(PID_TUNING);
@@ -926,35 +912,6 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
         return MAV_RESULT_FAILED;
     }
 
-#if MOUNT == ENABLED
-        // Sets the region of interest (ROI) for the camera
-    case MAV_CMD_DO_SET_ROI:
-        // sanity check location
-        if (!check_latlng(packet.param5, packet.param6)) {
-            return MAV_RESULT_FAILED;
-        }
-        Location roi_loc;
-        roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
-        roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
-        roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
-        if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) {
-            // switch off the camera tracking if enabled
-            if (plane.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
-                plane.camera_mount.set_mode_to_default();
-            }
-        } else {
-            // send the command to the camera mount
-            plane.camera_mount.set_roi_target(roi_loc);
-        }
-        return MAV_RESULT_ACCEPTED;
-#endif
-
-#if MOUNT == ENABLED
-    case MAV_CMD_DO_MOUNT_CONTROL:
-        plane.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
-        return MAV_RESULT_ACCEPTED;
-#endif
-
     case MAV_CMD_MISSION_START:
         plane.set_mode(AUTO, MODE_REASON_GCS_COMMAND);
         return MAV_RESULT_ACCEPTED;
@@ -1164,14 +1121,6 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
     }
 #endif // GEOFENCE_ENABLED
 
-    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
@@ -1303,22 +1252,6 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
         break;
     }
 
-#if MOUNT == ENABLED
-    //deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
-    case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
-    {
-        plane.camera_mount.configure_msg(msg);
-        break;
-    }
-
-    //deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
-    case MAVLINK_MSG_ID_MOUNT_CONTROL:
-    {
-        plane.camera_mount.control_msg(msg);
-        break;
-    }
-#endif // MOUNT == ENABLED
-
     case MAVLINK_MSG_ID_RADIO:
     case MAVLINK_MSG_ID_RADIO_STATUS:
     {
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 1701ef13ea..886144ccf3 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -745,7 +745,7 @@ private:
     // Camera/Antenna mount tracking and stabilisation stuff
 #if MOUNT == ENABLED
     // current_loc uses the baro/gps soloution for altitude rather than gps only.
-    AP_Mount camera_mount{ahrs, current_loc};
+    AP_Mount camera_mount{current_loc};
 #endif
 
     // Arming/Disarming mangement class