Add sending of RELAY_STATUS message

Tools/autotest/rover.py

@@ -607,6 +607,55 @@ def ServoRelayEvents(self):
             raise NotAchievedException(
                 "Pin mask unchanged after relay cmd")
         self.progress("Pin mask changed after relay command")
+        self.do_set_relay(0, 0)
+
+        self.set_message_rate_hz("RELAY_STATUS", 10)
+
+        # default configuration for relays in sim have one relay:
+        self.assert_received_message_field_values('RELAY_STATUS', {
+            "present": 3,
+            "on": 0,
+        })
+        self.do_set_relay(0, 1)
+        self.assert_received_message_field_values('RELAY_STATUS', {
+            "present": 3,
+            "on": 1,
+        })
+        self.do_set_relay(1, 1)
+        self.assert_received_message_field_values('RELAY_STATUS', {
+            "present": 3,
+            "on": 3,
+        })
+        self.do_set_relay(0, 0)
+        self.do_set_relay(1, 0)
+        self.assert_received_message_field_values('RELAY_STATUS', {
+            "present": 3,
+            "on": 0,
+        })
+
+        # add another servo:
+        self.set_parameter("RELAY_PIN6", 14)
+        self.assert_received_message_field_values('RELAY_STATUS', {
+            "present": 35,
+            "on": 0,
+        })
+        self.do_set_relay(5, 1)
+        self.assert_received_message_field_values('RELAY_STATUS', {
+            "present": 35,
+            "on": 32,
+        })
+        self.do_set_relay(0, 1)
+        self.assert_received_message_field_values('RELAY_STATUS', {
+            "present": 35,
+            "on": 33,
+        })
+        self.do_set_relay(5, 0)
+        self.assert_received_message_field_values('RELAY_STATUS', {
+            "present": 35,
+            "on": 1,
+        })
+
+        self.set_message_rate_hz("RELAY_STATUS", 0)
 
     def MAVProxy_SetModeUsingSwitch(self):
         """Set modes via mavproxy switch"""

libraries/AP_HAL_ChibiOS/hwdef/include/minimize_features.inc

@@ -133,3 +133,6 @@ define MODE_GUIDED_NOGPS_ENABLED 0
 define MODE_SYSTEMID_ENABLED 0
 define WEATHERVANE_ENABLED 0
 define MODE_AUTOROTATE_ENABLED 0
+
+# don't send RELAY_STATUS messages:
+define AP_MAVLINK_MSG_RELAY_STATUS_ENABLED 0

libraries/AP_Relay/AP_Relay.cpp

@@ -5,11 +5,14 @@
  *      Author: Amilcar Lucas
  */
 
-#include <AP_HAL/AP_HAL.h>
-#include "AP_Relay.h"
+#include "AP_Relay_config.h"
 
 #if AP_RELAY_ENABLED
 
+#include "AP_Relay.h"
+
+#include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS.h>
 #include <AP_Logger/AP_Logger.h>
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@@ -190,6 +193,40 @@ bool AP_Relay::arming_checks(size_t buflen, char *buffer) const
     return true;
 }
 
+
+#if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
+// this method may only return false if there is no space in the
+// supplied link for the message.
+bool AP_Relay::send_relay_status(const GCS_MAVLINK &link) const
+{
+    if (!HAVE_PAYLOAD_SPACE(link.get_chan(), RELAY_STATUS)) {
+        return false;
+    }
+
+    uint16_t present_mask = 0;
+    uint16_t on_mask = 0;
+    for (auto i=0; i<AP_RELAY_NUM_RELAYS; i++) {
+        if (!enabled(i)) {
+            continue;
+        }
+        const uint16_t relay_bit_mask = 1U << i;
+        present_mask |= relay_bit_mask;
+
+        if (_pin_states & relay_bit_mask) {
+            on_mask |= relay_bit_mask;
+        }
+    }
+
+    mavlink_msg_relay_status_send(
+        link.get_chan(),
+        AP_HAL::millis(),
+        on_mask,
+        present_mask
+        );
+    return true;
+}
+#endif  // AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
+
 namespace AP {
 
 AP_Relay *relay()

libraries/AP_Relay/AP_Relay.h

@@ -41,7 +41,7 @@ class AP_Relay {
     }
 
     // see if the relay is enabled
-    bool        enabled(uint8_t instance) { return instance < AP_RELAY_NUM_RELAYS && _pin[instance] != -1; }
+    bool        enabled(uint8_t instance) const { return instance < AP_RELAY_NUM_RELAYS && _pin[instance] != -1; }
 
     // toggle the relay status
     void        toggle(uint8_t instance);
@@ -53,6 +53,8 @@ class AP_Relay {
 
     static const struct AP_Param::GroupInfo        var_info[];
 
+    bool send_relay_status(const class GCS_MAVLINK &link) const;
+
 private:
     static AP_Relay *singleton;
 

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);
 

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

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

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
 };