From 7be507948fb02a8c8a151097212e4448fa7e9cf3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 18 Dec 2012 18:12:05 +1100
Subject: [PATCH] SITL: added SITL_State::loop_hook()

this prevents us using so much CPU time, and ensures stdout is flushed
---
 .../AP_HAL_AVR_SITL/AP_HAL_AVR_SITL_Main.h    |  9 ++++--
 libraries/AP_HAL_AVR_SITL/SITL_State.cpp      | 32 +++++++++++++++++++
 libraries/AP_HAL_AVR_SITL/SITL_State.h        |  1 +
 libraries/AP_HAL_AVR_SITL/UARTDriver.h        |  4 ++-
 4 files changed, 42 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_AVR_SITL/AP_HAL_AVR_SITL_Main.h b/libraries/AP_HAL_AVR_SITL/AP_HAL_AVR_SITL_Main.h
index ad28f122ce..b92ff9958b 100644
--- a/libraries/AP_HAL_AVR_SITL/AP_HAL_AVR_SITL_Main.h
+++ b/libraries/AP_HAL_AVR_SITL/AP_HAL_AVR_SITL_Main.h
@@ -5,9 +5,12 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 #define AP_HAL_MAIN() extern "C" {\
     int main (int argc, char * const argv[]) {	\
-	hal.init(argc, argv);		\
-        setup();\
-        for(;;) loop();\
+	hal.init(argc, argv); \
+        setup(); \
+        for(;;) { \
+		loop(); \
+		AVR_SITL::SITL_State::loop_hook(); \
+	} \
         return 0;\
     }\
     }
diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
index 4f5f76d88d..f140373f2c 100644
--- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp
@@ -17,6 +17,7 @@
 #include <unistd.h>
 #include <stdlib.h>
 #include <errno.h>
+#include <sys/select.h>
 
 #include <AP_Param.h>
 
@@ -450,4 +451,35 @@ void SITL_State::init(int argc, char * const argv[])
 	_parse_command_line(argc, argv);
 }
 
+// wait for serial input, or 100usec
+void SITL_State::loop_hook(void)
+{
+    struct timeval tv;
+    fd_set fds;
+    int fd, max_fd = 0;
+
+    FD_ZERO(&fds);
+    fd = ((AVR_SITL::SITLUARTDriver*)hal.uartA)->_fd;
+    if (fd != -1) {
+        FD_SET(fd, &fds);
+        max_fd = max(fd, max_fd);
+    }
+    fd = ((AVR_SITL::SITLUARTDriver*)hal.uartB)->_fd;
+    if (fd != -1) {
+        FD_SET(fd, &fds);
+        max_fd = max(fd, max_fd);
+    }
+    fd = ((AVR_SITL::SITLUARTDriver*)hal.uartC)->_fd;
+    if (fd != -1) {
+        FD_SET(fd, &fds);
+        max_fd = max(fd, max_fd);
+    }
+    tv.tv_sec = 0;
+    tv.tv_usec = 100;
+    fflush(stdout);
+    fflush(stderr);
+    select(max_fd+1, &fds, NULL, NULL, &tv);
+}
+
+
 #endif
diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.h b/libraries/AP_HAL_AVR_SITL/SITL_State.h
index c20ec4e75b..0590ff9314 100644
--- a/libraries/AP_HAL_AVR_SITL/SITL_State.h
+++ b/libraries/AP_HAL_AVR_SITL/SITL_State.h
@@ -37,6 +37,7 @@ public:
     ssize_t gps_read(int fd, void *buf, size_t count);
     static uint16_t pwm_output[11];
     static uint16_t pwm_input[8];
+    static void loop_hook(void);
 
 private:
     void _parse_command_line(int argc, char * const argv[]);
diff --git a/libraries/AP_HAL_AVR_SITL/UARTDriver.h b/libraries/AP_HAL_AVR_SITL/UARTDriver.h
index d5b7a92fe7..822c01d0ba 100644
--- a/libraries/AP_HAL_AVR_SITL/UARTDriver.h
+++ b/libraries/AP_HAL_AVR_SITL/UARTDriver.h
@@ -56,11 +56,13 @@ public:
     /* Implementations of Print virtual methods */
     size_t write(uint8_t c);
 
+    // file descriptor, exposed so SITL_State::loop_hook() can use it
+	int _fd;
+
 private:
     uint8_t _portNumber;
 	bool _connected; // true if a client has connected
 	int _listen_fd;  // socket we are listening on
-	int _fd;         // data socket
 	int _serial_port;
 	static bool _console;
 	bool _nonblocking_writes;