Browse Source

GCS_MAVLink: correct is_streaming check and update of is-streaming mask

master
Peter Barker 6 years ago committed by Andrew Tridgell
parent
commit
a025794bae
  1. 2
      libraries/GCS_MAVLink/GCS.h
  2. 6
      libraries/GCS_MAVLink/GCS_Common.cpp

2
libraries/GCS_MAVLink/GCS.h

@ -378,7 +378,7 @@ public: @@ -378,7 +378,7 @@ public:
return GCS_MAVLINK::active_channel_mask() & (1 << (chan-MAVLINK_COMM_0));
}
bool is_streaming() const {
return GCS_MAVLINK::streaming_channel_mask() & (1 << (chan-MAVLINK_COMM_0));
return sending_bucket_id != no_bucket_to_send;
}
mavlink_channel_t get_chan() const { return chan; }

6
libraries/GCS_MAVLink/GCS_Common.cpp

@ -1659,6 +1659,12 @@ void GCS_MAVLINK::send_message(enum ap_message id) @@ -1659,6 +1659,12 @@ void GCS_MAVLINK::send_message(enum ap_message id)
{
if (id == MSG_HEARTBEAT) {
save_signing_timestamp(false);
// update the mask of all streaming channels
if (is_streaming()) {
GCS_MAVLINK::chan_is_streaming |= (1U<<chan-MAVLINK_COMM_0);
} else {
GCS_MAVLINK::chan_is_streaming &= ~(1U<<chan-MAVLINK_COMM_0);
}
}
pushed_ap_message_ids.set(id);

Loading…
Cancel
Save