From 6f902e54f993a93f536df5732652feda03e7cae4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 22 Sep 2021 13:22:05 +0200 Subject: [PATCH] mavlink: correctly lock radio status - hrt_elapsed_time_atomic is not atomic on posix - other fields like _radio_status_mult need protection as well --- src/modules/mavlink/mavlink_main.cpp | 18 ++++++++++++++++-- src/modules/mavlink/mavlink_main.h | 1 + src/modules/mavlink/mavlink_receiver.cpp | 2 +- 3 files changed, 18 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1b72dad3fb..c1a35de53b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1451,6 +1451,9 @@ Mavlink::update_rate_mult() } float hardware_mult = 1.0f; + bool log_radio_timeout = false; + + pthread_mutex_lock(&_radio_status_mutex); // scale down if we have a TX err rate suggesting link congestion if ((_tstatus.tx_error_rate_avg > 0.f) && !_radio_status_critical) { @@ -1459,9 +1462,9 @@ Mavlink::update_rate_mult() } else if (_radio_status_available) { // check for RADIO_STATUS timeout and reset - if (hrt_elapsed_time_atomic(&_rstatus.timestamp) > (_param_mav_radio_timeout.get() * 1_s)) { - PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id); + if (hrt_elapsed_time(&_rstatus.timestamp) > (_param_mav_radio_timeout.get() * 1_s)) { _radio_status_available = false; + log_radio_timeout = true; if (_use_software_mav_throttling) { _radio_status_critical = false; @@ -1472,6 +1475,12 @@ Mavlink::update_rate_mult() hardware_mult *= _radio_status_mult; } + pthread_mutex_unlock(&_radio_status_mutex); + + if (log_radio_timeout) { + PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id); + } + /* pick the minimum from bandwidth mult and hardware mult as limit */ _rate_mult = fminf(bandwidth_mult, hardware_mult); @@ -1482,6 +1491,7 @@ Mavlink::update_rate_mult() void Mavlink::update_radio_status(const radio_status_s &radio_status) { + pthread_mutex_lock(&_radio_status_mutex); _rstatus = radio_status; _radio_status_available = true; @@ -1506,6 +1516,8 @@ Mavlink::update_radio_status(const radio_status_s &radio_status) /* Constrain radio status multiplier between 1% and 100% to allow recovery */ _radio_status_mult = math::constrain(_radio_status_mult, 0.01f, 1.0f); } + + pthread_mutex_unlock(&_radio_status_mutex); } int @@ -2177,6 +2189,7 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize send mutex */ pthread_mutex_init(&_send_mutex, nullptr); + pthread_mutex_init(&_radio_status_mutex, nullptr); /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ if (_forwarding_on) { @@ -2618,6 +2631,7 @@ Mavlink::task_main(int argc, char *argv[]) } pthread_mutex_destroy(&_send_mutex); + pthread_mutex_destroy(&_radio_status_mutex); _task_running = false; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c3ba8344e2..3a3c340888 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -658,6 +658,7 @@ private: pthread_mutex_t _message_buffer_mutex {}; pthread_mutex_t _send_mutex {}; + pthread_mutex_t _radio_status_mutex {}; DEFINE_PARAMETERS( (ParamInt) _param_mav_sys_id, diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 022d8002eb..6449bd9c52 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1518,7 +1518,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) radio_status_s status{}; - hrt_store_absolute_time(&status.timestamp); + status.timestamp = hrt_absolute_time(); status.rssi = rstatus.rssi; status.remote_rssi = rstatus.remrssi; status.txbuf = rstatus.txbuf;