From 4f37a9363a7e2ade1772a9d55f57729288ec3cb9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 28 Nov 2012 14:23:53 +1100
Subject: [PATCH] SITL: prevent the GPS pipe filling up and delaying GPS
 readings

---
 libraries/Desktop/support/sitl_gps.cpp | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/libraries/Desktop/support/sitl_gps.cpp b/libraries/Desktop/support/sitl_gps.cpp
index 756b3dce2a..b20008b742 100644
--- a/libraries/Desktop/support/sitl_gps.cpp
+++ b/libraries/Desktop/support/sitl_gps.cpp
@@ -15,6 +15,7 @@
 #include <SITL.h>
 #include <AP_GPS.h>
 #include <AP_GPS_UBLOX.h>
+#include <sys/ioctl.h>
 #include "desktop.h"
 #include "util.h"
 
@@ -46,6 +47,17 @@ static struct {
  */
 ssize_t sitl_gps_read(int fd, void *buf, size_t count)
 {
+#ifdef FIONREAD
+	// use FIONREAD to get exact value if possible
+	int num_ready;
+	while (ioctl(fd, FIONREAD, &num_ready) == 0 && num_ready > 256) {
+		// the pipe is filling up - drain it
+		uint8_t tmp[128];
+		if (read(fd, tmp, sizeof(tmp)) != sizeof(tmp)) {
+			break;
+		}
+	}
+#endif
 	return read(fd, buf, count);
 }