aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: fixed mavlink packet corruption with multiple threads

Browse Source 
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 committed by Peter Barker
parent 828af93e31
commit 44d9d410a1
3 changed files with 30 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1051,6 +1051,7 @@ bool GCS_MAVLINK::do_try_send_message(const ap_message id)
if (telemetry_delayed()) {
return false;
}
WITH_SEMAPHORE(comm_chan_lock(chan));
#if GCS_DEBUG_SEND_MESSAGE_TIMINGS
void *data = hal.scheduler->disable_interrupts_save();
uint32_t start_send_message_us = AP_HAL::micros();
@ -2058,6 +2059,8 @@ void GCS_MAVLINK::service_statustext(void)
// want to move idx.
const uint16_t payload_size = PAYLOAD_SIZE(chan, STATUSTEXT);
for (uint8_t idx=0; idx<_statustext_queue.available(); ) {
WITH_SEMAPHORE(comm_chan_lock(chan));
if (txspace() < payload_size) {
break;
}

29
libraries/GCS_MAVLink/GCS_MAVLink.cpp
View File

@ -41,6 +41,7 @@ bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS];
// per-channel lock
static HAL_Semaphore chan_locks[MAVLINK_COMM_NUM_BUFFERS];
static bool chan_discard[MAVLINK_COMM_NUM_BUFFERS];
mavlink_system_t mavlink_system = {7,1};
@ -90,7 +91,7 @@ uint16_t comm_get_txspace(mavlink_channel_t chan)
*/
void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
{
if (!valid_channel(chan) || mavlink_comm_port[chan] == nullptr) {
if (!valid_channel(chan) || mavlink_comm_port[chan] == nullptr || chan_discard[chan]) {
return;
}
if (gcs_alternative_active[chan]) {
@ -109,16 +110,34 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
/*
lock a channel for send
if there is insufficient space to send size bytes then all bytes
written to the channel by the mavlink library will be discarded
while the lock is held.
*/
void comm_send_lock(mavlink_channel_t chan)
void comm_send_lock(mavlink_channel_t chan_m, uint16_t size)
{
chan_locks[(uint8_t)chan].take_blocking();
const uint8_t chan = uint8_t(chan_m);
chan_locks[chan].take_blocking();
if (mavlink_comm_port[chan]->txspace() < size) {
chan_discard[chan] = true;
}
}
/*
unlock a channel
*/
void comm_send_unlock(mavlink_channel_t chan)
void comm_send_unlock(mavlink_channel_t chan_m)
{
chan_locks[(uint8_t)chan].give();
const uint8_t chan = uint8_t(chan_m);
chan_discard[chan] = false;
chan_locks[chan].give();
}
/*
return reference to GCS channel lock, allowing for
HAVE_PAYLOAD_SPACE() to be run with a locked channel
*/
HAL_Semaphore &comm_chan_lock(mavlink_channel_t chan)
{
return chan_locks[uint8_t(chan)];
}

5
libraries/GCS_MAVLink/GCS_MAVLink.h
View File

@ -11,7 +11,7 @@
#define MAVLINK_SEND_UART_BYTES(chan, buf, len) comm_send_buffer(chan, buf, len)
#define MAVLINK_START_UART_SEND(chan, size) comm_send_lock(chan)
#define MAVLINK_START_UART_SEND(chan, size) comm_send_lock(chan, size)
#define MAVLINK_END_UART_SEND(chan, size) comm_send_unlock(chan)
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -70,7 +70,8 @@ uint16_t comm_get_txspace(mavlink_channel_t chan);
#include "include/mavlink/v2.0/all/mavlink.h"
// lock and unlock a channel, for multi-threaded mavlink send
void comm_send_lock(mavlink_channel_t chan);
void comm_send_lock(mavlink_channel_t chan, uint16_t size);
void comm_send_unlock(mavlink_channel_t chan);
HAL_Semaphore &comm_chan_lock(mavlink_channel_t chan);
#pragma GCC diagnostic pop