GCS_MAVLink: handle RADIO_RC_CHANNELS

ArduPilot · Aug 10, 2023 · 65e6da7 · 65e6da7
1 parent 6eb6306
commit 65e6da7
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 0 deletions.
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
@@ -674,6 +674,7 @@ class GCS_MAVLINK
     void handle_optical_flow(const mavlink_message_t &msg);
 
     void handle_manual_control(const mavlink_message_t &msg);
+    void handle_radio_rc_channels(const mavlink_message_t &msg);
 
     // default empty handling of LANDING_TARGET
     virtual void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { }
@@ -738,6 +739,9 @@ class GCS_MAVLINK
     mavlink_message_t _channel_buffer;
     mavlink_status_t _channel_status;
 
+    // for mavlink radio
+    mavlink_radio_t _mavlink_radio_packet;
+
     const AP_SerialManager::UARTState *uartstate;
 
     // last time we got a non-zero RSSI from RADIO_STATUS

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -4037,6 +4037,9 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
     case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
         handle_rc_channels_override(msg);
         break;
+    case MAVLINK_MSG_ID_RADIO_RC_CHANNELS:
+        handle_radio_rc_channels(msg);
+        break;
 
 #if AP_OPTICALFLOW_ENABLED
     case MAVLINK_MSG_ID_OPTICAL_FLOW:
@@ -6344,6 +6347,12 @@ bool GCS_MAVLINK::accept_packet(const mavlink_status_t &status,
         return true;
     }
 
+    // handle messages from a mavlink receiver
+    if ((msg.compid == MAV_COMP_ID_TELEMETRY_RADIO) &&
+        (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS)) {
+        return true;
+    }
+
     if (!sysid_enforce()) {
         return true;
     }
@@ -6716,3 +6725,12 @@ MAV_RESULT GCS_MAVLINK::handle_control_high_latency(const mavlink_command_long_t
     return MAV_RESULT_ACCEPTED;
 }
 #endif // HAL_HIGH_LATENCY2_ENABLED
+
+void GCS_MAVLINK::handle_radio_rc_channels(const mavlink_message_t &msg)
+{
+    mavlink_radio_rc_channels_t packet;
+    mavlink_msg_radio_rc_channels_decode(&msg, &packet);
+
+    AP::RC().handle_radio_rc_channels(&packet);
+}
+
diff --git a/libraries/GCS_MAVLink/MAVLink_routing.cpp b/libraries/GCS_MAVLink/MAVLink_routing.cpp
@@ -127,6 +127,13 @@ bool MAVLink_routing::check_and_forward(GCS_MAVLINK &in_link, const mavlink_mess
     int16_t target_component = -1;
     get_targets(msg, target_system, target_component);
 
+    // handle messages from a mavlink receiver as if it had target_system = our system, target_component = 0
+    if ((msg.compid == MAV_COMP_ID_TELEMETRY_RADIO) &&
+        (msg.msgid == MAVLINK_MSG_ID_RADIO_RC_CHANNELS)) {
+        target_system = mavlink_system.sysid;
+        target_component = 0;
+    }
+
     bool broadcast_system = (target_system == 0 || target_system == -1);
     bool broadcast_component = (target_component == 0 || target_component == -1);
     bool match_system = broadcast_system || (target_system == mavlink_system.sysid);