From b44f20d7052927b0e8faed20302af79ee714ebe2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 15 Jan 2016 10:48:43 -0800
Subject: [PATCH] mavlink: option to specify the UDP remote port

This enables using several mavlink instances for SITL.
---
 src/modules/mavlink/mavlink_main.cpp | 27 ++++++++++++++++++++-------
 src/modules/mavlink/mavlink_main.h   |  3 +++
 2 files changed, 23 insertions(+), 7 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 06bee091cf..b12db887d3 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -185,6 +185,7 @@ Mavlink::Mavlink() :
 	_socket_fd(-1),
 	_protocol(SERIAL),
 	_network_port(14556),
+	_remote_port(DEFAULT_REMOTE_PORT_UDP),
 	_rstatus {},
 	_message_buffer {},
 	_message_buffer_mutex {},
@@ -1056,7 +1057,7 @@ Mavlink::init_udp()
 	if (_mode != MAVLINK_MODE_ONBOARD) {
 		_src_addr.sin_family = AF_INET;
 		inet_aton("127.0.0.1", &_src_addr.sin_addr);
-		_src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP);
+		_src_addr.sin_port = htons(_remote_port);
 		set_client_source_initialized();
 	}
 
@@ -1064,7 +1065,7 @@ Mavlink::init_udp()
 	memset((char *)&_bcast_addr, 0, sizeof(_bcast_addr));
 	_bcast_addr.sin_family = AF_INET;
 	inet_aton("255.255.255.255", &_bcast_addr.sin_addr);
-	_bcast_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP);
+	_bcast_addr.sin_port = htons(_remote_port);
 
 #endif
 }
@@ -1521,7 +1522,7 @@ Mavlink::task_main(int argc, char *argv[])
 	char* eptr;
 	int temp_int_arg;
 
-	while ((ch = px4_getopt(argc, argv, "b:r:d:u:m:fpvwx", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "b:r:d:u:o:m:fpvwx", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'b':
 			_baudrate = strtoul(myoptarg, NULL, 10);
@@ -1547,7 +1548,7 @@ Mavlink::task_main(int argc, char *argv[])
 			_device_name = myoptarg;
 			set_protocol(SERIAL);
 			break;
-			
+
 		case 'u':
 			temp_int_arg = strtoul(myoptarg, &eptr, 10);
 			if ( *eptr == '\0' ) {
@@ -1559,6 +1560,18 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 			break;
 
+		case 'o':
+			temp_int_arg = strtoul(myoptarg, &eptr, 10);
+			if ( *eptr == '\0' ) {
+				warnx("set remote port %d", temp_int_arg);
+				_remote_port = temp_int_arg;
+				set_protocol(UDP);
+			} else {
+				warnx("invalid remote udp_port '%s'", myoptarg);
+				err_flag = true;
+			}
+			break;
+
 //		case 'e':
 //			mavlink_link_termination_allowed = true;
 //			break;
@@ -1868,7 +1881,7 @@ Mavlink::task_main(int argc, char *argv[])
 		hrt_abstime t = hrt_absolute_time();
 
 		update_rate_mult();
-		
+
 		_mission_manager->check_active_mission();
 
 		if (param_sub->update(&param_time, nullptr)) {
@@ -1880,7 +1893,7 @@ Mavlink::task_main(int argc, char *argv[])
 		if (_uart_fd >= 0 && _radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
 			/* request to configure radio and radio is present */
 			FILE *fs = fdopen(_uart_fd, "w");
-			
+
 			if (fs) {
 				/* switch to AT command mode */
 				usleep(1200000);
@@ -2320,7 +2333,7 @@ Mavlink::stream_command(int argc, char *argv[])
 
 static void usage()
 {
-	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-u udp_port] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
+	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-u network_port] [-o remote_port] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
 }
 
 int mavlink_main(int argc, char *argv[])
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 3febaacf5b..95d460e789 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -337,6 +337,8 @@ public:
 
 	unsigned short		get_network_port() { return _network_port; }
 
+	unsigned short		get_remote_port() { return _remote_port; }
+
 	int 			get_socket_fd () { return _socket_fd; };
 #ifdef __PX4_POSIX
 	struct sockaddr_in *	get_client_source_address() { return &_src_addr; }
@@ -441,6 +443,7 @@ private:
 	int _socket_fd;
 	Protocol	_protocol;
 	unsigned short _network_port;
+	unsigned short _remote_port;
 
 	struct telemetry_status_s	_rstatus;			///< receive status