diff --git a/jetson/mavlink_quic_relay/CMakeLists.txt b/jetson/mavlink_quic_relay/CMakeLists.txt index 27e725c..1f17fe9 100644 --- a/jetson/mavlink_quic_relay/CMakeLists.txt +++ b/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) diff --git a/jetson/mavlink_quic_relay/config/relay_params.yaml b/jetson/mavlink_quic_relay/config/relay_params.yaml index 6a47f03..ebae12e 100644 --- a/jetson/mavlink_quic_relay/config/relay_params.yaml +++ b/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 \ No newline at end of file diff --git a/jetson/mavlink_quic_relay/include/mavlink_quic_relay/ros_interface.h b/jetson/mavlink_quic_relay/include/mavlink_quic_relay/ap_interface.h similarity index 68% rename from jetson/mavlink_quic_relay/include/mavlink_quic_relay/ros_interface.h rename to jetson/mavlink_quic_relay/include/mavlink_quic_relay/ap_interface.h index d420be7..9c819ff 100644 --- a/jetson/mavlink_quic_relay/include/mavlink_quic_relay/ros_interface.h +++ b/jetson/mavlink_quic_relay/include/mavlink_quic_relay/ap_interface.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include @@ -13,6 +12,9 @@ #include #include #include +#include + +#include "serial_port.h" namespace mavlink_quic_relay { @@ -24,15 +26,13 @@ struct MavlinkFrame std::vector 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 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 toRawBytes(const mavros_msgs::Mavlink& msg); - - /// Convert raw MAVLink bytes back to mavros_msgs::Mavlink. - static mavros_msgs::Mavlink fromRawBytes(const std::vector& 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 diff --git a/jetson/mavlink_quic_relay/include/mavlink_quic_relay/reconnect_manager.h b/jetson/mavlink_quic_relay/include/mavlink_quic_relay/reconnect_manager.h index 2bb013b..e255703 100644 --- a/jetson/mavlink_quic_relay/include/mavlink_quic_relay/reconnect_manager.h +++ b/jetson/mavlink_quic_relay/include/mavlink_quic_relay/reconnect_manager.h @@ -7,7 +7,7 @@ #include #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::DISCONNECTED}; std::atomic running_{false}; diff --git a/jetson/mavlink_quic_relay/include/mavlink_quic_relay/relay_node.h b/jetson/mavlink_quic_relay/include/mavlink_quic_relay/relay_node.h index 003b27c..263a3a7 100644 --- a/jetson/mavlink_quic_relay/include/mavlink_quic_relay/relay_node.h +++ b/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 quic_client_; - std::unique_ptr ros_interface_; + std::unique_ptr ap_interface_; std::unique_ptr classifier_; std::unique_ptr reconnect_manager_; diff --git a/jetson/mavlink_quic_relay/include/mavlink_quic_relay/serial_port.h b/jetson/mavlink_quic_relay/include/mavlink_quic_relay/serial_port.h new file mode 100644 index 0000000..efd37b9 --- /dev/null +++ b/jetson/mavlink_quic_relay/include/mavlink_quic_relay/serial_port.h @@ -0,0 +1,114 @@ +/** + * @file serial_port.h + * + * @brief UART communication with AP + * + * @author Kunal Shrivastava (kunal@suind.com) + * + */ + +#pragma once + +#ifndef SERIAL_PORT_H_ +#define SERIAL_PORT_H_ + +// ------------------------------------------------------------------------------ +// Includes +// ------------------------------------------------------------------------------ +#include // File control definitions +#include // This uses POSIX Threads +#include +#include // Standard input/output definitions +#include // POSIX terminal control definitions +#include // UNIX standard function definitions + +#include +#include "ros/ros.h" + +#include "mavlink/v2.0/all/mavlink.h" + +// ------------------------------------------------------------------------------ +// Defines +// ------------------------------------------------------------------------------ + +// The following two non-standard baudrates should have been defined by the system +// If not, just fallback to number +#ifndef B460800 +#define B460800 460800 +#endif + +#ifndef B921600 +#define B921600 921600 +#endif + +// Status flags +#define SERIAL_PORT_OPEN 1 +#define SERIAL_PORT_CLOSED 0 +#define SERIAL_PORT_ERROR -1 + +// ------------------------------------------------------------------------------ +// Prototypes +// ------------------------------------------------------------------------------ + +// class Serial_Port; + +// ---------------------------------------------------------------------------------- +// Serial Port Manager Class +// ---------------------------------------------------------------------------------- +/* + * Serial Port Class + * + * This object handles the opening and closing of the offboard computer's + * serial port over which we'll communicate. It also has methods to write + * a byte stream buffer. MAVlink is not used in this object yet, it's just + * a serialization interface. To help with read and write pthreading, it + * gaurds any port operation with a pthread mutex. + */ + +class Serial_Port +{ + public: + Serial_Port(); + Serial_Port(const char *uart_name_, const char *alt_uart_name_, int baudrate_); + void initialize_defaults(); + ~Serial_Port(); + + int read_message(mavlink_message_t &message); + int write_message(const mavlink_message_t &message); + + void open_serial(); + void close_serial(); + + void start(); + void stop(); + + void handle_quit(int sig); + + // pixhawk_bridge::PixHawkBridge *pixhawk_bridge_; + // log_downloader::LogDownloader *log_downloader_; + + private: + mavlink_status_t lastStatus; + mavlink_status_t* mavlink_chan_status_; + pthread_mutex_t lock; + + bool debug_ = false; + int status_; + bool port_open_success_ = false; + std::string type_; + + const char *uart_name_; + const char *uart_name_alt_; + int baudrate_; + int fd_; + + int _open_port(const char *port); + bool _setup_port(int baud, int data_bits, int stop_bits, bool parity, bool hardware_control); + int _read_port(uint8_t &cp); + int _write_port(char *buf, unsigned len); + + mavlink_signing_t signing; + void initSigning(); +}; + +#endif // SERIAL_PORT_H_ diff --git a/jetson/mavlink_quic_relay/package.xml b/jetson/mavlink_quic_relay/package.xml index ed05809..acf1fed 100644 --- a/jetson/mavlink_quic_relay/package.xml +++ b/jetson/mavlink_quic_relay/package.xml @@ -9,7 +9,8 @@ ALPN: mavlink-quic-v1. - Kevin + Kevin + Kunal MIT @@ -20,9 +21,11 @@ roscpp - mavros_msgs std_msgs + + pixhawk_bridge + rostest diff --git a/jetson/mavlink_quic_relay/src/ap_interface.cpp b/jetson/mavlink_quic_relay/src/ap_interface.cpp new file mode 100644 index 0000000..4e95af5 --- /dev/null +++ b/jetson/mavlink_quic_relay/src/ap_interface.cpp @@ -0,0 +1,412 @@ +#include "mavlink_quic_relay/ap_interface.h" + +#include +#include +#include + +namespace mavlink_quic_relay +{ + +// --------------------------------------------------------------------------- +// BoundedQueue +// --------------------------------------------------------------------------- + +BoundedQueue::BoundedQueue(std::size_t max_size) : max_size_(max_size) {} + +void BoundedQueue::push(MavlinkFrame frame) +{ + std::lock_guard lock(mutex_); + if (queue_.size() >= max_size_) + { + queue_.pop(); + } + queue_.push(std::move(frame)); +} + +std::optional BoundedQueue::tryPop() +{ + std::lock_guard lock(mutex_); + if (queue_.empty()) + { + return std::nullopt; + } + MavlinkFrame frame = std::move(queue_.front()); + queue_.pop(); + return frame; +} + +std::size_t BoundedQueue::size() const +{ + std::lock_guard lock(mutex_); + return queue_.size(); +} + +void BoundedQueue::clear() +{ + std::lock_guard lock(mutex_); + while (!queue_.empty()) + { + queue_.pop(); + } +} + +// --------------------------------------------------------------------------- +// ApInterface +// --------------------------------------------------------------------------- + +ApInterface::ApInterface(ros::NodeHandle& nh, ApInterfaceConfig config) + : nh_(nh), + config_(std::move(config)), + outbound_queue_(config_.outbound_queue_max), + inbound_queue_(config_.inbound_queue_max) +{ + loadParameters(); + + drain_timer_ = nh_.createTimer(config_.drain_period, &ApInterface::drainInboundCallback, this); + + node_start_time_ = ros::Time::now(); + + if (config_.enable_serial_io) + { + serial_port_ = new Serial_Port(ser_port_name_.c_str(), alt_ser_port_name_.c_str(), ser_baud_rate_); + serial_port_->start(); + read_thread_ = std::thread(&ApInterface::readThread, this); + } +} + +ApInterface::~ApInterface() +{ + time_to_exit_ = true; + if (read_thread_.joinable()) + { + read_thread_.join(); + } + if (serial_port_ != nullptr) + { + serial_port_->stop(); + delete serial_port_; + serial_port_ = nullptr; + } +} + +std::optional ApInterface::popOutbound() { return outbound_queue_.tryPop(); } + +void ApInterface::pushInbound(MavlinkFrame frame) { inbound_queue_.push(std::move(frame)); } + +void ApInterface::clearOutboundQueue() { outbound_queue_.clear(); } + +void ApInterface::drainInboundCallback(const ros::TimerEvent& /*event*/) +{ + if (!warn_logged_ && !received_any_message_.load(std::memory_order_relaxed)) + { + const double elapsed = (ros::Time::now() - node_start_time_).toSec(); + if (elapsed > config_.no_message_warn_timeout_s) + { + ROS_WARN("No MAVLink messages received on /mavlink/from -- is the FC node running?"); + warn_logged_ = true; + } + } + + constexpr int kMaxDrainPerTick = 10; + for (int i = 0; i < kMaxDrainPerTick; ++i) + { + auto frame = inbound_queue_.tryPop(); + if (!frame) + { + break; + } + mavlink_message_t mavlink_msg; + if(!frameToMessage(*frame, mavlink_msg)) + { + ROS_ERROR("Could not convert mavlink frame to mavlink msg"); + } + if (serial_port_ != nullptr) + { + serial_port_->write_message(mavlink_msg); + } + } +} + +void ApInterface::readThread() +{ + reading_status_ = true; + while (!time_to_exit_) + { + readMessages(); + if (heartbeat_detected_) + { + current_detection_time_ = getTimeUsec(); + comm_link_ok_ = true; + heartbeat_detected_ = false; + } + if (getTimeUsec() - current_detection_time_ > 2000000) + { + comm_link_ok_ = false; + ROS_ERROR_THROTTLE( + 5, + "MavlinkQuicRelay: No heartbeat received from autopilot. Please check the connection details"); + } + } + + reading_status_ = false; + + return; +} + +void ApInterface::readMessages() +{ + bool success; // receive success flag + + mavlink_message_t mavlink_msg; + if (serial_port_ == nullptr) + { + return; + } + + success = serial_port_->read_message(mavlink_msg); + + if (success) + { + switch (mavlink_msg.msgid) + { + case MAVLINK_MSG_ID_HEARTBEAT: + ROS_INFO("HeartBeat Received %d", mavlink_msg.sysid); + heartbeat_detected_ = true; + break; + default: + break; + } + // send the mavlink frame to QUIC buffer + MavlinkFrame frame = toFrame(mavlink_msg); + outbound_queue_.push(std::move(frame)); + } +} + +uint64_t ApInterface::getTimeUsec() +{ + static struct timeval _time_stamp; + gettimeofday(&_time_stamp, NULL); + return _time_stamp.tv_sec * 1000000 + _time_stamp.tv_usec; +} + +void ApInterface::loadParameters() +{ + nh_.param("ser_port_name", ser_port_name_, "/dev/ttyACM0"); + nh_.param("alt_ser_port_name", alt_ser_port_name_, "/dev/ttyACM1"); + nh_.param("ser_baud_rate", ser_baud_rate_, 115200); +} + +MavlinkFrame ApInterface::toFrame(const mavlink_message_t& mavlink_msg) +{ + MavlinkFrame frame; + frame.msgid = mavlink_msg.msgid; + + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t len = mavlink_msg_to_send_buffer(buffer, &mavlink_msg); + + frame.raw_bytes.assign(buffer, buffer + len); + + return frame; +} + +bool ApInterface::frameToMessage(const MavlinkFrame& frame, mavlink_message_t& msg_out) +{ + mavlink_status_t status{}; + mavlink_message_t parsed_msg{}; + + for (uint8_t byte : frame.raw_bytes) + { + if (mavlink_parse_char(MAVLINK_COMM_0, byte, &parsed_msg, &status)) + { + msg_out = parsed_msg; + return true; + } + } + + return false; +} + +// --------------------------------------------------------------------------- +// toRawBytes — mavros_msgs::Mavlink → wire bytes +// --------------------------------------------------------------------------- + +// std::vector ApInterface::toRawBytes(const mavros_msgs::Mavlink& msg) +// { +// const uint8_t magic = static_cast(msg.magic); + +// if (magic == 0xFE) +// { +// // MAVLink v1: [magic][len][seq][sysid][compid][msgid(1)][payload][cksum(2)] +// const std::size_t total = 6 + msg.len + 2; +// std::vector raw; +// raw.reserve(total); + +// raw.push_back(magic); +// raw.push_back(static_cast(msg.len)); +// raw.push_back(static_cast(msg.seq)); +// raw.push_back(static_cast(msg.sysid)); +// raw.push_back(static_cast(msg.compid)); +// raw.push_back(static_cast(msg.msgid & 0xFF)); + +// // Unpack payload from payload64 (big-endian word packing used by mavros) +// std::size_t bytes_written = 0; +// for (uint64_t word : msg.payload64) +// { +// for (int shift = 56; shift >= 0 && bytes_written < msg.len; shift -= 8) +// { +// raw.push_back(static_cast((word >> shift) & 0xFF)); +// ++bytes_written; +// } +// } + +// const uint16_t cksum = static_cast(msg.checksum & 0xFFFF); +// raw.push_back(static_cast(cksum & 0xFF)); +// raw.push_back(static_cast((cksum >> 8) & 0xFF)); + +// return raw; +// } + +// // MAVLink v2 (0xFD): +// // [magic][len][incompat_flags][compat_flags][seq][sysid][compid] +// // [msgid_low][msgid_mid][msgid_high][payload(len)][cksum(2)] +// const std::size_t total = 10 + msg.len + 2; +// std::vector raw; +// raw.reserve(total); + +// raw.push_back(magic); +// raw.push_back(static_cast(msg.len)); +// raw.push_back(static_cast(msg.incompat_flags)); +// raw.push_back(static_cast(msg.compat_flags)); +// raw.push_back(static_cast(msg.seq)); +// raw.push_back(static_cast(msg.sysid)); +// raw.push_back(static_cast(msg.compid)); + +// const uint32_t msgid32 = static_cast(msg.msgid); +// raw.push_back(static_cast(msgid32 & 0xFF)); +// raw.push_back(static_cast((msgid32 >> 8) & 0xFF)); +// raw.push_back(static_cast((msgid32 >> 16) & 0xFF)); + +// // Unpack payload from payload64 (big-endian word packing used by mavros) +// std::size_t bytes_written = 0; +// for (uint64_t word : msg.payload64) +// { +// for (int shift = 56; shift >= 0 && bytes_written < msg.len; shift -= 8) +// { +// raw.push_back(static_cast((word >> shift) & 0xFF)); +// ++bytes_written; +// } +// } + +// const uint16_t cksum = static_cast(msg.checksum & 0xFFFF); +// raw.push_back(static_cast(cksum & 0xFF)); +// raw.push_back(static_cast((cksum >> 8) & 0xFF)); + +// return raw; +// } + +// // --------------------------------------------------------------------------- +// // fromRawBytes — wire bytes → mavros_msgs::Mavlink +// // --------------------------------------------------------------------------- + +// mavros_msgs::Mavlink ApInterface::fromRawBytes(const std::vector& raw) +// { +// mavros_msgs::Mavlink msg; + +// if (raw.empty()) +// { +// return msg; +// } + +// const uint8_t magic = raw[0]; +// msg.magic = magic; + +// if (magic == 0xFE) +// { +// // MAVLink v1 minimum: 6 header + payload + 2 checksum = at least 8 bytes +// if (raw.size() < 8) +// { +// ROS_WARN("fromRawBytes: MAVLink v1 frame too short (%zu bytes)", raw.size()); +// return msg; +// } + +// msg.len = raw[1]; +// msg.seq = raw[2]; +// msg.sysid = raw[3]; +// msg.compid = raw[4]; +// msg.msgid = raw[5]; + +// const std::size_t payload_start = 6; +// const std::size_t payload_end = payload_start + msg.len; + +// if (raw.size() < payload_end + 2) +// { +// ROS_WARN("fromRawBytes: MAVLink v1 frame truncated"); +// return msg; +// } + +// // Pack payload bytes into payload64 words (big-endian, pad last word) +// const uint8_t* p = raw.data() + payload_start; +// std::size_t remaining = msg.len; +// while (remaining > 0) +// { +// uint64_t word = 0; +// for (int shift = 56; shift >= 0 && remaining > 0; shift -= 8, --remaining) +// { +// word |= (static_cast(*p++) << shift); +// } +// msg.payload64.push_back(word); +// } + +// const uint8_t ckl = raw[payload_end]; +// const uint8_t ckh = raw[payload_end + 1]; +// msg.checksum = static_cast(ckl) | (static_cast(ckh) << 8); + +// return msg; +// } + +// // MAVLink v2 minimum: 10 header + payload + 2 checksum = at least 12 bytes +// if (raw.size() < 12) +// { +// ROS_WARN("fromRawBytes: MAVLink v2 frame too short (%zu bytes)", raw.size()); +// return msg; +// } + +// msg.len = raw[1]; +// msg.incompat_flags = raw[2]; +// msg.compat_flags = raw[3]; +// msg.seq = raw[4]; +// msg.sysid = raw[5]; +// msg.compid = raw[6]; +// msg.msgid = +// static_cast(raw[7]) | (static_cast(raw[8]) << 8) | (static_cast(raw[9]) << 16); + +// const std::size_t payload_start = 10; +// const std::size_t payload_end = payload_start + msg.len; + +// if (raw.size() < payload_end + 2) +// { +// ROS_WARN("fromRawBytes: MAVLink v2 frame truncated"); +// return msg; +// } + +// // Pack payload bytes into payload64 words (big-endian, pad last word) +// const uint8_t* p = raw.data() + payload_start; +// std::size_t remaining = msg.len; +// while (remaining > 0) +// { +// uint64_t word = 0; +// for (int shift = 56; shift >= 0 && remaining > 0; shift -= 8, --remaining) +// { +// word |= (static_cast(*p++) << shift); +// } +// msg.payload64.push_back(word); +// } + +// const uint8_t ckl = raw[payload_end]; +// const uint8_t ckh = raw[payload_end + 1]; +// msg.checksum = static_cast(ckl) | (static_cast(ckh) << 8); + +// return msg; +// } + +} // namespace mavlink_quic_relay diff --git a/jetson/mavlink_quic_relay/src/main.cpp b/jetson/mavlink_quic_relay/src/main.cpp index cfe15c9..be960d8 100644 --- a/jetson/mavlink_quic_relay/src/main.cpp +++ b/jetson/mavlink_quic_relay/src/main.cpp @@ -29,11 +29,10 @@ int main(int argc, char** argv) ros::AsyncSpinner spinner(4); spinner.start(); - auto relay_config = mavlink_quic_relay::loadRelayNodeConfig(nh); mavlink_quic_relay::RelayNode relay(nh, relay_config); - relay.start(); + relay.start(); ros::waitForShutdown(); relay.stop(); diff --git a/jetson/mavlink_quic_relay/src/reconnect_manager.cpp b/jetson/mavlink_quic_relay/src/reconnect_manager.cpp index 965b5e3..0f8f8cb 100644 --- a/jetson/mavlink_quic_relay/src/reconnect_manager.cpp +++ b/jetson/mavlink_quic_relay/src/reconnect_manager.cpp @@ -9,8 +9,8 @@ namespace mavlink_quic_relay { -ReconnectManager::ReconnectManager(QuicClient& client, RosInterface& ros_iface) - : quic_client_(client), ros_iface_(ros_iface), rng_(std::random_device{}()) +ReconnectManager::ReconnectManager(QuicClient& client, ApInterface& ap_iface) + : quic_client_(client), ap_iface_(ap_iface), rng_(std::random_device{}()) { } @@ -42,7 +42,7 @@ void ReconnectManager::onConnected() void ReconnectManager::onDisconnected() { state_.store(State::DISCONNECTED, std::memory_order_relaxed); - ros_iface_.clearOutboundQueue(); + ap_iface_.clearOutboundQueue(); should_reconnect_.store(true, std::memory_order_relaxed); reconnect_cv_.notify_one(); ROS_WARN("mavlink_quic_relay: QUIC connection lost, scheduling reconnect"); diff --git a/jetson/mavlink_quic_relay/src/relay_node.cpp b/jetson/mavlink_quic_relay/src/relay_node.cpp index a7f61e0..314caf5 100644 --- a/jetson/mavlink_quic_relay/src/relay_node.cpp +++ b/jetson/mavlink_quic_relay/src/relay_node.cpp @@ -11,14 +11,14 @@ namespace mavlink_quic_relay RelayNode::RelayNode(ros::NodeHandle& nh, RelayNodeConfig config) : config_(std::move(config)), nh_(nh) { classifier_ = std::make_unique(); - ros_interface_ = std::make_unique(nh_, config_.ros); + ap_interface_ = std::make_unique(nh_, config_.ros); quic_client_ = std::make_unique(config_.quic); quic_client_->setFrameReceivedCallback([this](std::vector frame) { onFrameReceived(std::move(frame)); }); quic_client_->setConnectionStateCallback([this](bool connected) { onConnectionStateChanged(connected); }); - reconnect_manager_ = std::make_unique(*quic_client_, *ros_interface_); + reconnect_manager_ = std::make_unique(*quic_client_, *ap_interface_); quic_client_->setAuthOkCallback([this] { onAuthOk(); }); quic_client_->setAuthFailCallback([this] { onAuthFailed(); }); @@ -76,7 +76,7 @@ void RelayNode::senderLoop() while (true) { - auto frame = ros_interface_->popOutbound(); + auto frame = ap_interface_->popOutbound(); if (!frame) { break; @@ -156,7 +156,7 @@ void RelayNode::onFrameReceived(std::vector frame) } } - ros_interface_->pushInbound(MavlinkFrame{msgid, std::move(frame)}); + ap_interface_->pushInbound(MavlinkFrame{msgid, std::move(frame)}); } RelayNodeConfig loadRelayNodeConfig(ros::NodeHandle& nh) @@ -184,9 +184,6 @@ RelayNodeConfig loadRelayNodeConfig(ros::NodeHandle& nh) nh.param("idle_timeout_ms", idle_ms, 60000); cfg.quic.idle_timeout_ms = static_cast(idle_ms); - nh.param("mavlink_from_topic", cfg.ros.mavlink_from_topic, "/mavlink/from"); - nh.param("mavlink_to_topic", cfg.ros.mavlink_to_topic, "/mavlink/to"); - int pq_size = 100; nh.param("priority_queue_size", pq_size, 100); cfg.priority_queue_size = static_cast(pq_size); diff --git a/jetson/mavlink_quic_relay/src/ros_interface.cpp b/jetson/mavlink_quic_relay/src/ros_interface.cpp deleted file mode 100644 index 59f5958..0000000 --- a/jetson/mavlink_quic_relay/src/ros_interface.cpp +++ /dev/null @@ -1,298 +0,0 @@ -#include "mavlink_quic_relay/ros_interface.h" - -#include -#include - -namespace mavlink_quic_relay -{ - -// --------------------------------------------------------------------------- -// BoundedQueue -// --------------------------------------------------------------------------- - -BoundedQueue::BoundedQueue(std::size_t max_size) : max_size_(max_size) {} - -void BoundedQueue::push(MavlinkFrame frame) -{ - std::lock_guard lock(mutex_); - if (queue_.size() >= max_size_) - { - queue_.pop(); - } - queue_.push(std::move(frame)); -} - -std::optional BoundedQueue::tryPop() -{ - std::lock_guard lock(mutex_); - if (queue_.empty()) - { - return std::nullopt; - } - MavlinkFrame frame = std::move(queue_.front()); - queue_.pop(); - return frame; -} - -std::size_t BoundedQueue::size() const -{ - std::lock_guard lock(mutex_); - return queue_.size(); -} - -void BoundedQueue::clear() -{ - std::lock_guard lock(mutex_); - while (!queue_.empty()) - { - queue_.pop(); - } -} - -// --------------------------------------------------------------------------- -// RosInterface -// --------------------------------------------------------------------------- - -RosInterface::RosInterface(ros::NodeHandle& nh, RosInterfaceConfig config) - : nh_(nh), - config_(std::move(config)), - outbound_queue_(config_.outbound_queue_max), - inbound_queue_(config_.inbound_queue_max) -{ - mavlink_sub_ = - nh_.subscribe(config_.mavlink_from_topic, 100, &RosInterface::mavlinkFromCallback, this); - - mavlink_pub_ = nh_.advertise(config_.mavlink_to_topic, 100); - - drain_timer_ = nh_.createTimer(config_.drain_period, &RosInterface::drainInboundCallback, this); - - node_start_time_ = ros::Time::now(); -} - -std::optional RosInterface::popOutbound() { return outbound_queue_.tryPop(); } - -void RosInterface::pushInbound(MavlinkFrame frame) { inbound_queue_.push(std::move(frame)); } - -void RosInterface::clearOutboundQueue() { outbound_queue_.clear(); } - -void RosInterface::mavlinkFromCallback(const mavros_msgs::Mavlink::ConstPtr& msg) -{ - received_any_message_.store(true, std::memory_order_relaxed); - - MavlinkFrame frame; - frame.msgid = static_cast(msg->msgid); - frame.raw_bytes = toRawBytes(*msg); - - ROS_DEBUG_STREAM("Received MAVLink msgid=" << msg->msgid); - - outbound_queue_.push(std::move(frame)); -} - -void RosInterface::drainInboundCallback(const ros::TimerEvent& /*event*/) -{ - if (!warn_logged_ && !received_any_message_.load(std::memory_order_relaxed)) - { - const double elapsed = (ros::Time::now() - node_start_time_).toSec(); - if (elapsed > config_.no_message_warn_timeout_s) - { - ROS_WARN("No MAVLink messages received on /mavlink/from -- is the FC node running?"); - warn_logged_ = true; - } - } - - constexpr int kMaxDrainPerTick = 10; - for (int i = 0; i < kMaxDrainPerTick; ++i) - { - auto frame = inbound_queue_.tryPop(); - if (!frame) - { - break; - } - mavlink_pub_.publish(fromRawBytes(frame->raw_bytes)); - } -} - -// --------------------------------------------------------------------------- -// toRawBytes — mavros_msgs::Mavlink → wire bytes -// --------------------------------------------------------------------------- - -std::vector RosInterface::toRawBytes(const mavros_msgs::Mavlink& msg) -{ - const uint8_t magic = static_cast(msg.magic); - - if (magic == 0xFE) - { - // MAVLink v1: [magic][len][seq][sysid][compid][msgid(1)][payload][cksum(2)] - const std::size_t total = 6 + msg.len + 2; - std::vector raw; - raw.reserve(total); - - raw.push_back(magic); - raw.push_back(static_cast(msg.len)); - raw.push_back(static_cast(msg.seq)); - raw.push_back(static_cast(msg.sysid)); - raw.push_back(static_cast(msg.compid)); - raw.push_back(static_cast(msg.msgid & 0xFF)); - - // Unpack payload from payload64 (big-endian word packing used by mavros) - std::size_t bytes_written = 0; - for (uint64_t word : msg.payload64) - { - for (int shift = 56; shift >= 0 && bytes_written < msg.len; shift -= 8) - { - raw.push_back(static_cast((word >> shift) & 0xFF)); - ++bytes_written; - } - } - - const uint16_t cksum = static_cast(msg.checksum & 0xFFFF); - raw.push_back(static_cast(cksum & 0xFF)); - raw.push_back(static_cast((cksum >> 8) & 0xFF)); - - return raw; - } - - // MAVLink v2 (0xFD): - // [magic][len][incompat_flags][compat_flags][seq][sysid][compid] - // [msgid_low][msgid_mid][msgid_high][payload(len)][cksum(2)] - const std::size_t total = 10 + msg.len + 2; - std::vector raw; - raw.reserve(total); - - raw.push_back(magic); - raw.push_back(static_cast(msg.len)); - raw.push_back(static_cast(msg.incompat_flags)); - raw.push_back(static_cast(msg.compat_flags)); - raw.push_back(static_cast(msg.seq)); - raw.push_back(static_cast(msg.sysid)); - raw.push_back(static_cast(msg.compid)); - - const uint32_t msgid32 = static_cast(msg.msgid); - raw.push_back(static_cast(msgid32 & 0xFF)); - raw.push_back(static_cast((msgid32 >> 8) & 0xFF)); - raw.push_back(static_cast((msgid32 >> 16) & 0xFF)); - - // Unpack payload from payload64 (big-endian word packing used by mavros) - std::size_t bytes_written = 0; - for (uint64_t word : msg.payload64) - { - for (int shift = 56; shift >= 0 && bytes_written < msg.len; shift -= 8) - { - raw.push_back(static_cast((word >> shift) & 0xFF)); - ++bytes_written; - } - } - - const uint16_t cksum = static_cast(msg.checksum & 0xFFFF); - raw.push_back(static_cast(cksum & 0xFF)); - raw.push_back(static_cast((cksum >> 8) & 0xFF)); - - return raw; -} - -// --------------------------------------------------------------------------- -// fromRawBytes — wire bytes → mavros_msgs::Mavlink -// --------------------------------------------------------------------------- - -mavros_msgs::Mavlink RosInterface::fromRawBytes(const std::vector& raw) -{ - mavros_msgs::Mavlink msg; - - if (raw.empty()) - { - return msg; - } - - const uint8_t magic = raw[0]; - msg.magic = magic; - - if (magic == 0xFE) - { - // MAVLink v1 minimum: 6 header + payload + 2 checksum = at least 8 bytes - if (raw.size() < 8) - { - ROS_WARN("fromRawBytes: MAVLink v1 frame too short (%zu bytes)", raw.size()); - return msg; - } - - msg.len = raw[1]; - msg.seq = raw[2]; - msg.sysid = raw[3]; - msg.compid = raw[4]; - msg.msgid = raw[5]; - - const std::size_t payload_start = 6; - const std::size_t payload_end = payload_start + msg.len; - - if (raw.size() < payload_end + 2) - { - ROS_WARN("fromRawBytes: MAVLink v1 frame truncated"); - return msg; - } - - // Pack payload bytes into payload64 words (big-endian, pad last word) - const uint8_t* p = raw.data() + payload_start; - std::size_t remaining = msg.len; - while (remaining > 0) - { - uint64_t word = 0; - for (int shift = 56; shift >= 0 && remaining > 0; shift -= 8, --remaining) - { - word |= (static_cast(*p++) << shift); - } - msg.payload64.push_back(word); - } - - const uint8_t ckl = raw[payload_end]; - const uint8_t ckh = raw[payload_end + 1]; - msg.checksum = static_cast(ckl) | (static_cast(ckh) << 8); - - return msg; - } - - // MAVLink v2 minimum: 10 header + payload + 2 checksum = at least 12 bytes - if (raw.size() < 12) - { - ROS_WARN("fromRawBytes: MAVLink v2 frame too short (%zu bytes)", raw.size()); - return msg; - } - - msg.len = raw[1]; - msg.incompat_flags = raw[2]; - msg.compat_flags = raw[3]; - msg.seq = raw[4]; - msg.sysid = raw[5]; - msg.compid = raw[6]; - msg.msgid = - static_cast(raw[7]) | (static_cast(raw[8]) << 8) | (static_cast(raw[9]) << 16); - - const std::size_t payload_start = 10; - const std::size_t payload_end = payload_start + msg.len; - - if (raw.size() < payload_end + 2) - { - ROS_WARN("fromRawBytes: MAVLink v2 frame truncated"); - return msg; - } - - // Pack payload bytes into payload64 words (big-endian, pad last word) - const uint8_t* p = raw.data() + payload_start; - std::size_t remaining = msg.len; - while (remaining > 0) - { - uint64_t word = 0; - for (int shift = 56; shift >= 0 && remaining > 0; shift -= 8, --remaining) - { - word |= (static_cast(*p++) << shift); - } - msg.payload64.push_back(word); - } - - const uint8_t ckl = raw[payload_end]; - const uint8_t ckh = raw[payload_end + 1]; - msg.checksum = static_cast(ckl) | (static_cast(ckh) << 8); - - return msg; -} - -} // namespace mavlink_quic_relay diff --git a/jetson/mavlink_quic_relay/src/serial_port.cpp b/jetson/mavlink_quic_relay/src/serial_port.cpp new file mode 100644 index 0000000..c09effb --- /dev/null +++ b/jetson/mavlink_quic_relay/src/serial_port.cpp @@ -0,0 +1,520 @@ +/** + * @file serial_port.cpp + * + * @brief UART communication with AP + * + * @author Kunal Shrivastava (kunal@suind.com) + * + */ + +// ------------------------------------------------------------------------------ +// Includes +// ------------------------------------------------------------------------------ +#include "mavlink_quic_relay/serial_port.h" + +static bool accept_unsigned_callback(const mavlink_status_t *status, uint32_t msgid) +{ + return false; +} +static mavlink_signing_streams_t signing_streams; +// ---------------------------------------------------------------------------------- +// Serial Port Manager Class +// ---------------------------------------------------------------------------------- + +// ------------------------------------------------------------------------------ +// Con/De structors +// ------------------------------------------------------------------------------ +Serial_Port::Serial_Port(const char* uart_name, const char* alt_uart_name, int baudrate) +{ + initialize_defaults(); + uart_name_ = uart_name; + uart_name_alt_ = alt_uart_name; + baudrate_ = baudrate; +} + +Serial_Port::Serial_Port() { initialize_defaults(); } + +Serial_Port::~Serial_Port() +{ + // destroy mutex + pthread_mutex_destroy(&lock); +} + +void Serial_Port::initialize_defaults() +{ + // Initialize attributes + debug_ = false; + fd_ = -1; + status_ = SERIAL_PORT_CLOSED; + + uart_name_ = (char *)"/dev/ttyACM0"; + uart_name_alt_ = (char *)"/dev/ttyACM1"; + baudrate_ = 115200; + type_ = "serial"; + + // Start mutex + int result = pthread_mutex_init(&lock, NULL); + if (result != 0) + { + printf("\n mutex init failed\n"); + throw 1; + } +} + +// ------------------------------------------------------------------------------ +// Read from Serial +// ------------------------------------------------------------------------------ +int Serial_Port::read_message(mavlink_message_t &message) +{ + uint8_t cp; + mavlink_status_t status; + uint8_t msgReceived = false; + + // -------------------------------------------------------------------------- + // READ FROM PORT + // -------------------------------------------------------------------------- + + // this function locks the port during read + int result = _read_port(cp); + + // -------------------------------------------------------------------------- + // PARSE MESSAGE + // -------------------------------------------------------------------------- + if (result > 0) + { + // the parsing + msgReceived = mavlink_parse_char(MAVLINK_COMM_0, cp, &message, &status); + + // check for dropped packets + if ((lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) && debug_) + { + printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count); + unsigned char v = cp; + fprintf(stderr, "%02x ", v); + } + lastStatus = status; + } + + // Couldn't read from port + else + { + // fprintf(stderr, "ERROR: Could not read from fd_ %d\n", fd_); + ROS_ERROR_THROTTLE(1.0, "Could not read from fd_ %d", fd_); + } + + // -------------------------------------------------------------------------- + // DEBUGGING REPORTS + // -------------------------------------------------------------------------- + if (msgReceived) + { + if (debug_) + { + // Report info + printf("Received message from serial with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, + message.compid); + + fprintf(stderr, "Received serial data: "); + unsigned int i; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + + // check message is write length + unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, &message); + + // message length error + if (messageLength > MAVLINK_MAX_PACKET_LEN) + { + fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n"); + } + + // print out the buffer + else + { + for (i = 0; i < messageLength; i++) + { + unsigned char v = buffer[i]; + fprintf(stderr, "%02x ", v); + } + fprintf(stderr, "\n"); + } + } + } + + // Done! + return msgReceived; +} + +// ------------------------------------------------------------------------------ +// Write to Serial +// ------------------------------------------------------------------------------ +int Serial_Port::write_message(const mavlink_message_t &message) +{ + char buf[2041]; + + // Translate message to buffer + unsigned len = mavlink_msg_to_send_buffer((uint8_t *)buf, &message); + + // Write buffer to serial port, locks port while writing + int bytesWritten = _write_port(buf, len); + + return bytesWritten; +} + +// ------------------------------------------------------------------------------ +// Open Serial Port +// ------------------------------------------------------------------------------ +/** + * throws EXIT_FAILURE if could not open the port + */ +void Serial_Port::open_serial() +{ + // -------------------------------------------------------------------------- + // OPEN PORT + // -------------------------------------------------------------------------- + ROS_INFO_THROTTLE(5, "OPEN PORT"); + const char *port_connected; + + fd_ = _open_port(uart_name_); + + // Check success + if (fd_ == -1) + { + ROS_ERROR_THROTTLE(5, "Failure, could not open serial port. Trying alternative port"); + ROS_INFO_THROTTLE(5, "Alternate Port Name: %s", uart_name_alt_); + fd_ = _open_port(uart_name_alt_); + if (fd_ == -1) + { + port_open_success_ = false; + return; + } + else + { + port_connected = uart_name_alt_; + port_open_success_ = true; + } + // throw EXIT_FAILURE; + } + else + { + port_connected = uart_name_; + port_open_success_ = true; + } + + // -------------------------------------------------------------------------- + // SETUP PORT + // -------------------------------------------------------------------------- + bool success = _setup_port(baudrate_, 8, 1, false, false); + + // -------------------------------------------------------------------------- + // CHECK STATUS + // -------------------------------------------------------------------------- + if (!success) + { + printf("failure, could not configure port.\n"); + throw EXIT_FAILURE; + } + if (fd_ <= 0) + { + printf("Connection attempt to port %s with %d baud, 8N1 failed, exiting.\n", port_connected, baudrate_); + throw EXIT_FAILURE; + } + + // -------------------------------------------------------------------------- + // CONNECTED! + // -------------------------------------------------------------------------- + printf("Connected to %s with %d baud, 8 data bits, no parity, 1 stop bit (8N1)\n", port_connected, baudrate_); + lastStatus.packet_rx_drop_count = 0; + + status_ = true; + + printf("\n"); + + return; +} + +// ------------------------------------------------------------------------------ +// Close Serial Port +// ------------------------------------------------------------------------------ +void Serial_Port::close_serial() +{ + printf("CLOSE PORT\n"); + + int result = close(fd_); + + if (result) + { + fprintf(stderr, "WARNING: Error on port close (%i)\n", result); + } + + status_ = false; + + printf("\n"); +} + +// ------------------------------------------------------------------------------ +// Convenience Functions +// ------------------------------------------------------------------------------ +void Serial_Port::start() +{ + open_serial(); + while (!port_open_success_) + { + ROS_ERROR_THROTTLE(5, "Failed to Open Port, please check if port details correct"); + open_serial(); + usleep(500000); + } + initSigning(); +} + +void Serial_Port::stop() { close_serial(); } + +void Serial_Port::initSigning() +{ + mavlink_chan_status_ = mavlink_get_channel_status(MAVLINK_COMM_1); + if (mavlink_chan_status_ == nullptr) + { + ROS_ERROR("Failed to load signing key - no status"); + return; + } + + memcpy(signing.secret_key, MAVLINK_KEY, 32); + signing.link_id = (uint8_t)MAVLINK_COMM_1; + // use a timestamp 1 minute past the last recorded + // timestamp. Combined with saving the key once every 30s this + // prevents a window for replay attacks + // signing.timestamp = key.timestamp + 60UL * 100UL * 1000UL; + // signing.timestamp = ros::Time::now().toNSec() + 60UL * 100UL * 1000UL; + signing.flags = MAVLINK_SIGNING_FLAG_SIGN_OUTGOING; + signing.accept_unsigned_callback = accept_unsigned_callback; + mavlink_chan_status_->signing = &signing; + mavlink_chan_status_->signing_streams = &signing_streams; +} + +// ------------------------------------------------------------------------------ +// Quit Handler +// ------------------------------------------------------------------------------ +void Serial_Port::handle_quit(int sig) +{ + try + { + stop(); + } + catch (int error) + { + fprintf(stderr, "Warning, could not stop serial port\n"); + } +} + +// ------------------------------------------------------------------------------ +// Helper Function - Open Serial Port File Descriptor +// ------------------------------------------------------------------------------ +// Where the actual port opening happens, returns file descriptor 'fd_' +int Serial_Port::_open_port(const char *port) +{ + // Open serial port + // O_RDWR - Read and write + // O_NOCTTY - Ignore special chars like CTRL-C + fd_ = open(port, O_RDWR | O_NOCTTY | O_NDELAY); + + // Check for Errors + if (fd_ == -1) + { + /* Could not open the port. */ + return (-1); + } + + // Finalize + else + { + fcntl(fd_, F_SETFL, 0); + } + + // Done! + return fd_; +} + +// ------------------------------------------------------------------------------ +// Helper Function - Setup Serial Port +// ------------------------------------------------------------------------------ +// Sets configuration, flags, and baud rate +bool Serial_Port::_setup_port(int baud, int data_bits, int stop_bits, bool parity, bool hardware_control) +{ + // Check file descriptor + if (!isatty(fd_)) + { + fprintf(stderr, "\nERROR: file descriptor %d is NOT a serial port\n", fd_); + return false; + } + + // Read file descritor configuration + struct termios config; + if (tcgetattr(fd_, &config) < 0) + { + fprintf(stderr, "\nERROR: could not read configuration of fd_ %d\n", fd_); + return false; + } + + // Input flags - Turn off input processing + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON | IXOFF | IXANY); + + // Output flags - Turn off output processing + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST); + +#ifdef OLCUC + config.c_oflag &= ~OLCUC; +#endif + +#ifdef ONOEOT + config.c_oflag &= ~ONOEOT; +#endif + + // No line processing: + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + // Turn off character processing + // clear current char size mask, no parity checking, + // no output processing, force 8 bit input + config.c_cflag |= (CLOCAL | CREAD); // ignore modem controls, + // enable reading + config.c_cflag &= ~(CSIZE | PARENB | PARODD); + config.c_cflag |= parity; + config.c_cflag |= CS8; + + // One input byte is enough to return from read() + // Inter-character timer off + config.c_cc[VMIN] = 0; + config.c_cc[VTIME] = 1; // was 0 + + // Get the current options for the port + ////struct termios options; + ////tcgetattr(fd_, &options); + + // Apply baudrate_ + switch (baud) + { + case 1200: + if (cfsetispeed(&config, B1200) < 0 || cfsetospeed(&config, B1200) < 0) + { + fprintf(stderr, "\nERROR: Could not set desired baud rate of %d Baud\n", baud); + return false; + } + break; + case 1800: + cfsetispeed(&config, B1800); + cfsetospeed(&config, B1800); + break; + case 9600: + cfsetispeed(&config, B9600); + cfsetospeed(&config, B9600); + break; + case 19200: + cfsetispeed(&config, B19200); + cfsetospeed(&config, B19200); + break; + case 38400: + if (cfsetispeed(&config, B38400) < 0 || cfsetospeed(&config, B38400) < 0) + { + fprintf(stderr, "\nERROR: Could not set desired baud rate of %d Baud\n", baud); + return false; + } + break; + case 57600: + if (cfsetispeed(&config, B57600) < 0 || cfsetospeed(&config, B57600) < 0) + { + fprintf(stderr, "\nERROR: Could not set desired baud rate of %d Baud\n", baud); + return false; + } + break; + case 115200: + if (cfsetispeed(&config, B115200) < 0 || cfsetospeed(&config, B115200) < 0) + { + fprintf(stderr, "\nERROR: Could not set desired baud rate of %d Baud\n", baud); + return false; + } + break; + + // These two non-standard (by the 70'ties ) rates are fully supported on + // current Debian and Mac OS versions (tested since 2010). + case 460800: + if (cfsetispeed(&config, B460800) < 0 || cfsetospeed(&config, B460800) < 0) + { + fprintf(stderr, "\nERROR: Could not set desired baud rate of %d Baud\n", baud); + return false; + } + break; + case 921600: + if (cfsetispeed(&config, B921600) < 0 || cfsetospeed(&config, B921600) < 0) + { + fprintf(stderr, "\nERROR: Could not set desired baud rate of %d Baud\n", baud); + return false; + } + break; + case 1500000: + if (cfsetispeed(&config, B1500000) < 0 || cfsetospeed(&config, B1500000) < 0) + { + fprintf(stderr, "\nERROR: Could not set desired baud rate of %d Baud\n", baud); + return false; + } + break; + default: + fprintf(stderr, "ERROR: Desired baud rate %d could not be set, aborting.\n", baud); + return false; + + break; + } + + // Finally, apply the configuration + if (tcsetattr(fd_, TCSAFLUSH, &config) < 0) + { + fprintf(stderr, "\nERROR: could not set configuration of fd_ %d\n", fd_); + return false; + } + + // Done! + return true; +} + +// ------------------------------------------------------------------------------ +// Read Port with Lock +// ------------------------------------------------------------------------------ +int Serial_Port::_read_port(uint8_t &cp) +{ + // Lock + pthread_mutex_lock(&lock); + + int result = read(fd_, &cp, 1); + + // Unlock + pthread_mutex_unlock(&lock); + + return result; +} + +// ------------------------------------------------------------------------------ +// Write Port with Lock +// ------------------------------------------------------------------------------ +int Serial_Port::_write_port(char *buf, unsigned len) +{ + // Lock + pthread_mutex_lock(&lock); + + // Write packet via serial link + const int bytesWritten = static_cast(write(fd_, buf, len)); + + // Wait until all data has been written + // tcdrain (fd_); + + // Unlock + pthread_mutex_unlock(&lock); + + return bytesWritten; +} diff --git a/jetson/mavlink_quic_relay/test/test_ap_interface_uart_setup.cpp b/jetson/mavlink_quic_relay/test/test_ap_interface_uart_setup.cpp new file mode 100644 index 0000000..b1cf86b --- /dev/null +++ b/jetson/mavlink_quic_relay/test/test_ap_interface_uart_setup.cpp @@ -0,0 +1,58 @@ +#include +#include + +#include + +#include "mavlink_quic_relay/ap_interface.h" + +using mavlink_quic_relay::ApInterface; +using mavlink_quic_relay::ApInterfaceConfig; + +TEST(ApInterfaceUartSetupTest, LoadsConfiguredUartParameters) +{ + ros::NodeHandle nh("~uart_setup_cfg"); + nh.setParam("ser_port_name", std::string("/dev/ttyUSB77")); + nh.setParam("alt_ser_port_name", std::string("/dev/ttyUSB78")); + nh.setParam("ser_baud_rate", 57600); + + ApInterfaceConfig cfg; + cfg.enable_serial_io = false; + ApInterface iface(nh, cfg); + + EXPECT_EQ(iface.serialPortName(), "/dev/ttyUSB77"); + EXPECT_EQ(iface.altSerialPortName(), "/dev/ttyUSB78"); + EXPECT_EQ(iface.serialBaudRate(), 57600); +} + +TEST(ApInterfaceUartSetupTest, UsesFallbackUartDefaults) +{ + ros::NodeHandle nh("~uart_setup_default"); + + ApInterfaceConfig cfg; + cfg.enable_serial_io = false; + ApInterface iface(nh, cfg); + + EXPECT_EQ(iface.serialPortName(), "/dev/ttyACM0"); + EXPECT_EQ(iface.altSerialPortName(), "/dev/ttyACM1"); + EXPECT_EQ(iface.serialBaudRate(), 115200); +} + +TEST(ApInterfaceUartSetupTest, ConstructAndDestroyWithoutSerialIo) +{ + { + ros::NodeHandle nh("~uart_setup_no_serial"); + ApInterfaceConfig cfg; + cfg.enable_serial_io = false; + ApInterface iface(nh, cfg); + EXPECT_EQ(iface.serialPortName(), "/dev/ttyACM0"); + } + SUCCEED(); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "test_ap_interface_uart_setup", ros::init_options::AnonymousName); + ros::Time::init(); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/jetson/mavlink_quic_relay/test/test_reconnect_manager.cpp b/jetson/mavlink_quic_relay/test/test_reconnect_manager.cpp index af2812e..193de38 100644 --- a/jetson/mavlink_quic_relay/test/test_reconnect_manager.cpp +++ b/jetson/mavlink_quic_relay/test/test_reconnect_manager.cpp @@ -19,13 +19,13 @@ #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" using mavlink_quic_relay::QuicClient; using mavlink_quic_relay::QuicClientConfig; using mavlink_quic_relay::ReconnectManager; -using mavlink_quic_relay::RosInterface; -using mavlink_quic_relay::RosInterfaceConfig; +using mavlink_quic_relay::ApInterface; +using mavlink_quic_relay::ApInterfaceConfig; using State = mavlink_quic_relay::ReconnectManager::State; class ReconnectManagerTest : public ::testing::Test @@ -34,13 +34,15 @@ class ReconnectManagerTest : public ::testing::Test void SetUp() override { nh_ = std::make_unique(); - ros_iface_ = std::make_unique(*nh_, RosInterfaceConfig{}); + ApInterfaceConfig cfg; + cfg.enable_serial_io = false; + ros_iface_ = std::make_unique(*nh_, cfg); quic_client_ = std::make_unique(QuicClientConfig{}); rm_ = std::make_unique(*quic_client_, *ros_iface_); } std::unique_ptr nh_; - std::unique_ptr ros_iface_; + std::unique_ptr ros_iface_; std::unique_ptr quic_client_; std::unique_ptr rm_; }; diff --git a/jetson/mavlink_quic_relay/test/test_ros_interface_framing.cpp b/jetson/mavlink_quic_relay/test/test_ros_interface_framing.cpp index ad8433d..c4ab497 100644 --- a/jetson/mavlink_quic_relay/test/test_ros_interface_framing.cpp +++ b/jetson/mavlink_quic_relay/test/test_ros_interface_framing.cpp @@ -1,10 +1,10 @@ // test_ros_interface_framing.cpp // // Unit tests for the toRawBytes and fromRawBytes conversion logic used by -// RosInterface. Those methods are private, so we duplicate their +// ApInterface. Those methods are private, so we duplicate their // implementations here as free functions and test those instead. // -// Duplicated from RosInterface for testing — keep in sync with ros_interface.cpp +// Duplicated from ApInterface for testing — keep in sync with ros_interface.cpp #include #include diff --git a/jetson/mavlink_quic_relay/test/test_thread_safe_queue.cpp b/jetson/mavlink_quic_relay/test/test_thread_safe_queue.cpp index b76e86f..863eca8 100644 --- a/jetson/mavlink_quic_relay/test/test_thread_safe_queue.cpp +++ b/jetson/mavlink_quic_relay/test/test_thread_safe_queue.cpp @@ -5,7 +5,7 @@ #include #include -#include "mavlink_quic_relay/ros_interface.h" +#include "mavlink_quic_relay/ap_interface.h" using mavlink_quic_relay::BoundedQueue; using mavlink_quic_relay::MavlinkFrame;