From 08bfeb3dc784958ee62d826bd7e9d5eed7e0ce2e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 6 Apr 2020 11:32:34 +0200 Subject: [PATCH] mavlink: fix potential busy loop upon USB disconnect When disconnecting USB poll returns successfully, but the read returns -1 with ENOTCONN. In addition this also ensures there's no busy loop when poll returns an error. MAVLink continues to work after reconnecting USB. --- src/modules/mavlink/mavlink_receiver.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6628640638..21f0e2ee79 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2809,10 +2809,16 @@ MavlinkReceiver::Run() updateParams(); } - if (poll(&fds[0], 1, timeout) > 0) { + int ret = poll(&fds[0], 1, timeout); + + if (ret > 0) { if (_mavlink->get_protocol() == Protocol::SERIAL) { /* non-blocking read. read may return negative values */ nread = ::read(fds[0].fd, buf, sizeof(buf)); + + if (nread == -1 && errno == ENOTCONN) { // Not connected (can happen for USB) + usleep(100000); + } } #if defined(MAVLINK_UDP) @@ -2895,6 +2901,9 @@ MavlinkReceiver::Run() } #endif // MAVLINK_UDP + + } else if (ret == -1) { + usleep(10000); } hrt_abstime t = hrt_absolute_time();