From 741c42469c945c11bc40124e7e0ffff73d90aff5 Mon Sep 17 00:00:00 2001
From: chobits <chobits@itri.org.tw>
Date: Fri, 17 Aug 2018 14:40:43 +0800
Subject: [PATCH] mavlink: custom mavlink output destination

---
 .../src/main_flow/ArduPilot/dev_console.c        |  6 ++++++
 .../src/main_flow/ArduPilot/mavlink_wifi.c       | 16 ++++++++++++++--
 .../src/main_flow/ArduPilot/mavlink_wifi.h       |  2 ++
 3 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/app/dashcam/src/main_flow/ArduPilot/dev_console.c b/app/dashcam/src/main_flow/ArduPilot/dev_console.c
index 2874d76..10e6f32 100644
--- a/app/dashcam/src/main_flow/ArduPilot/dev_console.c
+++ b/app/dashcam/src/main_flow/ArduPilot/dev_console.c
@@ -966,6 +966,12 @@ static void check_wifi_txt(void)
         if (strcmp(name, "CHANNEL") == 0) {
             channel = atoi(value);
         }
+
+        if (strcmp(name, "MAVLINK_DST_IP") == 0) {
+            mavlink_set_dst_ip(inet_addr(value));
+        } else if (strcmp(name, "MAVLINK_DST_PORT") == 0) {
+            mavlink_set_dst_port(atoi(value));
+        }
     }
 
     WiFi_QueryAndSet(SET_BEACON_OFF, NULL, NULL);
diff --git a/app/dashcam/src/main_flow/ArduPilot/mavlink_wifi.c b/app/dashcam/src/main_flow/ArduPilot/mavlink_wifi.c
index 557d2a2..35e0931 100644
--- a/app/dashcam/src/main_flow/ArduPilot/mavlink_wifi.c
+++ b/app/dashcam/src/main_flow/ArduPilot/mavlink_wifi.c
@@ -47,6 +47,8 @@ static int sitl_sock = -1;
 static char stm32_id[40];
 static bool rc_ok;
 static bool vehicle_armed;
+static uint32_t mavlink_dst_ip = INADDR_BROADCAST;
+static uint16_t mavlink_dst_port = MAVLINK_DEST_PORT;
 
 #define QUEUE_SIZE 50*1024
 
@@ -237,8 +239,8 @@ static void send_mavlink_wifi(int fd, mavlink_message_t *msg)
         memset(&addr, 0x0, sizeof(addr));
 
         addr.sin_family = AF_INET;
-        addr.sin_addr.s_addr = htonl(INADDR_BROADCAST);
-        addr.sin_port = htons(MAVLINK_DEST_PORT);
+        addr.sin_addr.s_addr = mavlink_dst_ip;
+        addr.sin_port = htons(mavlink_dst_port);
         
         //console_printf("sending pkt of len %u\n", len);
         sendto(fd, buf, len, 0, (struct sockaddr*)&addr, sizeof(addr));
@@ -1474,6 +1476,16 @@ void mavlink_set_sitl(bool enable)
     console_printf("mavlink_sitl=%s\n", enable?"true":"false");
 }
 
+void mavlink_set_dst_ip(uint32_t ip)
+{
+    mavlink_dst_ip = ip;
+}
+
+void mavlink_set_dst_port(uint16_t port)
+{
+    mavlink_dst_port = port;
+}
+
 enum FlightCommand {
     flightControllerTakeOff = 1,
     flightControllerLand = 2,
diff --git a/app/dashcam/src/main_flow/ArduPilot/mavlink_wifi.h b/app/dashcam/src/main_flow/ArduPilot/mavlink_wifi.h
index 5d64f77..abf20c1 100644
--- a/app/dashcam/src/main_flow/ArduPilot/mavlink_wifi.h
+++ b/app/dashcam/src/main_flow/ArduPilot/mavlink_wifi.h
@@ -11,6 +11,8 @@ void mavlink_show_stats(void);
 void mavlink_set_debug(uint8_t debug_level);
 int mavlink_set_flight_response(int index, int value);
 void mavlink_set_sitl(bool enable);
+void mavlink_set_dst_ip(uint32_t ip);
+void mavlink_set_dst_port(uint16_t port);
 int mavlink_fc_send(mavlink_message_t *msg);
 bool toggle_recording(void);
 bool take_snapshot(void);