mirror of https://github.com/ArduPilot/ardupilot
SITL: first version of FlightAxis SITL support
This commit is contained in:
Andrew Tridgell
parent
76e3ae190a
commit
7a7bd0607f
|
|
@ -133,7 +133,9 @@ void Aircraft::update_position(void)
|
|||
time_now_us += frame_time_us;
|
||||
}
|
||||
last_time_us = time_now_us;
|
||||
sync_frame_time();
|
||||
if (use_time_sync) {
|
||||
sync_frame_time();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -125,6 +125,7 @@ protected:
|
|||
uint8_t instance;
|
||||
const char *autotest_dir;
|
||||
const char *frame;
|
||||
bool use_time_sync = true;
|
||||
|
||||
bool on_ground(const Vector3f &pos) const;
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,315 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
simulator connector for FlightAxis
|
||||
*/
|
||||
|
||||
#include "SIM_FlightAxis.h"
|
||||
|
||||
#include <arpa/inet.h>
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#define FLIGHTAXIS_SERVER_IP "192.168.2.48"
|
||||
#define FLIGHTAXIS_SERVER_PORT 18083
|
||||
|
||||
namespace SITL {
|
||||
|
||||
// the asprintf() calls are not worth checking for SITL
|
||||
#pragma GCC diagnostic ignored "-Wunused-result"
|
||||
|
||||
FlightAxis::FlightAxis(const char *home_str, const char *frame_str) :
|
||||
Aircraft(home_str, frame_str)
|
||||
{
|
||||
use_time_sync = false;
|
||||
rate_hz = 250 / target_speedup;
|
||||
heli_demix = strstr(frame_str, "helidemix") != NULL;
|
||||
rev4_servos = strstr(frame_str, "rev4") != NULL;
|
||||
}
|
||||
|
||||
/*
|
||||
extremely primitive SOAP parser that assumes the format used by FlightAxis
|
||||
*/
|
||||
void FlightAxis::parse_reply(const char *reply)
|
||||
{
|
||||
const char *reply0 = reply;
|
||||
for (uint16_t i=0; i<num_keys; i++) {
|
||||
const char *p = strstr(reply, keytable[i].key);
|
||||
if (p == nullptr) {
|
||||
p = strstr(reply0, keytable[i].key);
|
||||
}
|
||||
if (p == nullptr) {
|
||||
printf("Failed to find key %s\n", keytable[i].key);
|
||||
break;
|
||||
}
|
||||
p += strlen(keytable[i].key) + 1;
|
||||
keytable[i].ref = atof(p);
|
||||
// this assumes key order
|
||||
reply = p;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
make a SOAP request, returning body of reply
|
||||
*/
|
||||
char *FlightAxis::soap_request(const char *action, const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
char *req1;
|
||||
|
||||
va_start(ap, fmt);
|
||||
vasprintf(&req1, fmt, ap);
|
||||
va_end(ap);
|
||||
|
||||
//printf("%s\n", req1);
|
||||
|
||||
// open SOAP socket to FlightAxis
|
||||
SocketAPM sock(false);
|
||||
if (!sock.connect(FLIGHTAXIS_SERVER_IP, FLIGHTAXIS_SERVER_PORT)) {
|
||||
free(req1);
|
||||
return nullptr;
|
||||
}
|
||||
sock.set_blocking(false);
|
||||
|
||||
char *req;
|
||||
asprintf(&req, R"(POST / HTTP/1.1
|
||||
soapaction: '%s'
|
||||
content-length: %u
|
||||
content-type: text/xml;charset='UTF-8'
|
||||
Connection: Keep-Alive
|
||||
|
||||
%s)",
|
||||
action,
|
||||
(unsigned)strlen(req1), req1);
|
||||
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 nullptr;
|
||||
}
|
||||
char *p = strstr(reply, "Content-Length: ");
|
||||
if (p == nullptr) {
|
||||
printf("No Content-Length\n");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
// get the content length
|
||||
uint32_t content_length = strtoul(p+16, NULL, 10);
|
||||
char *body = strstr(p, "\r\n\r\n");
|
||||
if (body == nullptr) {
|
||||
printf("No body\n");
|
||||
return nullptr;
|
||||
}
|
||||
body += 4;
|
||||
|
||||
// get the rest of the body
|
||||
uint32_t expected_length = content_length + (body - reply);
|
||||
if (expected_length >= sizeof(reply)) {
|
||||
printf("Reply too large %u\n", expected_length);
|
||||
return nullptr;
|
||||
}
|
||||
while (ret < expected_length) {
|
||||
ssize_t ret2 = sock.recv(&reply[ret], sizeof(reply)-(1+ret), 100);
|
||||
if (ret2 <= 0) {
|
||||
return nullptr;
|
||||
}
|
||||
ret += ret2;
|
||||
}
|
||||
return strdup(reply);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void FlightAxis::exchange_data(const struct sitl_input &input)
|
||||
{
|
||||
if (!controller_started) {
|
||||
printf("Starting controller\n");
|
||||
// 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: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: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);
|
||||
activation_frame_counter = frame_counter;
|
||||
controller_started = true;
|
||||
}
|
||||
|
||||
float scaled_servos[8];
|
||||
for (uint8_t i=0; i<8; i++) {
|
||||
scaled_servos[i] = (input.servos[i] - 1000) / 1000.0f;
|
||||
}
|
||||
|
||||
if (rev4_servos) {
|
||||
// swap first 4 and last 4 servos, for quadplane testing
|
||||
float saved[4];
|
||||
memcpy(saved, &scaled_servos[0], sizeof(saved));
|
||||
memcpy(&scaled_servos[0], &scaled_servos[4], sizeof(saved));
|
||||
memcpy(&scaled_servos[4], saved, sizeof(saved));
|
||||
}
|
||||
|
||||
if (heli_demix) {
|
||||
// FlightAxis expects "roll/pitch/collective/yaw" input
|
||||
float swash1 = scaled_servos[0];
|
||||
float swash2 = scaled_servos[1];
|
||||
float swash3 = scaled_servos[2];
|
||||
|
||||
float roll_rate = swash1 - swash2;
|
||||
float pitch_rate = -((swash1+swash2) / 2.0f - swash3);
|
||||
|
||||
scaled_servos[0] = constrain_float(roll_rate + 0.5, 0, 1);
|
||||
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'>
|
||||
<soap:Body>
|
||||
<ExchangeData>
|
||||
<pControlInputs>
|
||||
<m-selectedChannels>255</m-selectedChannels>
|
||||
<m-channelValues-0to1>
|
||||
<item>%.4f</item>
|
||||
<item>%.4f</item>
|
||||
<item>%.4f</item>
|
||||
<item>%.4f</item>
|
||||
<item>%.4f</item>
|
||||
<item>%.4f</item>
|
||||
<item>%.4f</item>
|
||||
<item>%.4f</item>
|
||||
</m-channelValues-0to1>
|
||||
</pControlInputs>
|
||||
</ExchangeData>
|
||||
</soap:Body>
|
||||
</soap:Envelope>)",
|
||||
scaled_servos[0],
|
||||
scaled_servos[1],
|
||||
scaled_servos[2],
|
||||
scaled_servos[3],
|
||||
scaled_servos[4],
|
||||
scaled_servos[5],
|
||||
scaled_servos[6],
|
||||
scaled_servos[7]);
|
||||
|
||||
if (reply) {
|
||||
parse_reply(reply);
|
||||
free(reply);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
update the FlightAxis simulation by one time step
|
||||
*/
|
||||
void FlightAxis::update(const struct sitl_input &input)
|
||||
{
|
||||
Vector3f last_velocity_ef = velocity_ef;
|
||||
|
||||
exchange_data(input);
|
||||
|
||||
float dt_seconds = state.m_currentPhysicsTime_SEC - last_time_s;
|
||||
if (dt_seconds <= 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (initial_time_s <= 0) {
|
||||
dt_seconds = 0.001f;
|
||||
initial_time_s = state.m_currentPhysicsTime_SEC - dt_seconds;
|
||||
}
|
||||
|
||||
Quaternion quat(state.m_orientationQuaternion_W,
|
||||
state.m_orientationQuaternion_Z,
|
||||
-state.m_orientationQuaternion_Y,
|
||||
-state.m_orientationQuaternion_Z);
|
||||
quat.rotation_matrix(dcm);
|
||||
dcm.from_euler(radians(state.m_roll_DEG),
|
||||
radians(state.m_inclination_DEG),
|
||||
-radians(state.m_azimuth_DEG));
|
||||
Quaternion quat2;
|
||||
quat2.from_rotation_matrix(dcm);
|
||||
gyro = Vector3f(radians(constrain_float(state.m_rollRate_DEGpSEC, -2000, 2000)),
|
||||
radians(constrain_float(state.m_pitchRate_DEGpSEC, -2000, 2000)),
|
||||
-radians(constrain_float(state.m_yawRate_DEGpSEC, -2000, 2000))) * target_speedup;
|
||||
velocity_ef = Vector3f(state.m_velocityWorldU_MPS,
|
||||
state.m_velocityWorldV_MPS,
|
||||
state.m_velocityWorldW_MPS);
|
||||
position = Vector3f(state.m_aircraftPositionY_MTR,
|
||||
state.m_aircraftPositionX_MTR,
|
||||
-state.m_altitudeAGL_MTR);
|
||||
|
||||
// offset based on first position to account for offset in RF world
|
||||
if (position_offset.is_zero()) {
|
||||
position_offset = position;
|
||||
}
|
||||
position -= position_offset;
|
||||
|
||||
// the accel values given in the state are very strange. Calculate
|
||||
// it from delta-velocity instead, although this does introduce
|
||||
// noise
|
||||
Vector3f accel_ef = (velocity_ef - last_velocity_ef) / dt_seconds;
|
||||
accel_ef.z -= GRAVITY_MSS;
|
||||
accel_body = dcm.transposed() * accel_ef;
|
||||
accel_body.x = constrain_float(accel_body.x, -16, 16);
|
||||
accel_body.y = constrain_float(accel_body.y, -16, 16);
|
||||
accel_body.z = constrain_float(accel_body.z, -16, 16);
|
||||
|
||||
airspeed = state.m_airspeed_MPS;
|
||||
|
||||
battery_voltage = state.m_batteryVoltage_VOLTS;
|
||||
battery_current = state.m_batteryCurrentDraw_AMPS;
|
||||
rpm1 = state.m_propRPM;
|
||||
rpm2 = state.m_heliMainRotorRPM;
|
||||
|
||||
update_position();
|
||||
time_now_us = (state.m_currentPhysicsTime_SEC - initial_time_s)*1.0e6;
|
||||
|
||||
if (frame_counter++ % 1000 == 0) {
|
||||
if (last_frame_count_s != 0) {
|
||||
printf("%.2f FPS\n",
|
||||
1000 / (state.m_currentPhysicsTime_SEC - last_frame_count_s));
|
||||
printf("(%.3f %.3f %.3f %.3f) (%.3f %.3f %.3f %.3f)\n",
|
||||
quat.q1, quat.q2, quat.q3, quat.q4,
|
||||
quat2.q1, quat2.q2, quat2.q3, quat2.q4);
|
||||
} else {
|
||||
printf("Initial position %f %f %f\n", position.x, position.y, position.z);
|
||||
}
|
||||
last_frame_count_s = state.m_currentPhysicsTime_SEC;
|
||||
}
|
||||
|
||||
last_time_s = state.m_currentPhysicsTime_SEC;
|
||||
}
|
||||
|
||||
} // namespace SITL
|
||||
|
|
@ -0,0 +1,161 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
simulator connection for ardupilot version of FlightAxis
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/utility/Socket.h>
|
||||
|
||||
#include "SIM_Aircraft.h"
|
||||
|
||||
namespace SITL {
|
||||
|
||||
/*
|
||||
a FlightAxis simulator
|
||||
*/
|
||||
class FlightAxis : public Aircraft {
|
||||
public:
|
||||
FlightAxis(const char *home_str, const char *frame_str);
|
||||
|
||||
/* update model by one time step */
|
||||
void update(const struct sitl_input &input);
|
||||
|
||||
/* static object creator */
|
||||
static Aircraft *create(const char *home_str, const char *frame_str) {
|
||||
return new FlightAxis(home_str, frame_str);
|
||||
}
|
||||
|
||||
struct state {
|
||||
double m_airspeed_MPS;
|
||||
double m_altitudeASL_MTR;
|
||||
double m_altitudeAGL_MTR;
|
||||
double m_groundspeed_MPS;
|
||||
double m_pitchRate_DEGpSEC;
|
||||
double m_rollRate_DEGpSEC;
|
||||
double m_yawRate_DEGpSEC;
|
||||
double m_azimuth_DEG;
|
||||
double m_inclination_DEG;
|
||||
double m_roll_DEG;
|
||||
double m_aircraftPositionX_MTR;
|
||||
double m_aircraftPositionY_MTR;
|
||||
double m_velocityWorldU_MPS;
|
||||
double m_velocityWorldV_MPS;
|
||||
double m_velocityWorldW_MPS;
|
||||
double m_velocityBodyU_MPS;
|
||||
double m_velocityBodyV_MPS;
|
||||
double m_velocityBodyW_MPS;
|
||||
double m_accelerationWorldAX_MPS2;
|
||||
double m_accelerationWorldAY_MPS2;
|
||||
double m_accelerationWorldAZ_MPS2;
|
||||
double m_accelerationBodyAX_MPS2;
|
||||
double m_accelerationBodyAY_MPS2;
|
||||
double m_accelerationBodyAZ_MPS2;
|
||||
double m_windX_MPS;
|
||||
double m_windY_MPS;
|
||||
double m_windZ_MPS;
|
||||
double m_propRPM;
|
||||
double m_heliMainRotorRPM;
|
||||
double m_batteryVoltage_VOLTS;
|
||||
double m_batteryCurrentDraw_AMPS;
|
||||
double m_batteryRemainingCapacity_MAH;
|
||||
double m_fuelRemaining_OZ;
|
||||
double m_isLocked;
|
||||
double m_hasLostComponents;
|
||||
double m_anEngineIsRunning;
|
||||
double m_isTouchingGround;
|
||||
double m_currentAircraftStatus;
|
||||
double m_currentPhysicsTime_SEC;
|
||||
double m_currentPhysicsSpeedMultiplier;
|
||||
double m_orientationQuaternion_X;
|
||||
double m_orientationQuaternion_Y;
|
||||
double m_orientationQuaternion_Z;
|
||||
double m_orientationQuaternion_W;
|
||||
double m_flightAxisControllerIsActive;
|
||||
} state;
|
||||
|
||||
static const uint16_t num_keys = sizeof(state)/sizeof(double);
|
||||
|
||||
struct keytable {
|
||||
const char *key;
|
||||
double &ref;
|
||||
} keytable[num_keys] = {
|
||||
{ "m-airspeed-MPS", state.m_airspeed_MPS },
|
||||
{ "m-altitudeASL-MTR", state.m_altitudeASL_MTR },
|
||||
{ "m-altitudeAGL-MTR", state.m_altitudeAGL_MTR },
|
||||
{ "m-groundspeed-MPS", state.m_groundspeed_MPS },
|
||||
{ "m-pitchRate-DEGpSEC", state.m_pitchRate_DEGpSEC },
|
||||
{ "m-rollRate-DEGpSEC", state.m_rollRate_DEGpSEC },
|
||||
{ "m-yawRate-DEGpSEC", state.m_yawRate_DEGpSEC },
|
||||
{ "m-azimuth-DEG", state.m_azimuth_DEG },
|
||||
{ "m-inclination-DEG", state.m_inclination_DEG },
|
||||
{ "m-roll-DEG", state.m_roll_DEG },
|
||||
{ "m-aircraftPositionX-MTR", state.m_aircraftPositionX_MTR },
|
||||
{ "m-aircraftPositionY-MTR", state.m_aircraftPositionY_MTR },
|
||||
{ "m-velocityWorldU-MPS", state.m_velocityWorldU_MPS },
|
||||
{ "m-velocityWorldV-MPS", state.m_velocityWorldV_MPS },
|
||||
{ "m-velocityWorldW-MPS", state.m_velocityWorldW_MPS },
|
||||
{ "m-velocityBodyU-MPS", state.m_velocityBodyU_MPS },
|
||||
{ "m-velocityBodyV-MPS", state.m_velocityBodyV_MPS },
|
||||
{ "m-velocityBodyW-MPS", state.m_velocityBodyW_MPS },
|
||||
{ "m-accelerationWorldAX-MPS2", state.m_accelerationWorldAX_MPS2 },
|
||||
{ "m-accelerationWorldAY-MPS2", state.m_accelerationWorldAY_MPS2 },
|
||||
{ "m-accelerationWorldAZ-MPS2", state.m_accelerationWorldAZ_MPS2 },
|
||||
{ "m-accelerationBodyAX-MPS2", state.m_accelerationBodyAX_MPS2 },
|
||||
{ "m-accelerationBodyAY-MPS2", state.m_accelerationBodyAY_MPS2 },
|
||||
{ "m-accelerationBodyAZ-MPS2", state.m_accelerationBodyAZ_MPS2 },
|
||||
{ "m-windX-MPS", state.m_windX_MPS },
|
||||
{ "m-windY-MPS", state.m_windY_MPS },
|
||||
{ "m-windZ-MPS", state.m_windZ_MPS },
|
||||
{ "m-propRPM", state.m_propRPM },
|
||||
{ "m-heliMainRotorRPM", state.m_heliMainRotorRPM },
|
||||
{ "m-batteryVoltage-VOLTS", state.m_batteryVoltage_VOLTS },
|
||||
{ "m-batteryCurrentDraw-AMPS", state.m_batteryCurrentDraw_AMPS },
|
||||
{ "m-batteryRemainingCapacity-MAH", state.m_batteryRemainingCapacity_MAH },
|
||||
{ "m-fuelRemaining-OZ", state.m_fuelRemaining_OZ },
|
||||
{ "m-isLocked", state.m_isLocked },
|
||||
{ "m-hasLostComponents", state.m_hasLostComponents },
|
||||
{ "m-anEngineIsRunning", state.m_anEngineIsRunning },
|
||||
{ "m-isTouchingGround", state.m_isTouchingGround },
|
||||
{ "m-currentAircraftStatus", state.m_currentAircraftStatus },
|
||||
{ "m-currentPhysicsTime-SEC", state.m_currentPhysicsTime_SEC },
|
||||
{ "m-currentPhysicsSpeedMultiplier", state.m_currentPhysicsSpeedMultiplier },
|
||||
{ "m-orientationQuaternion-X", state.m_orientationQuaternion_X },
|
||||
{ "m-orientationQuaternion-Y", state.m_orientationQuaternion_Y },
|
||||
{ "m-orientationQuaternion-Z", state.m_orientationQuaternion_Z },
|
||||
{ "m-orientationQuaternion-W", state.m_orientationQuaternion_W },
|
||||
{ "m-flightAxisControllerIsActive", state.m_flightAxisControllerIsActive }
|
||||
};
|
||||
|
||||
private:
|
||||
char *soap_request(const char *action, const char *fmt, ...);
|
||||
void exchange_data(const struct sitl_input &input);
|
||||
void parse_reply(const char *reply);
|
||||
|
||||
double initial_time_s = 0;
|
||||
double last_time_s = 0;
|
||||
bool heli_demix = false;
|
||||
bool rev4_servos = false;
|
||||
bool controller_started = false;
|
||||
uint64_t frame_counter = 0;
|
||||
uint64_t activation_frame_counter = 0;
|
||||
double last_frame_count_s = 0;
|
||||
Vector3f position_offset;
|
||||
};
|
||||
|
||||
|
||||
} // namespace SITL
|
||||
Loading…
Reference in New Issue