diff --git a/libraries/SITL/SIM_Gazebo.cpp b/libraries/SITL/SIM_Gazebo.cpp index 43c1a44a17..4e58c2d36b 100644 --- a/libraries/SITL/SIM_Gazebo.cpp +++ b/libraries/SITL/SIM_Gazebo.cpp @@ -19,6 +19,7 @@ #include "SIM_Gazebo.h" #include +#include #include @@ -115,6 +116,26 @@ void Gazebo::recv_fdm(const struct sitl_input &input) */ } +void Gazebo::drain_sockets() +{ + const uint16_t buflen = 1024; + char buf[buflen]; + ssize_t received; + errno = 0; + do { + received = socket_in.recv(buf, buflen, 0); + if (received < 0) { + if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) { + fprintf(stderr, "error recv on socket in: %s \n", + strerror(errno)); + } + } else { + // fprintf(stderr, "received from control socket: %s\n", buf); + } + } while (received > 0); + +} + /* update the Gazebo simulation by one time step */ @@ -126,6 +147,7 @@ void Gazebo::update(const struct sitl_input &input) // update magnetic field update_mag_field_bf(); + drain_sockets(); } } // namespace SITL diff --git a/libraries/SITL/SIM_Gazebo.h b/libraries/SITL/SIM_Gazebo.h index 438f9b45d6..457939291c 100644 --- a/libraries/SITL/SIM_Gazebo.h +++ b/libraries/SITL/SIM_Gazebo.h @@ -60,6 +60,7 @@ private: void recv_fdm(const struct sitl_input &input); void send_servos(const struct sitl_input &input); + void drain_sockets(); double last_timestamp;