diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index caa97d2ff5..2e74601929 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -350,6 +350,9 @@ private: // bitmask of what mavlink channels are active static uint8_t mavlink_active; + // bitmask of what mavlink channels are streaming + static uint8_t chan_is_streaming; + // mavlink routing object static MAVLink_routing routing; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index c3959860de..00fdf8a95f 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -28,6 +28,7 @@ extern const AP_HAL::HAL& hal; uint32_t GCS_MAVLINK::last_radio_status_remrssi_ms; uint8_t GCS_MAVLINK::mavlink_active = 0; +uint8_t GCS_MAVLINK::chan_is_streaming = 0; ObjectArray GCS_MAVLINK::_statustext_queue(GCS_MAVLINK_PAYLOAD_STATUS_CAPACITY); uint32_t GCS_MAVLINK::reserve_param_space_start_ms; @@ -673,7 +674,23 @@ bool GCS_MAVLINK::stream_trigger(enum streams stream_num) rate *= adjust_rate_for_stream_trigger(stream_num); if (rate <= 0) { + if (chan_is_streaming | (1U<<(chan-MAVLINK_COMM_0))) { + // if currently streaming then check if all streams are disabled + // to allow runtime detection of user disabling streaming + bool is_streaming = false; + for (uint8_t i=0; i 0) { + is_streaming = true; + } + } + if (!is_streaming) { + // all streams have been turned off, clear the bit flag + chan_is_streaming &= ~(1U<<(chan-MAVLINK_COMM_0)); + } + } return false; + } else { + chan_is_streaming |= (1U<<(chan-MAVLINK_COMM_0)); } if (stream_ticks[stream_num] == 0) { @@ -1278,7 +1295,7 @@ void GCS_MAVLINK::send_statustext_all(MAV_SEVERITY severity, const char *fmt, .. hal.util->vsnprintf((char *)text, sizeof(text)-1, fmt, arg_list); va_end(arg_list); text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] = 0; - send_statustext(severity, mavlink_active, text); + send_statustext(severity, mavlink_active | chan_is_streaming, text); } /* @@ -1306,7 +1323,7 @@ void GCS_MAVLINK::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, c // filter destination ports to only allow active ports. statustext_t statustext{}; - statustext.bitmask = mavlink_active & dest_bitmask; + statustext.bitmask = (mavlink_active | chan_is_streaming) & dest_bitmask; if (!statustext.bitmask) { // nowhere to send return;