GCS_MAVLink: enable sending of RELAY_STATUS message

rmackay9 · Aug 8, 2023 · 13c83ee · 13c83ee
1 parent 1e18ca5
commit 13c83ee
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 0 deletions.
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
@@ -768,6 +768,7 @@ class GCS_MAVLINK
     virtual void        handleMessage(const mavlink_message_t &msg) = 0;
 
     MAV_RESULT handle_servorelay_message(const mavlink_command_long_t &packet);
+    bool send_relay_status() const;
 
     static bool command_long_stores_location(const MAV_CMD command);
 

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -1035,6 +1035,9 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
 #endif
 #if HAL_ADSB_ENABLED
         { MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_STATUS, MSG_UAVIONIX_ADSB_OUT_STATUS},
+#endif
+#if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
+        { MAVLINK_MSG_ID_RELAY_STATUS, MSG_RELAY_STATUS},
 #endif
             };
 
@@ -5534,6 +5537,19 @@ void GCS_MAVLINK::send_uavionix_adsb_out_status() const
 }
 #endif
 
+#if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
+bool GCS_MAVLINK::send_relay_status() const
+{
+    AP_Relay *relay = AP::relay();
+    if (relay == nullptr) {
+        // must only return false if out of space:
+        return true;
+    }
+
+    return relay->send_relay_status(*this);
+}
+#endif  // AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
+
 void GCS_MAVLINK::send_autopilot_state_for_gimbal_device() const
 {
     // get attitude
@@ -5994,6 +6010,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
 #endif
         break;
 
+    case MSG_RELAY_STATUS:
+#if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
+        ret = send_relay_status();
+        break;
+#endif
+
     default:
         // try_send_message must always at some stage return true for
         // a message, or we will attempt to infinitely retry the

diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL_Boards.h>
+#include <AP_Relay/AP_Relay_config.h>
 
 #ifndef HAL_GCS_ENABLED
 #define HAL_GCS_ENABLED 1
@@ -40,3 +41,6 @@
 #define HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED ((AP_FILESYSTEM_FATFS_ENABLED || AP_FILESYSTEM_POSIX_ENABLED) && BOARD_FLASH_SIZE > 1024)
 #endif
 
+#ifndef AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
+#define AP_MAVLINK_MSG_RELAY_STATUS_ENABLED HAL_GCS_ENABLED && AP_RELAY_ENABLED
+#endif
diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h
@@ -87,5 +87,6 @@ enum ap_message : uint8_t {
     MSG_ATTITUDE_TARGET,
     MSG_HYGROMETER,
     MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE,
+    MSG_RELAY_STATUS,
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };