diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6fa5918fc5..f623a0d7b3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1543,11 +1543,6 @@ 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; @@ -1724,6 +1719,11 @@ Mavlink::task_main(int argc, char *argv[]) /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); + /* init socket if necessary */ + if (get_protocol() == UDP) { + init_udp(); + } + /* if the protocol is serial, we send the system version blindly */ if (get_protocol() == SERIAL) { send_autopilot_capabilites(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 24d0940477..08b24f339d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1761,10 +1761,17 @@ MavlinkReceiver::receive_thread(void *arg) #ifdef __PX4_POSIX struct sockaddr_in srcaddr; socklen_t addrlen = sizeof(srcaddr); + if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) { + // make sure mavlink app has booted before we start using the socket + while (!_mavlink->boot_complete()) { + usleep(100000); + } + fds[0].fd = _mavlink->get_socket_fd(); fds[0].events = POLLIN; } + #endif ssize_t nread = 0; @@ -1779,7 +1786,6 @@ MavlinkReceiver::receive_thread(void *arg) } #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); }