mavlink udp:

- added option to stream messages over udp
- still hardcoded stuff (port)

src/modules/mavlink/mavlink_main.cpp

@@ -163,6 +163,7 @@ Mavlink::Mavlink() :
 	mavlink_link_termination_allowed(false),
 	_subscribe_to_stream(nullptr),
 	_subscribe_to_stream_rate(0.0f),
+	_udp_initialised(false),
 	_flow_control_enabled(true),
 	_last_write_success_time(0),
 	_last_write_try_time(0),
@@ -173,6 +174,8 @@ Mavlink::Mavlink() :
 	_rate_tx(0.0f),
 	_rate_txerr(0.0f),
 	_rate_rx(0.0f),
+	_socket_fd(-1),
+	_protocol(SERIAL),
 	_rstatus {},
 	_message_buffer {},
 	_message_buffer_mutex {},
@@ -800,20 +803,21 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 
 	pthread_mutex_lock(&_send_mutex);
 
-	unsigned buf_free = get_free_tx_buf();
-
 	uint8_t payload_len = mavlink_message_lengths[msgid];
 	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 
 	_last_write_try_time = hrt_absolute_time();
 
-	/* check if there is space in the buffer, let it overflow else */
+	if (get_protocol() == SERIAL) {
-	if (buf_free < packet_len) {
+		/* check if there is space in the buffer, let it overflow else */
-		/* no enough space in buffer to send */
+		unsigned buf_free = get_free_tx_buf();
-		count_txerr();
+		if (buf_free < packet_len) {
-		count_txerrbytes(packet_len);
+			 /* no enough space in buffer to send */
-		pthread_mutex_unlock(&_send_mutex);
+			count_txerr();
-		return;
+			count_txerrbytes(packet_len);
+			pthread_mutex_unlock(&_send_mutex);
+			return;
+		}
 	}
 
 	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
@@ -839,11 +843,16 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
 
-#ifndef __PX4_POSIX
+//#ifndef __PX4_POSIX
 	/* send message to UART */
-	ssize_t ret = ::write(_uart_fd, buf, packet_len);
+	size_t ret = -1;
+	if (get_protocol() == SERIAL) {
+		ret = ::write(_uart_fd, buf, packet_len);
+	} else if (get_protocol() == UDP) {
+		ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
+	}
 
-	if (ret != (int) packet_len) {
+	if (ret != (size_t) packet_len) {
 		count_txerr();
 		count_txerrbytes(packet_len);
 
@@ -851,7 +860,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 		_last_write_success_time = _last_write_try_time;
 		count_txbytes(packet_len);
 	}
-#endif
+//#endif
 
 	pthread_mutex_unlock(&_send_mutex);
 }
@@ -908,6 +917,31 @@ Mavlink::resend_message(mavlink_message_t *msg)
 	pthread_mutex_unlock(&_send_mutex);
 }
 
+void
+Mavlink::init_udp()
+{
+	memset((char *)&_myaddr, 0, sizeof(_myaddr));
+	_myaddr.sin_family = AF_INET;
+	_myaddr.sin_addr.s_addr = htonl(INADDR_ANY);
+	_myaddr.sin_port = htons(14556);
+
+	if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
+		PX4_WARN("create socket failed\n");
+		return;
+	}
+
+	if (bind(_socket_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
+		PX4_WARN("bind failed\n");
+		return;
+	}
+
+	unsigned char inbuf[256];
+	socklen_t addrlen = sizeof(_src_addr);
+
+	// wait for client to connect to socket
+	recvfrom(_socket_fd,inbuf,sizeof(inbuf),0,(struct sockaddr *)&_src_addr,&addrlen);
+}
+
 void
 Mavlink::handle_message(const mavlink_message_t *msg)
 {
@@ -1323,6 +1357,9 @@ Mavlink::task_main(int argc, char *argv[])
 
 		case 'd':
 			_device_name = myoptarg;
+			if (strncmp(_device_name, "udp",3) == 0) {
+				set_protocol(UDP);
+			}
 			break;
 
 //		case 'e':
@@ -1392,6 +1429,11 @@ Mavlink::task_main(int argc, char *argv[])
 	/* flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
 
+	/* init socket if necessary */
+	if (get_protocol() == UDP) {
+		init_udp();
+	}
+
 #ifndef __PX4_POSIX
 	struct termios uart_config_original;
 

src/modules/mavlink/mavlink_main.h

@@ -46,6 +46,8 @@
 #include <nuttx/fs/fs.h>
 #else
 #include <drivers/device/device.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
 #endif
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
@@ -65,6 +67,12 @@
 #include "mavlink_parameters.h"
 #include "mavlink_ftp.h"
 
+enum Protocol {
+	SERIAL = 0,
+	UDP,
+	TCP,
+};
+
 #ifdef __PX4_NUTTX
 class Mavlink
 #else
@@ -190,6 +198,11 @@ public:
 	 */
 	void			set_manual_input_mode_generation(bool generation_enabled) { _generate_rc = generation_enabled; }
 
+	/**
+	 * Set communication protocol for this mavlink instance
+	 */
+	void 		set_protocol(Protocol p) {_protocol = p;};
+
 	/**
 	 * Get the manual input generation mode
 	 *
@@ -305,6 +318,8 @@ public:
 
 	unsigned		get_system_type() { return _system_type; }
 
+	Protocol 		get_protocol() { return _protocol; };
+
 protected:
 	Mavlink			*next;
 
@@ -364,6 +379,7 @@ private:
 
 	char 			*_subscribe_to_stream;
 	float			_subscribe_to_stream_rate;
+	bool 			_udp_initialised;
 
 	bool			_flow_control_enabled;
 	uint64_t		_last_write_success_time;
@@ -377,6 +393,12 @@ private:
 	float			_rate_txerr;
 	float			_rate_rx;
 
+	int _socket_fd;
+	struct sockaddr_in _myaddr;
+	struct sockaddr_in _src_addr;
+
+	Protocol _protocol;
+
 	struct telemetry_status_s	_rstatus;			///< receive status
 
 	struct mavlink_message_buffer {
@@ -439,6 +461,8 @@ private:
 	 */
 	void update_rate_mult();
 
+	void init_udp();
+
 #ifdef __PX4_NUTTX
 	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
 #else