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)) {
- free(req1);
- return nullptr;
+ if (sock) {
+ delete sock;
}
- sock.set_blocking(false);
+ sock = new SocketAPM(false);
+ if (!sock->connect(controller_ip, controller_port)) {
+ ::printf("connect failed\n");
+ delete sock;
+ sock = nullptr;
+ free(req1);
+ return 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);
- if (ret <= 0) {
- printf("No data\n");
+ return true;
+}
+
+char *FlightAxis::soap_request_end(uint32_t timeout_ms)
+{
+ if (!sock) {
return nullptr;
}
- char *p = strstr(reply, "Content-Length: ");
+ if (!sock->pollin(timeout_ms)) {
+ return nullptr;
+ }
+ sock->set_blocking(true);
+ ssize_t ret = sock->recv(replybuf, sizeof(replybuf)-1, 1000);
+ if (ret <= 0) {
+ return nullptr;
+ }
+ 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];
};