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);