GCS_MAVLink: fixed mavlink packet corruption with multiple threads

this ensures we don't try to send more data to a uart than is
available in the tx buffer
This commit is contained in:
Andrew Tridgell 2022-02-14 17:28:16 +11:00
parent fd8d28e23c
commit cce89099c5

View File

@ -2053,6 +2053,8 @@ void GCS_MAVLINK::service_statustext(void)
{
GCS::StatusTextQueue &_statustext_queue = gcs().statustext_queue();
WITH_SEMAPHORE(comm_chan_lock(chan));
const uint8_t chan_bit = (1U<<chan);
// note the lack of idx++ here. We may remove the iteration item
// from the queue as the last thing we do, in which case we don't