aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: use async SOAP requests for flightaxis

Browse Source 
this greatly improves performance for cygwin
This commit is contained in:
Andrew Tridgell 2020-12-17 14:02:06 +11:00
parent 8d6d5c6b49
commit a31ea88ff5
2 changed files with 65 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
libraries/SITL/SIM_FlightAxis.cpp
View File

@ -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"(<?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;

5
libraries/SITL/SIM_FlightAxis.h
View File

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