diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1beac0c6f8..842428ff5b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1457,7 +1457,7 @@ Mavlink::update_rate_mult() } else if (_radio_status_available) { // check for RADIO_STATUS timeout and reset - if (hrt_elapsed_time(&_rstatus.timestamp) > (_param_mav_radio_timeout.get() * 1_s)) { + if (hrt_elapsed_time_atomic(&_rstatus.timestamp) > (_param_mav_radio_timeout.get() * 1_s)) { PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id); _radio_status_available = false; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index da97e95de4..cf491674b6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1506,7 +1506,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) radio_status_s status{}; - status.timestamp = hrt_absolute_time(); + hrt_store_absolute_time(&status.timestamp); status.rssi = rstatus.rssi; status.remote_rssi = rstatus.remrssi; status.txbuf = rstatus.txbuf;