diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index cb74a23040..859cf27685 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -5,11 +5,14 @@ * Author: Amilcar Lucas */ -#include -#include "AP_Relay.h" +#include "AP_Relay_config.h" #if AP_RELAY_ENABLED +#include "AP_Relay.h" + +#include +#include #include #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