SITL: use async SOAP requests for flightaxis

this greatly improves performance for cygwin

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"(<?xml version='1.0' encoding='UTF-8'?>
+        soap_request_start("RestoreOriginalControllerDevice", R"(<?xml version='1.0' encoding='UTF-8'?>
 <soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>
 <soap:Body>
 <RestoreOriginalControllerDevice><a>1</a><b>2</b></RestoreOriginalControllerDevice>
 </soap:Body>
 </soap:Envelope>)");
-        free(reply);
-        reply = soap_request("InjectUAVControllerInterface", R"(<?xml version='1.0' encoding='UTF-8'?>
+        soap_request_end(1000);
+        soap_request_start("InjectUAVControllerInterface", R"(<?xml version='1.0' encoding='UTF-8'?>
 <soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>
 <soap:Body>
 <InjectUAVControllerInterface><a>1</a><b>2</b></InjectUAVControllerInterface>
 </soap:Body>
 </soap:Envelope>)");
-        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"(<?xml version='1.0' encoding='UTF-8'?><soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>
+    if (!sock) {
+        soap_request_start("ExchangeData", R"(<?xml version='1.0' encoding='UTF-8'?><soap:Envelope xmlns:soap='http://schemas.xmlsoap.org/soap/envelope/' xmlns:xsd='http://www.w3.org/2001/XMLSchema' xmlns:xsi='http://www.w3.org/2001/XMLSchema-instance'>
 <soap:Body>
 <ExchangeData>
 <pControlInputs>
@@ -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;

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];
 };