Browse Source

GCS_MAVLink: send statustext to all GCSs until update_send is called

until update_send it called we don't mark channels as streaming, so
send_statustext won't send to that channel.

Calculating the streaming mask could have been done as part of the init
call, but this fix may allow for information getting to the user in the
case that the streaming parameters are all zero, too.
c415-sdk
Peter Barker 5 years ago committed by Andrew Tridgell
parent
commit
ebbcd00873
  1. 9
      libraries/GCS_MAVLink/GCS.cpp
  2. 3
      libraries/GCS_MAVLink/GCS.h
  3. 11
      libraries/GCS_MAVLink/GCS_Common.cpp

9
libraries/GCS_MAVLink/GCS.cpp

@ -39,7 +39,14 @@ void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list) @@ -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, ...)

3
libraries/GCS_MAVLink/GCS.h

@ -949,6 +949,9 @@ private: @@ -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;

11
libraries/GCS_MAVLink/GCS_Common.cpp

@ -1783,7 +1783,15 @@ void GCS::send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const cha @@ -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) @@ -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:

Loading…
Cancel
Save