diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index b31a8b3dd3..7e39a977e3 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -92,6 +93,15 @@ static const struct { { "RPM1_TYPE", 10 }, }; +/* + get system timestamp in seconds + */ +static double timestamp_sec() +{ + struct timeval tval; + gettimeofday(&tval,NULL); + return tval.tv_sec + (tval.tv_usec*1.0e-6); +} FlightAxis::FlightAxis(const char *frame_str) : Aircraft(frame_str) @@ -258,7 +268,6 @@ char *FlightAxis::soap_request_end(uint32_t timeout_ms) return strdup(replybuf); } - void FlightAxis::exchange_data(const struct sitl_input &input) { if (!sock && @@ -355,9 +364,21 @@ void FlightAxis::exchange_data(const struct sitl_input &input) char *reply = nullptr; if (sock) { reply = soap_request_end(0); + if (reply == nullptr) { + sock_error_count++; + if (sock_error_count >= 10000 && timestamp_sec() - last_recv_sec > 1) { + printf("socket timeout\n"); + delete sock; + sock = nullptr; + sock_error_count = 0; + last_recv_sec = timestamp_sec(); + } + } } if (reply) { + sock_error_count = 0; + last_recv_sec = timestamp_sec(); double lastt_s = state.m_currentPhysicsTime_SEC; parse_reply(reply); double dt = state.m_currentPhysicsTime_SEC - lastt_s; diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h index b0e48aa7fa..4086b76870 100644 --- a/libraries/SITL/SIM_FlightAxis.h +++ b/libraries/SITL/SIM_FlightAxis.h @@ -188,6 +188,8 @@ private: SocketAPM *sock; char replybuf[10000]; pid_t socket_pid; + uint32_t sock_error_count; + double last_recv_sec; };