Enable passing udp port to mavlink module via start args.

Usage is: -d udp[:<port>] If no port is specified, default port is
set to 14556. If -d isn’t specified then default is serial.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/modules/mavlink/mavlink_main.cpp

@@ -176,6 +176,7 @@ Mavlink::Mavlink() :
 	_rate_rx(0.0f),
 	_socket_fd(-1),
 	_protocol(SERIAL),
+        _udp_port(14556),
 	_rstatus {},
 	_message_buffer {},
 	_message_buffer_mutex {},
@@ -923,10 +924,12 @@ Mavlink::resend_message(mavlink_message_t *msg)
 void
 Mavlink::init_udp()
 {
+	PX4_INFO("Setting up UDP w/port %d\n",_udp_port);
+
 	memset((char *)&_myaddr, 0, sizeof(_myaddr));
 	_myaddr.sin_family = AF_INET;
 	_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
-	_myaddr.sin_port = htons(14556);
+	_myaddr.sin_port = htons(_udp_port);
 
 	if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
 		PX4_WARN("create socket failed\n");
@@ -1361,7 +1364,26 @@ Mavlink::task_main(int argc, char *argv[])
 		case 'd':
 			_device_name = myoptarg;
 			if (strncmp(_device_name, "udp",3) == 0) {
+				bool err = false;
 				set_protocol(UDP);
+				const char* p = strchr(_device_name,':');
+				if (p != nullptr ) {
+					p++;
+					if (strlen(p) > 0) {
+						char* eptr;
+						int port = strtol(p,&eptr,0);
+						if (*eptr == 0) {
+							_udp_port = port;
+						}
+						else {
+							err = true;
+						}
+					}
+				}
+				if (err) {
+					warnx("invalid device-name '%s'", myoptarg);
+				}
+				err_flag |= err;
 			}
 			break;
 
@@ -1920,7 +1942,7 @@ Mavlink::stream_command(int argc, char *argv[])
 
 static void usage()
 {
-	warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
+	warnx("usage: mavlink {start|stop-all|stream} [-d udp[:<port-num>]] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]");
 }
 
 int mavlink_main(int argc, char *argv[])

src/modules/mavlink/mavlink_main.h

@@ -320,6 +320,8 @@ public:
 
 	Protocol 		get_protocol() { return _protocol; };
 
+	unsigned short		get_udp_port() { return _udp_port; }
+
 protected:
 	Mavlink			*next;
 
@@ -335,8 +337,8 @@ private:
 	bool			_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
 	bool			_forward_externalsp;	/**< Forward external setpoint messages to controllers directly if in offboard mode */
 	bool			_is_usb_uart;		/**< Port is USB */
-	bool        		_wait_to_transmit;  	/**< Wait to transmit until received messages. */
-	bool        		_received_messages;	/**< Whether we've received valid mavlink messages. */
+	bool			_wait_to_transmit;  	/**< Wait to transmit until received messages. */
+	bool			_received_messages;	/**< Whether we've received valid mavlink messages. */
 
 	unsigned		_main_loop_delay;	/**< mainloop delay, depends on data rate */
 
@@ -397,7 +399,8 @@ private:
 	struct sockaddr_in _myaddr;
 	struct sockaddr_in _src_addr;
 
-	Protocol _protocol;
+	Protocol	_protocol;
+	unsigned short _udp_port;
 
 	struct telemetry_status_s	_rstatus;			///< receive status