gps read optimization

src/drivers/gps/gps.cpp

@@ -399,9 +399,16 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 			 * We are here because poll says there is some data, so this
 			 * won't block even on a blocking device. But don't read immediately
 			 * by 1-2 bytes, wait for some more data to save expensive read() calls.
+			 * If we have all requested data available, read it without waiting.
 			 * If more bytes are available, we'll go back to poll() again.
 			 */
+			int err = 0, bytesAvailable = 0;
+			err = ioctl(_serial_fd, FIONREAD, (unsigned long)&bytesAvailable);
+
+			if ((err != 0) || (bytesAvailable < buf_length)) {
 				usleep(GPS_WAIT_BEFORE_READ * 1000);
+			}
+
 			ret = ::read(_serial_fd, buf, buf_length);
 
 		} else {