GCS_MAVLink: support gimbal_manager_status and do_gimbal_manager_configure

Co-authored-by: Randy Mackay <rmackay9@yahoo.com>

libraries/GCS_MAVLink/GCS.h

@@ -368,6 +368,7 @@ public:
     void send_vibration() const;
     void send_gimbal_device_attitude_status() const;
     void send_gimbal_manager_information() const;
+    void send_gimbal_manager_status() const;
     void send_named_float(const char *name, float value) const;
     void send_home_position() const;
     void send_gps_global_origin() const;
@@ -638,7 +639,7 @@ protected:
     MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_do_set_roi_none();
 
-    virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet);
+    virtual MAV_RESULT handle_command_mount(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
     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);

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -990,6 +990,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
         { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS},
         { MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE},
         { MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, MSG_GIMBAL_MANAGER_INFORMATION},
+        { MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, MSG_GIMBAL_MANAGER_STATUS},
 #endif
 #if AP_OPTICALFLOW_ENABLED
         { MAVLINK_MSG_ID_OPTICAL_FLOW,          MSG_OPTICAL_FLOW},
@@ -4540,14 +4541,14 @@ MAV_RESULT GCS_MAVLINK::handle_command_accelcal_vehicle_pos(const mavlink_comman
 }
 #endif  // HAL_INS_ACCELCAL_ENABLED
 
-MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_long_t &packet)
+MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_long_t &packet, const mavlink_message_t &msg)
 {
 #if HAL_MOUNT_ENABLED
     AP_Mount *mount = AP::mount();
     if (mount == nullptr) {
         return MAV_RESULT_UNSUPPORTED;
     }
-    return mount->handle_command_long(packet);
+    return mount->handle_command_long(packet, msg);
 #else
     return MAV_RESULT_UNSUPPORTED;
 #endif
@@ -4678,12 +4679,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
         break;
 #endif
 
-    case MAV_CMD_DO_MOUNT_CONFIGURE:
-    case MAV_CMD_DO_MOUNT_CONTROL:
-    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
-        result = handle_command_mount(packet);
-        break;
-
 #if AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED
     case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
         result = handle_command_request_autopilot_capabilities(packet);
@@ -4892,6 +4887,14 @@ void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg)
         result = handle_command_preflight_calibration(packet, msg);
         break;
 
+    case MAV_CMD_DO_MOUNT_CONFIGURE:
+    case MAV_CMD_DO_MOUNT_CONTROL:
+    case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
+    case MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
+        // some mount commands require the source sysid and compid
+        result = handle_command_mount(packet, msg);
+        break;
+
     default:
          result = handle_command_long_packet(packet);
          break;
@@ -5334,9 +5337,7 @@ void GCS_MAVLINK::send_gimbal_device_attitude_status() const
     }
     mount->send_gimbal_device_attitude_status(chan);
 }
-#endif
 
-#if HAL_MOUNT_ENABLED
 void GCS_MAVLINK::send_gimbal_manager_information() const
 {
     AP_Mount *mount = AP::mount();
@@ -5345,6 +5346,15 @@ void GCS_MAVLINK::send_gimbal_manager_information() const
     }
     mount->send_gimbal_manager_information(chan);
 }
+
+void GCS_MAVLINK::send_gimbal_manager_status() const
+{
+    AP_Mount *mount = AP::mount();
+    if (mount == nullptr) {
+        return;
+    }
+    mount->send_gimbal_manager_status(chan);
+}
 #endif
 
 void GCS_MAVLINK::send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc)
@@ -5675,6 +5685,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
 #if HAL_MOUNT_ENABLED
         CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_INFORMATION);
         send_gimbal_manager_information();
+#endif
+        break;
+    case MSG_GIMBAL_MANAGER_STATUS:
+#if HAL_MOUNT_ENABLED
+        CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_STATUS);
+        send_gimbal_manager_status();
 #endif
         break;
     case MSG_OPTICAL_FLOW:

libraries/GCS_MAVLink/ap_message.h

@@ -51,6 +51,7 @@ enum ap_message : uint8_t {
     MSG_CAMERA_FEEDBACK,
     MSG_GIMBAL_DEVICE_ATTITUDE_STATUS,
     MSG_GIMBAL_MANAGER_INFORMATION,
+    MSG_GIMBAL_MANAGER_STATUS,
     MSG_OPTICAL_FLOW,
     MSG_MAG_CAL_PROGRESS,
     MSG_MAG_CAL_REPORT,