[mavlink] Use separate mutex for event buffer

This prevents the mavlink transmit loop from waiting on the module mutex
thus not stopping transmissions when the mutex is already taken.

This can happen when calling `mavlink status` from the mavlink shell,
where `Mavlink::get_status_all_instances()` takes the mutex and then
prints the status via pipes to the mavlink transmit buffer.
If that pipe cannot be emptied a deadlock happens.

Since the MavlinkReceiver thread also waits on the module mutex, both
reception and transmission of Mavlink packets are then prevented thus
disabling communications entirely.
This commit is contained in:
Niklas Hauser 2023-10-17 13:18:22 +02:00 committed by Daniel Agar
parent 48e09a4dea
commit f45b960eee
1 changed files with 14 additions and 7 deletions

View File

@ -81,6 +81,7 @@
#define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate
static pthread_mutex_t mavlink_module_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t mavlink_event_buffer_mutex = PTHREAD_MUTEX_INITIALIZER;
events::EventBuffer *Mavlink::_event_buffer = nullptr;
Mavlink *mavlink_module_instances[MAVLINK_COMM_NUM_BUFFERS] {};
@ -375,15 +376,21 @@ Mavlink::destroy_all_instances()
}
}
LockGuard lg{mavlink_module_mutex};
{
LockGuard lg{mavlink_module_mutex};
// we know all threads have exited, so it's safe to delete objects.
for (Mavlink *inst_to_del : mavlink_module_instances) {
delete inst_to_del;
// we know all threads have exited, so it's safe to delete objects.
for (Mavlink *inst_to_del : mavlink_module_instances) {
delete inst_to_del;
}
}
delete _event_buffer;
_event_buffer = nullptr;
{
LockGuard lg{mavlink_event_buffer_mutex};
delete _event_buffer;
_event_buffer = nullptr;
}
PX4_INFO("all instances stopped");
return OK;
@ -2543,7 +2550,7 @@ Mavlink::task_main(int argc, char *argv[])
/* handle new events */
if (check_events()) {
if (_event_sub.updated()) {
LockGuard lg{mavlink_module_mutex};
LockGuard lg{mavlink_event_buffer_mutex};
event_s orb_event;