GCS_MAVLink: support requests for gimbal-manager-information

ArduPilot · Apr 24, 2023 · 78e0726 · 78e0726
1 parent bd175af
commit 78e0726
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 1 deletion.
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
@@ -371,6 +371,7 @@ class GCS_MAVLINK
     void send_vfr_hud();
     void send_vibration() const;
     void send_gimbal_device_attitude_status() const;
+    void send_gimbal_manager_information() const;
     void send_named_float(const char *name, float value) const;
     void send_home_position() const;
     void send_gps_global_origin() const;

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -989,6 +989,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
 #if HAL_MOUNT_ENABLED
         { 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},
 #endif
 #if AP_OPTICALFLOW_ENABLED
         { MAVLINK_MSG_ID_OPTICAL_FLOW,          MSG_OPTICAL_FLOW},
@@ -5324,6 +5325,17 @@ void GCS_MAVLINK::send_gimbal_device_attitude_status() const
 }
 #endif
 
+#if HAL_MOUNT_ENABLED
+void GCS_MAVLINK::send_gimbal_manager_information() const
+{
+    AP_Mount *mount = AP::mount();
+    if (mount == nullptr) {
+        return;
+    }
+    mount->send_gimbal_manager_information(chan);
+}
+#endif
+
 void GCS_MAVLINK::send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc)
 {
 
@@ -5648,7 +5660,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         send_autopilot_state_for_gimbal_device();
 #endif
         break;
-
+    case MSG_GIMBAL_MANAGER_INFORMATION:
+#if HAL_MOUNT_ENABLED
+        CHECK_PAYLOAD_SIZE(GIMBAL_MANAGER_INFORMATION);
+        send_gimbal_manager_information();
+#endif
+        break;
     case MSG_OPTICAL_FLOW:
 #if AP_OPTICALFLOW_ENABLED
         CHECK_PAYLOAD_SIZE(OPTICAL_FLOW);

diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h
@@ -50,6 +50,7 @@ enum ap_message : uint8_t {
     MSG_BATTERY2,
     MSG_CAMERA_FEEDBACK,
     MSG_GIMBAL_DEVICE_ATTITUDE_STATUS,
+    MSG_GIMBAL_MANAGER_INFORMATION,
     MSG_OPTICAL_FLOW,
     MSG_MAG_CAL_PROGRESS,
     MSG_MAG_CAL_REPORT,