Merge pull request #2425 from tumbili/udp_receiver

Udp receiver

src/modules/mavlink/mavlink_main.cpp

@@ -435,7 +435,6 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 	}
 }
 
-#ifndef __PX4_POSIX
 int
 Mavlink::get_uart_fd(unsigned index)
 {
@@ -453,7 +452,6 @@ Mavlink::get_uart_fd()
 {
 	return _uart_fd;
 }
-#endif // __PX4_POSIX
 
 int
 Mavlink::get_instance_id()
@@ -810,6 +808,11 @@ Mavlink::get_free_tx_buf()
 
 #endif
 
+	// if we are using network sockets, return max lenght of one packet
+	if (get_protocol() == UDP || get_protocol() == TCP ) {
+		return  1500;
+	}
+
 	return buf_free;
 }
 

src/modules/mavlink/mavlink_main.h

@@ -45,9 +45,9 @@
 #ifdef __PX4_NUTTX
 #include <nuttx/fs/fs.h>
 #else
-#include <drivers/device/device.h>
 #include <sys/socket.h>
 #include <netinet/in.h>
+#include <drivers/device/device.h>
 #endif
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
@@ -123,11 +123,9 @@ public:
 
 	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
 
-#ifndef __PX4_QURT
 	static int		get_uart_fd(unsigned index);
 
 	int			get_uart_fd();
-#endif
 
 	/**
 	 * Get the MAVLink system id.
@@ -325,6 +323,8 @@ public:
 
 	unsigned short		get_network_port() { return _network_port; }
 
+	int 			get_socket_fd () { return _socket_fd; };
+
 protected:
 	Mavlink			*next;
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -1624,34 +1624,56 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 void *
 MavlinkReceiver::receive_thread(void *arg)
 {
-#ifndef __PX4_POSIX
-	int uart_fd = _mavlink->get_uart_fd();
 
 	const int timeout = 500;
 	uint8_t buf[32];
-
 	mavlink_message_t msg;
 
-	/* set thread name */
-	char thread_name[24];
-	sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
-	prctl(PR_SET_NAME, thread_name, getpid());
-
 	struct pollfd fds[1];
-	fds[0].fd = uart_fd;
-	fds[0].events = POLLIN;
 
+	int uart_fd = -1;
+
+	if (_mavlink->get_protocol() == SERIAL) {
+		uart_fd = _mavlink->get_uart_fd();
+#ifndef __PX4_POSIX
+		/* set thread name */
+		char thread_name[24];
+		sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
+		prctl(PR_SET_NAME, thread_name, getpid());
+#endif
+
+		fds[0].fd = uart_fd;
+		fds[0].events = POLLIN;
+	}
+#ifdef __PX4_POSIX
+	struct sockaddr_in srcaddr;
+	socklen_t addrlen = sizeof(srcaddr);
+	if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) {
+		fds[0].fd = _mavlink->get_socket_fd();
+		fds[0].events = POLLIN;
+	}
+#endif
 	ssize_t nread = 0;
 
 	while (!_mavlink->_task_should_exit) {
-		if (poll(fds, 1, timeout) > 0) {
-
-			/* non-blocking read. read may return negative values */
-			if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
-				/* to avoid reading very small chunks wait for data before reading */
-				usleep(1000);
+		if (poll(&fds[0], 1, timeout) > 0) {
+			if (_mavlink->get_protocol() == SERIAL) {
+				/* non-blocking read. read may return negative values */
+				if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
+					/* to avoid reading very small chunks wait for data before reading */
+					usleep(1000);
+				}
 			}
+#ifdef __PX4_POSIX
+			if (_mavlink->get_protocol() == UDP) {
 
+				if (fds[0].revents & POLLIN) {
+					nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen);
+				}
+			} else {
+				// could be TCP or other protocol
+			}
+#endif
 			/* if read failed, this loop won't execute */
 			for (ssize_t i = 0; i < nread; i++) {
 				if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) {
@@ -1667,7 +1689,6 @@ MavlinkReceiver::receive_thread(void *arg)
 			_mavlink->count_rxbytes(nread);
 		}
 	}
-#endif
 
 	return NULL;
 }