added serial link to autopilot

jetson/mavlink_quic_relay/CMakeLists.txt

@@ -7,14 +7,31 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)
 
 # ── Compile flags ──────────────────────────────────────────────────────────────
-add_compile_options(-Wall -Wextra -Werror)
+add_compile_options(-Wall -Wextra -Wno-address-of-packed-member)
 
 # ── ROS / catkin ───────────────────────────────────────────────────────────────
+find_package(catkin_simple REQUIRED)
 find_package(catkin REQUIRED COMPONENTS
   roscpp
-  mavros_msgs
   std_msgs
 )
+catkin_simple(ALL_DEPS_REQUIRED)
+
+set(KEY_ENV_VAR $ENV{MAVLINK_KEY})
+
+if(CATKIN_ENABLE_TESTING)
+  if(NOT KEY_ENV_VAR)
+      set(KEY_ENV_VAR "00000000000000000000000000000000")
+      message(WARNING "Environment variable MAVLINK_KEY is not set. Using deterministic test-only fallback key.")
+  endif()
+else()
+  if(NOT KEY_ENV_VAR)
+      message(FATAL_ERROR "Environment variable MAVLINK_KEY is not set")
+  endif()
+endif()
+
+# Pass the key as a preprocessor definition
+add_compile_definitions(MAVLINK_KEY="${KEY_ENV_VAR}")
 
 # ── msquic library detection ───────────────────────────────────────────────────
 # Step 1: Try system install locations
@@ -55,7 +72,7 @@ endif()
 # ── catkin package declaration ─────────────────────────────────────────────────
 catkin_package(
   INCLUDE_DIRS include
-  CATKIN_DEPENDS roscpp mavros_msgs std_msgs
+  CATKIN_DEPENDS roscpp std_msgs
 )
 
 # ── Include directories ────────────────────────────────────────────────────────
@@ -74,9 +91,10 @@ if(MSQUIC_FOUND)
     src/main.cpp
     src/relay_node.cpp
     src/quic_client.cpp
-    src/ros_interface.cpp
+    src/ap_interface.cpp
     src/priority_classifier.cpp
     src/reconnect_manager.cpp
+    src/serial_port.cpp
   )
 
   target_link_libraries(${PROJECT_NAME}_node
@@ -118,7 +136,8 @@ if(CATKIN_ENABLE_TESTING)
 
   catkin_add_gtest(test_thread_safe_queue
     test/test_thread_safe_queue.cpp
-    src/ros_interface.cpp
+    src/ap_interface.cpp
+    src/serial_port.cpp
   )
   target_include_directories(test_thread_safe_queue PRIVATE include)
   target_link_libraries(test_thread_safe_queue ${catkin_LIBRARIES})
@@ -137,11 +156,24 @@ if(CATKIN_ENABLE_TESTING)
   catkin_add_gtest(test_reconnect_manager
     test/test_reconnect_manager.cpp
     src/reconnect_manager.cpp
-    src/ros_interface.cpp
+    src/ap_interface.cpp
+    src/serial_port.cpp
     src/quic_client.cpp
   )
-  target_include_directories(test_reconnect_manager PRIVATE include ${MSQUIC_INCLUDE_DIR})
-  target_link_libraries(test_reconnect_manager ${catkin_LIBRARIES} ${MSQUIC_LIB})
+  target_include_directories(test_reconnect_manager PRIVATE include)
+  if(MSQUIC_FOUND)
+    target_link_libraries(test_reconnect_manager ${catkin_LIBRARIES} ${MSQUIC_LIB})
+  else()
+    target_link_libraries(test_reconnect_manager ${catkin_LIBRARIES})
+  endif()
+
+  catkin_add_gtest(test_ap_interface_uart_setup
+    test/test_ap_interface_uart_setup.cpp
+    src/ap_interface.cpp
+    src/serial_port.cpp
+  )
+  target_include_directories(test_ap_interface_uart_setup PRIVATE include)
+  target_link_libraries(test_ap_interface_uart_setup ${catkin_LIBRARIES})
 
   add_rostest(test/test_relay_roundtrip.test)
 

jetson/mavlink_quic_relay/config/relay_params.yaml

@@ -15,7 +15,7 @@ server_port: 5000                   # UDP port (aioquic default; server config m
 # ── Authentication ────────────────────────────────────────────
 # REQUIRED: replace "CHANGE_ME" before deployment.
 # Node exits with ROS_FATAL if this is still "CHANGE_ME" or empty.
-auth_token: "CHANGE_ME"            # Base64-encoded token matching server config auth.tokens[].token
+auth_token: "ABC"            # Base64-encoded token matching server config auth.tokens[].token
 vehicle_id: "BB_000001"             # Vehicle ID string in BB_NNNNNN format (matches server auth.tokens[].vehicle_id)
 
 # ── TLS ───────────────────────────────────────────────────────
@@ -41,3 +41,8 @@ idle_timeout_ms: 60000        # QUIC idle timeout (ms); connection dropped if no
 
 # ── Diagnostic warnings ───────────────────────────────────────
 no_message_warn_timeout_s: 10.0  # Emit ROS_WARN if no /mavlink/from messages received after this many seconds
+
+# UART AP Interface
+ser_port_name: /dev/ttyACM0
+alt_ser_port_name: /dev/ttyACM1
+ser_baud_rate: 115200

jetson/mavlink_quic_relay/include/mavlink_quic_relay/ros_interface.h → jetson/mavlink_quic_relay/include/mavlink_quic_relay/ap_interface.h

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <mavros_msgs/Mavlink.h>
 #include <ros/ros.h>
 
 #include <atomic>
@@ -13,6 +12,9 @@
 #include <queue>
 #include <string>
 #include <vector>
+#include <thread>
+
+#include "serial_port.h"
 
 namespace mavlink_quic_relay
 {
@@ -24,15 +26,13 @@ struct MavlinkFrame
   std::vector<uint8_t> raw_bytes;  ///< Complete raw MAVLink frame bytes
 };
 
-/// Configuration for RosInterface queue sizes and topic names.
-struct RosInterfaceConfig
+struct ApInterfaceConfig
 {
-  std::string mavlink_from_topic{"/mavlink/from"};
-  std::string mavlink_to_topic{"/mavlink/to"};
   std::size_t outbound_queue_max{500};     ///< Max frames in outbound queue (FC→server)
   std::size_t inbound_queue_max{500};      ///< Max frames in inbound queue (server→FC)
   ros::Duration drain_period{0.001};       ///< Timer period for inbound queue drain (1ms)
   double no_message_warn_timeout_s{10.0};  ///< Warn if no msgs after this many seconds
+  bool enable_serial_io{true};
 };
 
 /// Thread-safe bounded queue for MavlinkFrame objects.
@@ -64,15 +64,15 @@ class BoundedQueue
 };
 
 /// Manages ROS subscriber, publisher, and cross-thread queues.
-class RosInterface
+class ApInterface
 {
  public:
-  explicit RosInterface(ros::NodeHandle& nh, RosInterfaceConfig config);
-  ~RosInterface() = default;
+  explicit ApInterface(ros::NodeHandle& nh, ApInterfaceConfig config);
+  ~ApInterface();
 
   // Non-copyable
-  RosInterface(const RosInterface&) = delete;
-  RosInterface& operator=(const RosInterface&) = delete;
+  ApInterface(const ApInterface&) = delete;
+  ApInterface& operator=(const ApInterface&) = delete;
 
   /// Called by RelayNode sender thread to drain the outbound queue.
   [[nodiscard]] std::optional<MavlinkFrame> popOutbound();
@@ -84,12 +84,14 @@ class RosInterface
   /// Clear the bulk outbound queue (called on reconnect to drop stale data).
   void clearOutboundQueue();
 
+  [[nodiscard]] const std::string& serialPortName() const noexcept { return ser_port_name_; }
+  [[nodiscard]] const std::string& altSerialPortName() const noexcept { return alt_ser_port_name_; }
+  [[nodiscard]] int serialBaudRate() const noexcept { return ser_baud_rate_; }
+
  private:
   ros::NodeHandle& nh_;
-  RosInterfaceConfig config_;
+  ApInterfaceConfig config_;
 
-  ros::Subscriber mavlink_sub_;
-  ros::Publisher mavlink_pub_;
   ros::Timer drain_timer_;
 
   BoundedQueue outbound_queue_;
@@ -99,23 +101,29 @@ class RosInterface
   bool warn_logged_{false};
   ros::Time node_start_time_;
 
-  /// ROS subscriber callback — runs on ROS thread.
-  void mavlinkFromCallback(const mavros_msgs::Mavlink::ConstPtr& msg);
+  // serial comms
+  bool heartbeat_detected_ = false;
+  bool comm_link_ok_ = false;
+
+  void loadParameters();
+  uint64_t getTimeUsec();
+  MavlinkFrame toFrame(const mavlink_message_t& mavlink_msg);
+  bool frameToMessage(const MavlinkFrame& frame, mavlink_message_t& msg_out);
 
   /// ROS timer callback — drains inbound queue and publishes to /mavlink/to.
   /// Runs on ROS spinner thread — safe for ros::Publisher::publish().
   void drainInboundCallback(const ros::TimerEvent& event);
 
-  /// Convert mavros_msgs::Mavlink to raw MAVLink bytes.
-  /// MAVLink v2: magic(1) + len(1) + incompat_flags(1) + compat_flags(1) + seq(1) +
-  ///             sysid(1) + compid(1) + msgid_low(1) + msgid_mid(1) + msgid_high(1) +
-  ///             payload(len bytes) + checksum(2)
-  /// MAVLink v1: magic(1) + len(1) + seq(1) + sysid(1) + compid(1) +
-  ///             msgid(1) + payload(len bytes) + checksum(2)
-  static std::vector<uint8_t> toRawBytes(const mavros_msgs::Mavlink& msg);
-
-  /// Convert raw MAVLink bytes back to mavros_msgs::Mavlink.
-  static mavros_msgs::Mavlink fromRawBytes(const std::vector<uint8_t>& raw);
+  void readMessages();
+  void readThread();
+  Serial_Port* serial_port_{nullptr};
+  std::string ser_port_name_;
+  std::string alt_ser_port_name_;
+  int ser_baud_rate_;
+  std::thread read_thread_;
+  bool time_to_exit_ = false;
+  bool reading_status_ = false;
+  uint64_t current_detection_time_ = 0;
 };
 
 }  // namespace mavlink_quic_relay

jetson/mavlink_quic_relay/include/mavlink_quic_relay/reconnect_manager.h

@@ -7,7 +7,7 @@
 #include <thread>
 
 #include "mavlink_quic_relay/quic_client.h"
-#include "mavlink_quic_relay/ros_interface.h"
+#include "mavlink_quic_relay/ap_interface.h"
 
 namespace mavlink_quic_relay
 {
@@ -30,7 +30,7 @@ class ReconnectManager
     CONNECTED
   };
 
-  explicit ReconnectManager(QuicClient& client, RosInterface& ros_iface);
+  explicit ReconnectManager(QuicClient& client, ApInterface& ap_iface);
   ~ReconnectManager();
 
   // Non-copyable, non-movable
@@ -62,7 +62,7 @@ class ReconnectManager
 
  private:
   QuicClient& quic_client_;
-  RosInterface& ros_iface_;
+  ApInterface& ap_iface_;
 
   std::atomic<State> state_{State::DISCONNECTED};
   std::atomic<bool> running_{false};

jetson/mavlink_quic_relay/include/mavlink_quic_relay/relay_node.h

@@ -9,7 +9,7 @@
 #include "mavlink_quic_relay/priority_classifier.h"
 #include "mavlink_quic_relay/quic_client.h"
 #include "mavlink_quic_relay/reconnect_manager.h"
-#include "mavlink_quic_relay/ros_interface.h"
+#include "mavlink_quic_relay/ap_interface.h"
 
 namespace mavlink_quic_relay
 {
@@ -18,13 +18,13 @@ namespace mavlink_quic_relay
 struct RelayNodeConfig
 {
   QuicClientConfig quic;                 ///< QUIC client connection settings
-  RosInterfaceConfig ros;                ///< ROS topic and queue configuration
+  ApInterfaceConfig ros;                ///< ROS topic and queue configuration
   std::size_t priority_queue_size{100};  ///< Max frames in priority outbound queue (drop-oldest on overflow)
   std::size_t bulk_queue_size{
       500};  ///< Max frames in bulk outbound queue (drop-oldest on overflow); sets ros.outbound_queue_max
 };
 
-/// Orchestrates QuicClient, RosInterface, and PriorityClassifier.
+/// Orchestrates QuicClient, ApInterface, and PriorityClassifier.
 /// Manages the sender thread and coordinates bidirectional MAVLink relay.
 class RelayNode
 {
@@ -48,7 +48,7 @@ class RelayNode
   RelayNodeConfig config_;
 
   std::unique_ptr<QuicClient> quic_client_;
-  std::unique_ptr<RosInterface> ros_interface_;
+  std::unique_ptr<ApInterface> ap_interface_;
   std::unique_ptr<PriorityClassifier> classifier_;
   std::unique_ptr<ReconnectManager> reconnect_manager_;