From 80e8f9cdd84d48c1bc09657441487ebc9278cd0a Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Wed, 30 May 2018 20:18:40 +1000
Subject: [PATCH] AP_HAL_SITL: read all available SITL RC input packets

MAVProxy runs on its own clock.  While our clock is stopped for whatever
reason MAVProxy will continue to send RC input.  This patch always gives
us the most recent RC input from MAVProxy.

Also add sanity check on packet received
---
 libraries/AP_HAL_SITL/SITL_State.cpp | 22 +++++++++++++++++++---
 libraries/AP_HAL_SITL/SITL_State.h   |  1 +
 2 files changed, 20 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index bc6d9186b4..3da1430679 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -229,13 +229,26 @@ int SITL_State::sim_fd(const char *name, const char *arg)
  */
 void SITL_State::_check_rc_input(void)
 {
-    ssize_t size;
+    uint32_t count = 0;
+    while (_read_rc_sitl_input()) {
+        count++;
+    }
+
+    if (count > 100) {
+        ::fprintf(stderr, "Read %u rc inputs\n", count);
+    }
+}
+
+bool SITL_State::_read_rc_sitl_input()
+{
     struct pwm_packet {
         uint16_t pwm[16];
     } pwm_pkt;
 
-    size = _sitl_rc_in.recv(&pwm_pkt, sizeof(pwm_pkt), 0);
+    const ssize_t size = _sitl_rc_in.recv(&pwm_pkt, sizeof(pwm_pkt), 0);
     switch (size) {
+    case -1:
+        return false;
     case 8*2:
     case 16*2: {
         // a packet giving the receiver PWM inputs
@@ -259,9 +272,12 @@ void SITL_State::_check_rc_input(void)
                 pwm_input[i] = pwm;
             }
         }
-        break;
+        return true;
     }
+    default:
+        fprintf(stderr, "Malformed SITL RC input (%li)", size);
     }
+    return false;
 }
 
 /*
diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h
index 20f6fb2065..f10a144980 100644
--- a/libraries/AP_HAL_SITL/SITL_State.h
+++ b/libraries/AP_HAL_SITL/SITL_State.h
@@ -133,6 +133,7 @@ private:
     void _update_airspeed(float airspeed);
     void _update_gps_instance(SITL::SITL::GPSType gps_type, const struct gps_data *d, uint8_t instance);
     void _check_rc_input(void);
+    bool _read_rc_sitl_input();
     void _fdm_input_local(void);
     void _output_to_flightgear(void);
     void _simulator_servos(struct sitl_input &input);