GCS_MAVLink: move handling of gimbal messages up

GCS_MAVLink: move handling of mav_cmd_do_mount_control up

GCS_MAVLink: move handling of deprecated mount messages up

GCS_MAVLink: move handling of command_do_set_roi up

GCS_MAVLink: move handling of command_do_set_roi up

GCS_MAVLink: handle MAV_CMD_DO_MOUNT_CONFIGURE

GCS_MAVLink: call renamed AP_Mount send_mount_status msg

GCS_MAVLink: add support for MAV_CMD_DO_SET_ROI_LOCATION

libraries/GCS_MAVLink/GCS.h

@@ -190,7 +190,9 @@ public:
     void send_local_position() const;
     void send_vfr_hud();
     void send_vibration() const;
+    void send_mount_status() const;
     void send_named_float(const char *name, float value) const;
+    void send_gimbal_report() const;
     void send_home() const;
     void send_ekf_origin() const;
     virtual void send_position_target_global_int() { };
@@ -302,7 +304,8 @@ protected:
     void handle_common_rally_message(mavlink_message_t *msg);
     void handle_rally_fetch_point(mavlink_message_t *msg);
     void handle_rally_point(mavlink_message_t *msg);
-    void handle_gimbal_report(AP_Mount &mount, mavlink_message_t *msg) const;
+    virtual void handle_mount_message(const mavlink_message_t *msg);
+    void handle_param_value(mavlink_message_t *msg);
     void handle_radio_status(mavlink_message_t *msg, DataFlash_Class &dataflash, bool log_radio);
     void handle_serial_control(const mavlink_message_t *msg);
     void handle_vision_position_delta(mavlink_message_t *msg);
@@ -354,10 +357,14 @@ protected:
 
     void handle_command_long(mavlink_message_t* msg);
     MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_long_t &packet);
+    virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_mag_cal(const mavlink_command_long_t &packet);
     virtual MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_do_send_banner(const mavlink_command_long_t &packet);
+    MAV_RESULT handle_command_do_set_roi(const mavlink_command_int_t &packet);
+    MAV_RESULT handle_command_do_set_roi(const mavlink_command_long_t &packet);
+    virtual MAV_RESULT handle_command_do_set_roi(const Location &roi_loc);
     MAV_RESULT handle_command_do_gripper(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_do_set_mode(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_get_home_position(const mavlink_command_long_t &packet);

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -618,13 +618,28 @@ void GCS_MAVLINK::handle_mission_write_partial_list(AP_Mission &mission, mavlink
 
 
 /*
-  handle a GIMBAL_REPORT mavlink packet
+  pass mavlink messages to the AP_Mount singleton
  */
-void GCS_MAVLINK::handle_gimbal_report(AP_Mount &mount, mavlink_message_t *msg) const
+void GCS_MAVLINK::handle_mount_message(const mavlink_message_t *msg)
 {
-    mount.handle_gimbal_report(chan, msg);
+    AP_Mount *mount = AP::mount();
+    if (mount == nullptr) {
+        return;
+    }
+    mount->handle_message(chan, msg);
 }
 
+/*
+  pass parameter value messages through to mount library
+ */
+void GCS_MAVLINK::handle_param_value(mavlink_message_t *msg)
+{
+    AP_Mount *mount = AP::mount();
+    if (mount == nullptr) {
+        return;
+    }
+    mount->handle_param_value(msg);
+}
 
 void GCS_MAVLINK::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list)
 {
@@ -2060,6 +2075,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_long_t &pack
     return result;
 }
 
+
 // sets ekf_origin if it has not been set.
 //  should only be used when there is no GPS to provide an absolute position
 void GCS_MAVLINK::set_ekf_origin(const Location& loc)
@@ -2353,6 +2369,14 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
         handle_command_int(msg);
         break;
 
+    case MAVLINK_MSG_ID_GIMBAL_REPORT:
+        handle_mount_message(msg);
+        break;
+
+    case MAVLINK_MSG_ID_PARAM_VALUE:
+        handle_param_value(msg);
+        break;
+
     case MAVLINK_MSG_ID_SERIAL_CONTROL:
         handle_serial_control(msg);
         break;
@@ -2373,6 +2397,11 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
         AP_Notify::handle_led_control(msg);
         break;
 
+    case MAVLINK_MSG_ID_MOUNT_CONFIGURE: // deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE
+    case MAVLINK_MSG_ID_MOUNT_CONTROL: // deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
+        handle_mount_message(msg);
+        break;
+
     case MAVLINK_MSG_ID_PLAY_TUNE:
         // send message to Notify
         AP_Notify::handle_play_tune(msg);
@@ -2715,6 +2744,15 @@ MAV_RESULT GCS_MAVLINK::handle_command_accelcal_vehicle_pos(const mavlink_comman
     return MAV_RESULT_ACCEPTED;
 }
 
+MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_long_t &packet)
+{
+    AP_Mount *mount = AP::mount();
+    if (mount == nullptr) {
+        return MAV_RESULT_UNSUPPORTED;
+    }
+    return mount->handle_command_long(packet);
+}
+
 MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t &packet)
 {
     MAV_RESULT result = MAV_RESULT_FAILED;
@@ -2758,11 +2796,21 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
         result = handle_command_do_gripper(packet);
         break;
 
+    case MAV_CMD_DO_MOUNT_CONFIGURE:
+    case MAV_CMD_DO_MOUNT_CONTROL:
+        result = handle_command_mount(packet);
+        break;
+
     case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
         result = handle_command_request_autopilot_capabilities(packet);
         break;
     }
 
+    case MAV_CMD_DO_SET_ROI_LOCATION:
+    case MAV_CMD_DO_SET_ROI:
+        result = handle_command_do_set_roi(packet);
+        break;
+
     case MAV_CMD_PREFLIGHT_CALIBRATION:
         result = handle_command_preflight_calibration(packet);
         break;
@@ -2819,8 +2867,77 @@ void GCS_MAVLINK::handle_command_long(mavlink_message_t *msg)
     mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result);
 }
 
+MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const Location &roi_loc)
+{
+    AP_Mount *mount = AP::mount();
+    if (mount == nullptr) {
+        return MAV_RESULT_UNSUPPORTED;
+    }
+
+    // sanity check location
+    if (!check_latlng(roi_loc)) {
+        return MAV_RESULT_FAILED;
+    }
+
+    if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) {
+        // switch off the camera tracking if enabled
+        if (mount->get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
+            mount->set_mode_to_default();
+        }
+    } else {
+        // send the command to the camera mount
+        mount->set_roi_target(roi_loc);
+    }
+    return MAV_RESULT_ACCEPTED;
+}
+
+MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const mavlink_command_int_t &packet)
+{
+    // be aware that this method is called for both MAV_CMD_DO_SET_ROI
+    // and MAV_CMD_DO_SET_ROI_LOCATION.  If you intend to support any
+    // of the extra fields in the former then you will need to split
+    // off support for MAV_CMD_DO_SET_ROI_LOCATION (which doesn't
+    // support the extra fields).
+
+    // 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
+    Location roi_loc;
+    roi_loc.lat = packet.x;
+    roi_loc.lng = packet.y;
+    roi_loc.alt = (int32_t)(packet.z * 100.0f);
+    return handle_command_do_set_roi(roi_loc);
+}
+
+MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const mavlink_command_long_t &packet)
+{
+    // be aware that this method is called for both MAV_CMD_DO_SET_ROI
+    // and MAV_CMD_DO_SET_ROI_LOCATION.  If you intend to support any
+    // of the extra fields in the former then you will need to split
+    // off support for MAV_CMD_DO_SET_ROI_LOCATION (which doesn't
+    // support the extra fields).
+
+    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);
+    return handle_command_do_set_roi(roi_loc);
+}
+
 MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &packet)
 {
+    switch (packet.command) {
+    case MAV_CMD_DO_SET_ROI:
+    case MAV_CMD_DO_SET_ROI_LOCATION:
+        return handle_command_do_set_roi(packet);
+    default:
+        break;
+    }
+
     return MAV_RESULT_UNSUPPORTED;
 }
 
@@ -2941,6 +3058,24 @@ void GCS_MAVLINK::send_global_position_int()
         ahrs.yaw_sensor);                // compass heading in 1/100 degree
 }
 
+void GCS_MAVLINK::send_gimbal_report() const
+{
+    AP_Mount *mount = AP::mount();
+    if (mount == nullptr) {
+        return;
+    }
+    mount->send_gimbal_report(chan);
+}
+
+void GCS_MAVLINK::send_mount_status() const
+{
+    AP_Mount *mount = AP::mount();
+    if (mount == nullptr) {
+        return;
+    }
+    mount->send_mount_status(chan);
+}
+
 bool GCS_MAVLINK::try_send_message(const enum ap_message id)
 {
     if (telemetry_delayed()) {
@@ -2961,6 +3096,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         queued_param_send();
         break;
 
+    case MSG_GIMBAL_REPORT:
+        CHECK_PAYLOAD_SIZE(GIMBAL_REPORT);
+        send_gimbal_report();
+        break;
+
     case MSG_HEARTBEAT:
         CHECK_PAYLOAD_SIZE(HEARTBEAT);
         last_heartbeat_time = AP_HAL::millis();
@@ -3053,6 +3193,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         send_local_position();
         break;
 
+    case MSG_MOUNT_STATUS:
+        CHECK_PAYLOAD_SIZE(MOUNT_STATUS);
+        send_mount_status();
+        break;
+
     case MSG_POSITION_TARGET_GLOBAL_INT:
         CHECK_PAYLOAD_SIZE(POSITION_TARGET_GLOBAL_INT);
         send_position_target_global_int();