diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index a5468a86cc..f05b759d1c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -45,9 +45,9 @@ #ifdef __PX4_NUTTX #include #else -#include #include #include +#include #endif #include #include diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 90decb9e47..8c4b16b49c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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; }