diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a12133c021..0c017c5477 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -698,6 +698,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } else { _is_usb_uart = true; + /* USB has no baudrate, but use a magic number for 'fast' */ + _baudrate = 2000000; } #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ef14562ccc..95e0fdb91a 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -288,6 +288,8 @@ public: float get_rate_mult(); + float get_baudrate() { return _baudrate; } + /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } bool get_has_received_messages() { return _received_messages; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5f36354191..29f299f918 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1757,7 +1757,7 @@ MavlinkReceiver::receive_thread(void *arg) uint8_t buf[1600]; #else /* the serial port buffers internally as well, we just need to fit a small chunk */ - uint8_t buf[32]; + uint8_t buf[64]; #endif mavlink_message_t msg; @@ -1799,8 +1799,13 @@ MavlinkReceiver::receive_thread(void *arg) 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); + /* + * to avoid reading very small chunks wait for data before reading + * this is designed to target ~30 bytes at a time + */ + const unsigned character_count = 20; + unsigned sleeptime = (1.0f / (_mavlink->get_baudrate() / 10)) * character_count * 1000000; + usleep(sleeptime); } } #ifdef __PX4_POSIX