From afefa4d2a0abf51bc9766e070d99c7b324649de9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 22 Apr 2016 09:40:29 +0200 Subject: [PATCH] gps: use a max poll timeout of 50ms to check for orb msgs more often Before this, I measured a max time spent in poll of 197ms. By checking at least every 50ms we make sure to not lose any orb messages. --- src/drivers/gps/gps.cpp | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index ac6ffb2078..948b87aa37 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -68,6 +68,7 @@ #include #include #include +#include #include #include #include @@ -323,21 +324,25 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) { - /* check for new messages. Note that we assume poll_or_read is called with a higher frequency - * than we get new injection messages. - */ handleInjectDataTopic(); #ifndef __PX4_QURT /* For non QURT, use the usual polling. */ + //Poll only for the serial data. In the same thread we also need to handle orb messages, + //so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the + //two pollings use different underlying mechanisms (at least under posix), which makes this + //impossible. Instead we limit the maximum polling interval and regularly check for new orb + //messages. + //FIXME: add a unified poll() API + const int max_timeout = 50; + pollfd fds[1]; fds[0].fd = _serial_fd; fds[0].events = POLLIN; - /* Poll for new data, */ - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout)); if (ret > 0) { /* if we have new data from GPS, go handle it */ @@ -349,16 +354,15 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) * If more bytes are available, we'll go back to poll() again. */ usleep(GPS_WAIT_BEFORE_READ * 1000); - return ::read(_serial_fd, buf, buf_length); + ret = ::read(_serial_fd, buf, buf_length); } else { - return -1; + ret = -1; } - - } else { - return ret; } + return ret; + #else /* For QURT, just use read for now, since this doesn't block, we need to slow it down * just a bit. */