From e4a526e3b2cfbb4fd52b33c58f44f1c6605f89ec Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Thu, 19 Dec 2019 15:14:12 +0100 Subject: [PATCH] Don't sleep in middle of mavlink message reception --- src/modules/mavlink/mavlink_receiver.cpp | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 199f6b082c..d71b645a5c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2717,18 +2717,8 @@ MavlinkReceiver::Run() if (poll(&fds[0], 1, timeout) > 0) { if (_mavlink->get_protocol() == Protocol::SERIAL) { - - /* - * to avoid reading very small chunks wait for data before reading - * this is designed to target one message, so >20 bytes at a time - */ - const unsigned character_count = 20; - /* non-blocking read. read may return negative values */ - if ((nread = ::read(fds[0].fd, buf, sizeof(buf))) < (ssize_t)character_count) { - const unsigned sleeptime = character_count * 1000000 / (_mavlink->get_baudrate() / 10); - px4_usleep(sleeptime); - } + nread = ::read(fds[0].fd, buf, sizeof(buf)); } #if defined(MAVLINK_UDP)