diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index abe9a583f1..356f72c84c 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -39,7 +39,14 @@ void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list) { char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; hal.util->vsnprintf(text, sizeof(text), fmt, arg_list); - send_statustext(severity, GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask(), text); + uint8_t mask = GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask(); + if (!update_send_has_been_called) { + // we have not yet initialised the streaming-channel-mask, + // which is done as part of the update() call. So just send + // it to all channels: + mask = (1<<_num_gcs)-1; + } + send_statustext(severity, mask, text); } void GCS::send_text(MAV_SEVERITY severity, const char *fmt, ...) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 2a8f4a4d10..96f3872819 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -949,6 +949,9 @@ private: // true if we have already allocated protocol objects: bool initialised_missionitemprotocol_objects; + // true if update_send has ever been called: + bool update_send_has_been_called; + // handle passthru between two UARTs struct { bool enabled; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index ade1c3c1df..7c27b41818 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1783,7 +1783,15 @@ void GCS::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const cha // filter destination ports to only allow active ports. statustext_t statustext{}; - statustext.bitmask = (GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask() ) & dest_bitmask; + if (update_send_has_been_called) { + statustext.bitmask = (GCS_MAVLINK::active_channel_mask() | GCS_MAVLINK::streaming_channel_mask() ); + } else { + // we have not yet initialised the streaming-channel-mask, + // which is done as part of the update() call. So just send + // it to all channels: + statustext.bitmask = (1<<_num_gcs)-1; + } + statustext.bitmask &= dest_bitmask; if (!statustext.bitmask) { // nowhere to send return; @@ -1860,6 +1868,7 @@ void GCS::send_message(enum ap_message id) void GCS::update_send() { + update_send_has_been_called = true; if (!initialised_missionitemprotocol_objects) { initialised_missionitemprotocol_objects = true; // once-only initialisation of MissionItemProtocol objects: