HAL_QURT: send direct to GCS

allows for ArduPilot MAVLink XML
ArduPilot · Jul 6, 2024 · 44726f4 · 44726f4
1 parent cc4859d
commit 44726f4
Showing 1 changed file with 19 additions and 46 deletions.
diff --git a/libraries/AP_HAL_QURT/ap_host/src/main.cpp b/libraries/AP_HAL_QURT/ap_host/src/main.cpp
@@ -13,12 +13,14 @@
 #include "protocol.h"
 
 volatile bool _running = false;
-bool enable_debug = false;
-bool enable_remote_debug = false;
+static bool enable_debug = false;
+static bool enable_remote_debug = false;
 
-int socket_fd;
-struct sockaddr_in local_addr;
-struct sockaddr_in remote_addr;
+static int socket_fd;
+static struct sockaddr_in remote_addr;
+
+// GCS IP
+const char *address = "192.168.2.15";
 
 static void shutdown_signal_handler(int signo)
 {
@@ -71,9 +73,8 @@ static void receive_callback(const uint8_t *data, uint32_t length_in_bytes) {
 			// printf("0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x\n",
 			// 		data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8], data[9], data[10], data[11], data[12]);
 			if (_running) {
-				int bytes_sent = sendto(socket_fd, &data[1], length_in_bytes - 1, MSG_CONFIRM,
-					(const struct sockaddr*) &remote_addr, sizeof(remote_addr));
-				if (bytes_sent > 0) {
+                int bytes_sent = send(socket_fd, &data[1], length_in_bytes - 1, MSG_CONFIRM);
+                if (bytes_sent > 0) {
 					// printf("Send %d bytes to GCS\n", bytes_sent);
 					// if (data[8] == 22) printf("PARAM_VALUE %u\n", num_params++);
 				} else {
@@ -89,22 +90,6 @@ static void receive_callback(const uint8_t *data, uint32_t length_in_bytes) {
 	}
 }
 
-static void send_test_message(void) {
-	struct qurt_test_msg msg;
-
-	msg.byte_field = 0x45;
-	msg.word16_field = 0x1034;
-	msg.word32_field = 0x456508;
-	msg.word64_field = 0x3490000012221267;
-	msg.float_field = 67.59008;
-	msg.double_field = -329.099823;
-
-	printf("Sending test message Length = %u\n", sizeof(msg));
-    if (slpi_link_send((const uint8_t*) &msg, sizeof(msg))) {
-        fprintf(stderr, "slpi_link_send_data failed\n");
-	}
-}
-
 int main() {
 	printf("About to call init\n");
 
@@ -138,43 +123,31 @@ int main() {
         printf("slpi_link_initialize succeeded\n");
     }
 
-	send_test_message();
-
-	//initialize socket and structure
+    //initialize socket and structure
     socket_fd = socket(AF_INET, SOCK_DGRAM, 0);
     if (socket_fd == -1) {
     	fprintf(stderr, "Could not create socket");
 		return -1;
     }
 
-    //assign values
-    local_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
-    local_addr.sin_family = AF_INET;
-    local_addr.sin_port = htons(14552);
+    inet_aton(address, &remote_addr.sin_addr);
+    remote_addr.sin_family = AF_INET;
+    remote_addr.sin_port = htons(14550);
 
-    if (bind(socket_fd, (struct sockaddr*) &local_addr, sizeof(local_addr)) == 0) {
-		printf("Bind succeeded!\n");
+    if (connect(socket_fd,(struct sockaddr *)&remote_addr,sizeof(remote_addr)) == 0) {
+        printf("connect succeeded!\n");
 	} else {
-		fprintf(stderr, "Bind failed!\n");
+        printf("connect to %s failed!\n", address);
 	}
 
-	// allow re-bind on the same port
-	int one = 1;
-	setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR,(char *)&one, sizeof(one));
-
-	remote_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
-	remote_addr.sin_family = AF_INET;
-	remote_addr.sin_port = htons(14559);
-
-	printf("Waiting for receive\n");
+    printf("Waiting for receive\n");
 
 	printf("Enter ctrl-c to exit\n");
 	_running = true;
 	struct qurt_mavlink_msg msg;
 	while (_running) {
-		uint32_t bytes_received = recvfrom(socket_fd, msg.mav_msg, MAVLINK_MAX_PAYLOAD_LEN,
-			MSG_WAITALL, NULL, NULL);
-		if (bytes_received < 0) {
+        uint32_t bytes_received = recv(socket_fd, msg.mav_msg, MAVLINK_MAX_PAYLOAD_LEN, 0);
+        if (bytes_received < 0) {
 			fprintf(stderr, "Received failed");
 	    } else {
 	    	// printf("Message received. %d bytes\n", bytes_received);