From a31ea88ff5b1255c751b1d370bdc7a38d9967be2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 17 Dec 2020 14:02:06 +1100 Subject: [PATCH] SITL: use async SOAP requests for flightaxis this greatly improves performance for cygwin --- libraries/SITL/SIM_FlightAxis.cpp | 89 ++++++++++++++++++++----------- libraries/SITL/SIM_FlightAxis.h | 5 +- 2 files changed, 63 insertions(+), 31 deletions(-) diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 4b31821d55..0e3dbb2d4e 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -145,7 +145,7 @@ void FlightAxis::parse_reply(const char *reply) /* make a SOAP request, returning body of reply */ -char *FlightAxis::soap_request(const char *action, const char *fmt, ...) +bool FlightAxis::soap_request_start(const char *action, const char *fmt, ...) { va_list ap; char *req1; @@ -154,15 +154,19 @@ char *FlightAxis::soap_request(const char *action, const char *fmt, ...) vasprintf(&req1, fmt, ap); va_end(ap); - //printf("%s\n", req1); - // open SOAP socket to FlightAxis - SocketAPM sock(false); - if (!sock.connect(controller_ip, controller_port)) { + if (sock) { + delete sock; + } + sock = new SocketAPM(false); + if (!sock->connect(controller_ip, controller_port)) { + ::printf("connect failed\n"); + delete sock; + sock = nullptr; free(req1); - return nullptr; + return false; } - sock.set_blocking(false); + sock->set_blocking(false); char *req; asprintf(&req, R"(POST / HTTP/1.1 @@ -174,18 +178,31 @@ Connection: Keep-Alive %s)", action, (unsigned)strlen(req1), req1); - sock.send(req, strlen(req)); + sock->send(req, strlen(req)); free(req1); free(req); - char reply[10000]; - memset(reply, 0, sizeof(reply)); - ssize_t ret = sock.recv(reply, sizeof(reply)-1, 1000); + return true; +} + +char *FlightAxis::soap_request_end(uint32_t timeout_ms) +{ + if (!sock) { + return nullptr; + } + if (!sock->pollin(timeout_ms)) { + return nullptr; + } + sock->set_blocking(true); + ssize_t ret = sock->recv(replybuf, sizeof(replybuf)-1, 1000); if (ret <= 0) { - printf("No data\n"); return nullptr; } - char *p = strstr(reply, "Content-Length: "); + replybuf[ret] = 0; + + char *p = strstr(replybuf, "Content-Length: "); if (p == nullptr) { + delete sock; + sock = nullptr; printf("No Content-Length\n"); return nullptr; } @@ -195,51 +212,59 @@ Connection: Keep-Alive char *body = strstr(p, "\r\n\r\n"); if (body == nullptr) { printf("No body\n"); + delete sock; + sock = nullptr; return nullptr; } body += 4; // get the rest of the body - int32_t expected_length = content_length + (body - reply); - if (expected_length >= (int32_t)sizeof(reply)) { + int32_t expected_length = content_length + (body - replybuf); + if (expected_length >= (int32_t)sizeof(replybuf)) { printf("Reply too large %i\n", expected_length); + delete sock; + sock = nullptr; return nullptr; } while (ret < expected_length) { - ssize_t ret2 = sock.recv(&reply[ret], sizeof(reply)-(1+ret), 100); + ssize_t ret2 = sock->recv(&replybuf[ret], sizeof(replybuf)-(1+ret), 1000); if (ret2 <= 0) { + delete sock; + sock = nullptr; return nullptr; } // nul terminate - reply[ret+ret2] = 0; + replybuf[ret+ret2] = 0; ret += ret2; } - return strdup(reply); + delete sock; + sock = nullptr; + return strdup(replybuf); } - void FlightAxis::exchange_data(const struct sitl_input &input) { - if (!controller_started || - is_zero(state.m_flightAxisControllerIsActive) || - !is_zero(state.m_resetButtonHasBeenPressed)) { + if (!sock && + (!controller_started || + is_zero(state.m_flightAxisControllerIsActive) || + !is_zero(state.m_resetButtonHasBeenPressed))) { printf("Starting controller at %s\n", controller_ip); // call a restore first. This allows us to connect after the aircraft is changed in RealFlight - char *reply = soap_request("RestoreOriginalControllerDevice", R"( + soap_request_start("RestoreOriginalControllerDevice", R"( 12 )"); - free(reply); - reply = soap_request("InjectUAVControllerInterface", R"( + soap_request_end(1000); + soap_request_start("InjectUAVControllerInterface", R"( 12 )"); - free(reply); + soap_request_end(1000); activation_frame_counter = frame_counter; controller_started = true; } @@ -271,8 +296,8 @@ void FlightAxis::exchange_data(const struct sitl_input &input) scaled_servos[1] = constrain_float(pitch_rate + 0.5, 0, 1); } - - char *reply = soap_request("ExchangeData", R"( + if (!sock) { + soap_request_start("ExchangeData", R"( @@ -307,6 +332,12 @@ void FlightAxis::exchange_data(const struct sitl_input &input) scaled_servos[9], scaled_servos[10], scaled_servos[11]); + } + + char *reply = nullptr; + if (sock) { + reply = soap_request_end(0); + } if (reply) { double lastt_s = state.m_currentPhysicsTime_SEC; @@ -347,14 +378,12 @@ void FlightAxis::update(const struct sitl_input &input) delta_time = average_frame_time_s - extrapolated_s; } if (delta_time <= 0) { - usleep(1000); return; } time_now_us += delta_time * 1.0e6; extrapolate_sensors(delta_time); update_position(); update_mag_field_bf(); - usleep(delta_time*1.0e6); extrapolated_s += delta_time; report_FPS(); return; diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h index 68bfdf7e09..7672d9f084 100644 --- a/libraries/SITL/SIM_FlightAxis.h +++ b/libraries/SITL/SIM_FlightAxis.h @@ -156,7 +156,8 @@ public: }; private: - char *soap_request(const char *action, const char *fmt, ...); + bool soap_request_start(const char *action, const char *fmt, ...); + char *soap_request_end(uint32_t timeout_ms); void exchange_data(const struct sitl_input &input); void parse_reply(const char *reply); @@ -182,6 +183,8 @@ private: const char *controller_ip = "127.0.0.1"; uint16_t controller_port = 18083; + SocketAPM *sock; + char replybuf[10000]; };