From 9ef76de0c9bef18a15464af397307caaf2519aa9 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Tue, 30 Jul 2024 09:33:08 -0700 Subject: [PATCH] AP_HAL_QURT: Added support for dual mavlink streams --- libraries/AP_HAL_QURT/HAL_QURT_Class.cpp | 5 +- libraries/AP_HAL_QURT/UARTDriver.cpp | 7 +- libraries/AP_HAL_QURT/UARTDriver.h | 4 +- libraries/AP_HAL_QURT/ap_host/src/main.cpp | 165 ++++++++++++++----- libraries/AP_HAL_QURT/ap_host/src/protocol.h | 5 +- libraries/AP_HAL_QURT/replace.cpp | 18 +- 6 files changed, 151 insertions(+), 53 deletions(-) diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp index 60904b7e39..0200b9d27d 100644 --- a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp @@ -45,7 +45,8 @@ static void crash_error_handler(void) using namespace QURT; static UARTDriver_Console consoleDriver; -static UARTDriver_MAVLinkUDP serial0Driver; +static UARTDriver_MAVLinkUDP serial0Driver(0); +static UARTDriver_MAVLinkUDP serial1Driver(1); static UARTDriver_Local serial3Driver(QURT_UART_GPS); static UARTDriver_Local serial4Driver(QURT_UART_RCIN); @@ -64,7 +65,7 @@ bool qurt_ran_overtime; HAL_QURT::HAL_QURT() : AP_HAL::HAL( &serial0Driver, - nullptr, + &serial1Driver, nullptr, &serial3Driver, &serial4Driver, diff --git a/libraries/AP_HAL_QURT/UARTDriver.cpp b/libraries/AP_HAL_QURT/UARTDriver.cpp index d4cfe0ff3c..f1a056df10 100644 --- a/libraries/AP_HAL_QURT/UARTDriver.cpp +++ b/libraries/AP_HAL_QURT/UARTDriver.cpp @@ -143,11 +143,11 @@ void UARTDriver_Console::printf(const char *fmt, ...) methods for UARTDriver_MAVLinkUDP */ typedef void (*mavlink_data_callback_t)(const struct qurt_rpc_msg *msg, void* p); -extern void register_mavlink_data_callback(mavlink_data_callback_t func, void *p); +extern void register_mavlink_data_callback(uint8_t instance, mavlink_data_callback_t func, void *p); -UARTDriver_MAVLinkUDP::UARTDriver_MAVLinkUDP(void) +UARTDriver_MAVLinkUDP::UARTDriver_MAVLinkUDP(uint8_t instance) : inst(instance) { - register_mavlink_data_callback(_mavlink_data_cb, (void *) this); + register_mavlink_data_callback(instance, _mavlink_data_cb, (void *) this); } void UARTDriver_MAVLinkUDP::_mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p) @@ -182,6 +182,7 @@ bool UARTDriver_MAVLinkUDP::_write_pending_bytes(void) return false; } msg.msg_id = QURT_MSG_ID_MAVLINK_MSG; + msg.inst = inst; msg.seq = seq++; msg.data_length = _writebuf.read(msg.data, n); diff --git a/libraries/AP_HAL_QURT/UARTDriver.h b/libraries/AP_HAL_QURT/UARTDriver.h index c1692808c2..d27ef2d6b7 100644 --- a/libraries/AP_HAL_QURT/UARTDriver.h +++ b/libraries/AP_HAL_QURT/UARTDriver.h @@ -75,10 +75,11 @@ public: /* subclass for MAVLink UDP communications */ + class QURT::UARTDriver_MAVLinkUDP : public QURT::UARTDriver { public: - UARTDriver_MAVLinkUDP(void); + UARTDriver_MAVLinkUDP(uint8_t instance); bool _write_pending_bytes(void) override; @@ -93,6 +94,7 @@ public: private: static void _mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p); + uint8_t inst; uint32_t seq; }; diff --git a/libraries/AP_HAL_QURT/ap_host/src/main.cpp b/libraries/AP_HAL_QURT/ap_host/src/main.cpp index 7fa3f2d837..135c281195 100644 --- a/libraries/AP_HAL_QURT/ap_host/src/main.cpp +++ b/libraries/AP_HAL_QURT/ap_host/src/main.cpp @@ -23,16 +23,25 @@ volatile bool _running = false; static bool enable_debug = false; static bool enable_remote_debug = false; -static int socket_fd; -static bool connected; -static struct sockaddr_in remote_addr; +static int gcs_socket_fd; +static int obd_socket_fd; +static bool gcs_connected; +static bool obd_connected; +static struct sockaddr_in gcs_addr; +static struct sockaddr_in obd_addr; #define SO_NAME "ArduPilot.so" // setup for mavlink to localhost #define MAVLINK_UDP_LOCALHOST 1 -#define MAVLINK_UDP_PORT_LOCAL 14558 -#define MAVLINK_UDP_PORT_REMOTE 14559 + +// Ports for onboard stream +#define MAVLINK_OBD_UDP_PORT_LOCAL 14556 +#define MAVLINK_OBD_UDP_PORT_REMOTE 14557 + +// Ports for external GCS stream +#define MAVLINK_GCS_UDP_PORT_LOCAL 14558 +#define MAVLINK_GCS_UDP_PORT_REMOTE 14559 // directory for logs, parameters, terrain etc @@ -63,7 +72,7 @@ static void shutdown_signal_handler(int signo) static void slpi_init(void); static uint32_t num_params = 0; -static uint32_t expected_seq = 0; +static uint32_t expected_seq[MAX_MAVLINK_INSTANCES] = {0, 0}; static void receive_callback(const uint8_t *data, uint32_t length_in_bytes) { @@ -79,17 +88,28 @@ static void receive_callback(const uint8_t *data, uint32_t length_in_bytes) printf("Invalid lengths %d %d\n", msg->data_length, length_in_bytes); return; } - if (msg->seq != expected_seq) { - printf("Invalid seq %u %u\n", msg->seq, expected_seq); + if (msg->inst < MAX_MAVLINK_INSTANCES) { + if (msg->seq != expected_seq[msg->inst]) { + printf("Invalid seq %u %u\n", msg->seq, expected_seq[msg->inst]); + } + } else { + printf("Invalid instance %u\n", msg->inst); } - expected_seq = msg->seq + 1; + expected_seq[msg->inst] = msg->seq + 1; switch (msg->msg_id) { case QURT_MSG_ID_MAVLINK_MSG: { if (_running) { - const auto bytes_sent = sendto(socket_fd, msg->data, msg->data_length, 0, (struct sockaddr *)&remote_addr, sizeof(remote_addr)); - if (bytes_sent <= 0) { - fprintf(stderr, "Send to GCS failed\n"); + if (msg->inst == 0) { + const auto bytes_sent = sendto(gcs_socket_fd, msg->data, msg->data_length, 0, (struct sockaddr *)&gcs_addr, sizeof(gcs_addr)); + if (bytes_sent <= 0) { + fprintf(stderr, "Send to GCS failed\n"); + } + } else if (msg->inst == 1) { + const auto bytes_sent = sendto(obd_socket_fd, msg->data, msg->data_length, 0, (struct sockaddr *)&obd_addr, sizeof(obd_addr)); + if (bytes_sent <= 0) { + fprintf(stderr, "Send to onboard failed\n"); + } } } break; @@ -129,6 +149,45 @@ static void setup_directores(void) } } +void *obd_recv_thread(void *) +{ + + uint32_t next_seq = 0; + + printf("Waiting for OBD receive\n"); + + while (_running) { + struct qurt_rpc_msg msg {}; + struct sockaddr_in from; + socklen_t fromlen = sizeof(from); + uint32_t bytes_received = recvfrom(obd_socket_fd, msg.data, sizeof(msg.data), 0, + (struct sockaddr*)&from, &fromlen); + if (bytes_received > 0 && !obd_connected) { + obd_addr = from; + obd_connected = true; + printf("Connnected to OBD addr %s\n", inet_ntoa(from.sin_addr)); + } + if (bytes_received < 0) { + fprintf(stderr, "OBD receive failed"); + continue; + } + if (bytes_received > sizeof(msg.data)) { + printf("Invalid bytes_received %d\n", bytes_received); + continue; + } + msg.msg_id = QURT_MSG_ID_MAVLINK_MSG; + msg.inst = 1; + msg.data_length = bytes_received; + msg.seq = next_seq++; + // printf("Message received. %d bytes\n", bytes_received); + if (slpi_link_send((const uint8_t*) &msg, bytes_received + QURT_RPC_MSG_HEADER_LEN)) { + fprintf(stderr, "slpi_link_send_data failed for instance 1\n"); + } + } + + return NULL; +} + int main() { printf("Starting up\n"); @@ -158,36 +217,57 @@ int main() slpi_init(); - //initialize socket and structure - socket_fd = socket(AF_INET, SOCK_DGRAM, 0); - if (socket_fd == -1) { - fprintf(stderr, "Could not create socket"); + //initialize sockets and structures + gcs_socket_fd = socket(AF_INET, SOCK_DGRAM, 0); + if (gcs_socket_fd == -1) { + fprintf(stderr, "Could not create GCS socket"); + return -1; + } + obd_socket_fd = socket(AF_INET, SOCK_DGRAM, 0); + if (obd_socket_fd == -1) { + fprintf(stderr, "Could not create OBD socket"); return -1; } #if MAVLINK_UDP_LOCALHOST - // send to mavlink router on localhost - remote_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); - remote_addr.sin_family = AF_INET; - remote_addr.sin_port = htons(MAVLINK_UDP_PORT_REMOTE); + // send to mavlink router on localhost for GCS + gcs_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + gcs_addr.sin_family = AF_INET; + gcs_addr.sin_port = htons(MAVLINK_GCS_UDP_PORT_REMOTE); - struct sockaddr_in local {}; - local.sin_addr.s_addr = INADDR_ANY; - local.sin_family = AF_INET; - local.sin_port = htons(MAVLINK_UDP_PORT_LOCAL); + struct sockaddr_in gcs_local {}; + gcs_local.sin_addr.s_addr = INADDR_ANY; + gcs_local.sin_family = AF_INET; + gcs_local.sin_port = htons(MAVLINK_GCS_UDP_PORT_LOCAL); - if (bind(socket_fd, (struct sockaddr *)&local, sizeof(local)) == 0) { - printf("Bind localhost OK\n"); + if (bind(gcs_socket_fd, (struct sockaddr *)&gcs_local, sizeof(gcs_local)) == 0) { + printf("Bind localhost GCS socket OK\n"); } else { - printf("Bind failed: %s", strerror(errno)); + printf("Bind localhost GCS socket failed: %s", strerror(errno)); + } + + // send to mavlink router on localhost for onboard stream + obd_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + obd_addr.sin_family = AF_INET; + obd_addr.sin_port = htons(MAVLINK_OBD_UDP_PORT_REMOTE); + + struct sockaddr_in obd_local {}; + obd_local.sin_addr.s_addr = INADDR_ANY; + obd_local.sin_family = AF_INET; + obd_local.sin_port = htons(MAVLINK_OBD_UDP_PORT_LOCAL); + + if (bind(obd_socket_fd, (struct sockaddr *)&obd_local, sizeof(obd_local)) == 0) { + printf("Bind localhost OBD socket OK\n"); + } else { + printf("Bind localhost OBD socket failed: %s", strerror(errno)); } #else // broadcast directly to the local network broadcast address const char *bcast_address = get_ipv4_broadcast(); printf("Broadcast address=%s\n", bcast_address); - inet_aton(bcast_address, &remote_addr.sin_addr); - remote_addr.sin_family = AF_INET; - remote_addr.sin_port = htons(UDP_OUT_PORT); + inet_aton(bcast_address, &gcs_addr.sin_addr); + gcs_addr.sin_family = AF_INET; + gcs_addr.sin_port = htons(UDP_OUT_PORT); int one = 1; setsockopt(socket_fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one)); @@ -203,25 +283,31 @@ int main() } #endif - printf("Waiting for receive\n"); - printf("Enter ctrl-c to exit\n"); _running = true; + + pthread_t obd_recv_thread_id; + pthread_attr_t obd_recv_thread_attr; + pthread_attr_init(&obd_recv_thread_attr); + pthread_create(&obd_recv_thread_id, &obd_recv_thread_attr, obd_recv_thread, NULL); + uint32_t next_seq = 0; + printf("Waiting for GCS receive\n"); + while (_running) { struct qurt_rpc_msg msg {}; struct sockaddr_in from; socklen_t fromlen = sizeof(from); - uint32_t bytes_received = recvfrom(socket_fd, msg.data, sizeof(msg.data), 0, + uint32_t bytes_received = recvfrom(gcs_socket_fd, msg.data, sizeof(msg.data), 0, (struct sockaddr*)&from, &fromlen); - if (bytes_received > 0 && !connected) { - remote_addr = from; - connected = true; - printf("Connnected to %s\n", inet_ntoa(from.sin_addr)); + if (bytes_received > 0 && !gcs_connected) { + gcs_addr = from; + gcs_connected = true; + printf("Connnected to GCS at %s\n", inet_ntoa(from.sin_addr)); } if (bytes_received < 0) { - fprintf(stderr, "Received failed"); + fprintf(stderr, "GCS receive failed"); continue; } if (bytes_received > sizeof(msg.data)) { @@ -229,11 +315,12 @@ int main() continue; } msg.msg_id = QURT_MSG_ID_MAVLINK_MSG; + msg.inst = 0; msg.data_length = bytes_received; msg.seq = next_seq++; // printf("Message received. %d bytes\n", bytes_received); if (slpi_link_send((const uint8_t*) &msg, bytes_received + QURT_RPC_MSG_HEADER_LEN)) { - fprintf(stderr, "slpi_link_send_data failed\n"); + fprintf(stderr, "slpi_link_send_data failed for instance 0\n"); } } diff --git a/libraries/AP_HAL_QURT/ap_host/src/protocol.h b/libraries/AP_HAL_QURT/ap_host/src/protocol.h index 3b10ba3bf1..8e8782e192 100644 --- a/libraries/AP_HAL_QURT/ap_host/src/protocol.h +++ b/libraries/AP_HAL_QURT/ap_host/src/protocol.h @@ -6,12 +6,15 @@ #define QURT_MSG_ID_MAVLINK_MSG 1 #define QURT_MSG_ID_REBOOT 2 +#define MAX_MAVLINK_INSTANCES 2 + struct __attribute__((__packed__)) qurt_rpc_msg { uint8_t msg_id; + uint8_t inst; uint16_t data_length; uint32_t seq; uint8_t data[300]; }; -#define QURT_RPC_MSG_HEADER_LEN 7 +#define QURT_RPC_MSG_HEADER_LEN 8 diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp index 9da9f6428a..6f331f6a1f 100644 --- a/libraries/AP_HAL_QURT/replace.cpp +++ b/libraries/AP_HAL_QURT/replace.cpp @@ -149,14 +149,18 @@ int slpi_link_client_init(void) typedef void (*mavlink_data_callback_t)(const struct qurt_rpc_msg *msg, void* p); -static mavlink_data_callback_t mav_cb; -static void *mav_cb_ptr; +static mavlink_data_callback_t mav_cb[MAX_MAVLINK_INSTANCES]; +static void *mav_cb_ptr[MAX_MAVLINK_INSTANCES]; static uint32_t expected_seq; -void register_mavlink_data_callback(mavlink_data_callback_t func, void *p) +void register_mavlink_data_callback(uint8_t instance, mavlink_data_callback_t func, void *p) { - mav_cb = func; - mav_cb_ptr = p; + if (instance < MAX_MAVLINK_INSTANCES) { + mav_cb[instance] = func; + mav_cb_ptr[instance] = p; + } else { + HAP_PRINTF("Error: Invalid mavlink instance %u", instance); + } } int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes) @@ -175,8 +179,8 @@ int slpi_link_client_receive(const uint8_t *data, int data_len_in_bytes) switch (msg->msg_id) { case QURT_MSG_ID_MAVLINK_MSG: { - if (mav_cb) { - mav_cb(msg, mav_cb_ptr); + if ((msg->inst < MAX_MAVLINK_INSTANCES) && (mav_cb[msg->inst])) { + mav_cb[msg->inst](msg, mav_cb_ptr[msg->inst]); } break; }