First attempt at dual mavlink streams

Tools/ardupilotwaf/qurt.py

@@ -27,7 +27,7 @@ def build(bld):
     bld(
         # build ap_host
         source=[STUB_SO, MAIN_CPP],
-        rule="%s -I%s %s %s %s -o %s" % (AARCH64_CXX, STUB_INC.abspath(),
+        rule="%s -I%s %s %s %s -lpthread -o %s" % (AARCH64_CXX, STUB_INC.abspath(),
                                           MAIN_CPP.abspath(), IFADDR_CPP.abspath(), STUB_SO.abspath(), AP_HOST.abspath()),
         target=[AP_HOST],
         group='dynamic_sources'

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,

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);
 

libraries/AP_HAL_QURT/UARTDriver.h

@@ -75,10 +75,11 @@ class QURT::UARTDriver_Console : public QURT::UARTDriver
 /*
   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 @@ class QURT::UARTDriver_MAVLinkUDP : public QURT::UARTDriver
 
 private:
     static void _mavlink_data_cb(const struct qurt_rpc_msg *msg, void *p);
+	uint8_t inst;
     uint32_t seq;
 };
 

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,18 +88,29 @@ 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);
-    }
-    expected_seq = msg->seq + 1;
+	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->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,44 @@ 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 +216,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);
-
-    struct sockaddr_in local {};
-    local.sin_addr.s_addr = INADDR_ANY;
-    local.sin_family = AF_INET;
-    local.sin_port = htons(MAVLINK_UDP_PORT_LOCAL);
-
-    if (bind(socket_fd, (struct sockaddr *)&local, sizeof(local)) == 0) {
-        printf("Bind localhost OK\n");
+    // 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 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(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,37 +282,44 @@ 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)) {
             printf("Invalid bytes_received %d\n", bytes_received);
             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");
         }
     }
 

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
 

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;
     }