From 5c66295bdd5dddeb19fb941094dcca9d0b230455 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Sun, 1 Jun 2025 14:12:45 +0100 Subject: [PATCH] feat (MAVLink): use RADIO_STATUS to fill input_rc data Signed-off-by: Beniamino Pozzan --- src/modules/mavlink/mavlink_main.cpp | 11 +++++++++++ src/modules/mavlink/mavlink_main.h | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 18 ++++++++++++++---- 3 files changed, 27 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f364a57487..02bde7f0e7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1393,6 +1393,17 @@ Mavlink::update_radio_status(const radio_status_s &radio_status) pthread_mutex_unlock(&_radio_status_mutex); } +bool +Mavlink::get_radio_status(radio_status_s &radio_status) const +{ + if (_radio_status_available) { + radio_status = _rstatus; + return true; + } + + return false; +} + int Mavlink::configure_streams_to_default(const char *configure_single_stream) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 304bd06d58..0c8c163377 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -449,6 +449,8 @@ public: void update_radio_status(const radio_status_s &radio_status); + bool get_radio_status(radio_status_s &radio_status) const; + unsigned get_system_type() { return _param_mav_type.get(); } Protocol get_protocol() const { return _protocol; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e68353eabd..a55a00c3a6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2006,7 +2006,20 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) input_rc_s rc{}; // metadata rc.timestamp = rc.timestamp_last_signal = hrt_absolute_time(); - rc.rssi = input_rc_s::RSSI_MAX; + + radio_status_s radio_status{}; + + if (_mavlink.get_radio_status(radio_status)) { + rc.rssi_dbm = -(float)(radio_status.remote_rssi); + rc.link_quality = (int8_t)(radio_status.rssi / 2.55); // convert to percentage + + } else { + rc.rssi_dbm = NAN; + rc.link_quality = -1; + } + + rc.rssi = rc.link_quality; + rc.rc_failsafe = false; rc.rc_lost = false; rc.rc_lost_frame_count = 0; @@ -2071,9 +2084,6 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) } } - rc.link_quality = -1; - rc.rssi_dbm = NAN; - // publish uORB message _rc_pub.publish(rc); }