SITL: new XPlane backend

this makes use of DRefs to greatly improve XPlane support. It only
supports XPlane 11 and later

The key change is the use of a JSON file to map ArduPilot output
channels to DataRefs, and map raw joystick inputs to RC inputs

this gets rid of the awful throttle hack handling, and allows for
control of a much wider range of aircraft
This commit is contained in:
Andrew Tridgell 2023-01-28 16:47:25 +11:00
parent 34791eb324
commit 7a4483b091
13 changed files with 601 additions and 282 deletions

View File

@ -2,7 +2,7 @@
# assumes: Aileron, Elevator, Throttle, Rudder, flaps # assumes: Aileron, Elevator, Throttle, Rudder, flaps
{ {
"settings" : { "debug" : 1 }, "settings" : { "debug" : 0 },
"sim/operation/override/override_joystick" : { "type" : "fixed", "value" : 1 }, "sim/operation/override/override_joystick" : { "type" : "fixed", "value" : 1 },
"sim/operation/override/override_flightcontrol" : { "type" : "fixed", "value" : 0 }, "sim/operation/override/override_flightcontrol" : { "type" : "fixed", "value" : 0 },

View File

@ -37,6 +37,10 @@
#include <AP_Terrain/AP_Terrain.h> #include <AP_Terrain/AP_Terrain.h>
#include <AP_Scheduler/AP_Scheduler.h> #include <AP_Scheduler/AP_Scheduler.h>
#include <AP_BoardConfig/AP_BoardConfig.h> #include <AP_BoardConfig/AP_BoardConfig.h>
#if USE_PICOJSON
#include "picojson.h"
#include <AP_Filesystem/AP_Filesystem.h>
#endif
using namespace SITL; using namespace SITL;
@ -1135,4 +1139,3 @@ Vector3d Aircraft::get_position_relhome() const
pos.xy() += home.get_distance_NE_double(origin); pos.xy() += home.get_distance_NE_double(origin);
return pos; return pos;
} }

View File

@ -37,6 +37,10 @@
#include <Filter/Filter.h> #include <Filter/Filter.h>
#include "SIM_JSON_Master.h" #include "SIM_JSON_Master.h"
#ifndef USE_PICOJSON
#define USE_PICOJSON (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX)
#endif
namespace SITL { namespace SITL {
/* /*

View File

@ -20,11 +20,11 @@
#include <AP_Motors/AP_Motors.h> #include <AP_Motors/AP_Motors.h>
#include <AP_Baro/AP_Baro.h> #include <AP_Baro/AP_Baro.h>
#include <AP_Filesystem/AP_Filesystem.h> #include <AP_Filesystem/AP_Filesystem.h>
#include "SIM_Aircraft.h"
#include <stdio.h> #include <stdio.h>
#include <sys/stat.h> #include <sys/stat.h>
using namespace SITL; using namespace SITL;
static Motor quad_plus_motors[] = static Motor quad_plus_motors[] =
@ -341,40 +341,10 @@ void Frame::load_frame_params(const char *model_json)
if (fname == nullptr) { if (fname == nullptr) {
AP_HAL::panic("%s failed to load\n", model_json); AP_HAL::panic("%s failed to load\n", model_json);
} }
::printf("Loading model %s\n", fname); picojson::value *obj = (picojson::value *)load_json(model_json);
int fd = AP::FS().open(model_json, O_RDONLY); if (obj == nullptr) {
if (fd == -1) {
AP_HAL::panic("%s failed to load\n", model_json); AP_HAL::panic("%s failed to load\n", model_json);
} }
char buf[st.st_size+1];
memset(buf, '\0', sizeof(buf));
if (AP::FS().read(fd, buf, st.st_size) != st.st_size) {
AP_HAL::panic("%s failed to load\n", model_json);
}
AP::FS().close(fd);
char *start = strchr(buf, '{');
if (!start) {
AP_HAL::panic("Invalid json %s", model_json);
}
free(fname);
/*
remove comments, as not allowed by the parser
*/
for (char *p = strchr(start,'#'); p; p=strchr(p+1, '#')) {
// clear to end of line
do {
*p++ = ' ';
} while (*p != '\n' && *p != '\r' && *p);
}
picojson::value obj;
std::string err = picojson::parse(obj, start);
if (!err.empty()) {
AP_HAL::panic("Failed to load %s: %s", model_json, err.c_str());
exit(1);
}
enum class VarType { enum class VarType {
FLOAT, FLOAT,
@ -415,7 +385,7 @@ void Frame::load_frame_params(const char *model_json)
}; };
for (uint8_t i=0; i<ARRAY_SIZE(vars); i++) { for (uint8_t i=0; i<ARRAY_SIZE(vars); i++) {
auto v = obj.get(vars[i].label); auto v = obj->get(vars[i].label);
if (v.is<picojson::null>()) { if (v.is<picojson::null>()) {
// use default value // use default value
continue; continue;
@ -438,7 +408,7 @@ void Frame::load_frame_params(const char *model_json)
for (uint8_t i=0; i<ARRAY_SIZE(per_motor_vars); i++) { for (uint8_t i=0; i<ARRAY_SIZE(per_motor_vars); i++) {
for (uint8_t j=0; j<12; j++) { for (uint8_t j=0; j<12; j++) {
sprintf(label_name, "motor%i_%s", j+1, per_motor_vars[i].label); sprintf(label_name, "motor%i_%s", j+1, per_motor_vars[i].label);
auto v = obj.get(label_name); auto v = obj->get(label_name);
if (v.is<picojson::null>()) { if (v.is<picojson::null>()) {
// use default value // use default value
continue; continue;
@ -452,6 +422,8 @@ void Frame::load_frame_params(const char *model_json)
} }
} }
delete obj;
::printf("Loaded model params from %s\n", model_json); ::printf("Loaded model params from %s\n", model_json);
} }
@ -473,10 +445,11 @@ void Frame::parse_vector3(picojson::value val, const char* label, Vector3f &para
#endif #endif
#if AP_SIM_ENABLED
/* /*
initialise the frame initialise the frame
*/ */
#if AP_SIM_ENABLED
void Frame::init(const char *frame_str, Battery *_battery) void Frame::init(const char *frame_str, Battery *_battery)
{ {
model = default_model; model = default_model;

View File

@ -21,10 +21,6 @@
#include "SIM_Aircraft.h" #include "SIM_Aircraft.h"
#include "SIM_Motor.h" #include "SIM_Motor.h"
#ifndef USE_PICOJSON
#define USE_PICOJSON (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#endif
#if USE_PICOJSON #if USE_PICOJSON
#include "picojson.h" #include "picojson.h"
#endif #endif

View File

@ -59,6 +59,9 @@ GPS::GPS(uint8_t _instance) :
uint32_t GPS::device_baud() const uint32_t GPS::device_baud() const
{ {
if (_sitl == nullptr) {
return 0;
}
switch ((Type)_sitl->gps_type[instance].get()) { switch ((Type)_sitl->gps_type[instance].get()) {
case Type::NOVA: case Type::NOVA:
return 19200; return 19200;

View File

@ -28,14 +28,68 @@
#include <sys/types.h> #include <sys/types.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Filesystem/AP_Filesystem.h>
#include <SRV_Channel/SRV_Channel.h> #include <SRV_Channel/SRV_Channel.h>
#include "picojson.h"
#include <AP_Vehicle/AP_Vehicle_Type.h>
// ignore cast errors in this case to keep complexity down // ignore cast errors in this case to keep complexity down
#pragma GCC diagnostic ignored "-Wcast-align" #pragma GCC diagnostic ignored "-Wcast-align"
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
namespace SITL { #if APM_BUILD_TYPE(APM_BUILD_Heli)
#define XPLANE_JSON "xplane_heli.json"
#else
#define XPLANE_JSON "xplane_plane.json"
#endif
// DATA@ frame types. Thanks to TauLabs xplanesimulator.h
// (which strangely enough acknowledges APM as a source!)
enum {
FramRate = 0,
Times = 1,
SimStats = 2,
Speed = 3,
Gload = 4,
AtmosphereWeather = 5,
AtmosphereAircraft = 6,
SystemPressures = 7,
Joystick1 = 8,
Joystick2 = 9,
ArtStab = 10,
FlightCon = 11,
WingSweep = 12,
Trim = 13,
Brakes = 14,
AngularMoments = 15,
AngularVelocities = 16,
PitchRollHeading = 17,
AoA = 18,
MagCompass = 19,
LatLonAlt = 20,
LocVelDistTraveled = 21,
ThrottleCommand = 25,
CarbHeat = 30,
EngineRPM = 37,
PropRPM = 38,
PropPitch = 39,
Generator = 58,
JoystickRaw = 136,
};
enum RREF {
RREF_VERSION = 1,
};
static const uint8_t required_data[] {
Times, LatLonAlt, Speed, PitchRollHeading,
LocVelDistTraveled, AngularVelocities, Gload,
Trim,
PropPitch, EngineRPM, PropRPM,
JoystickRaw };
using namespace SITL;
XPlane::XPlane(const char *frame_str) : XPlane::XPlane(const char *frame_str) :
Aircraft(frame_str) Aircraft(frame_str)
@ -46,79 +100,248 @@ XPlane::XPlane(const char *frame_str) :
xplane_ip = colon+1; xplane_ip = colon+1;
} }
heli_frame = (strstr(frame_str, "-heli") != nullptr);
socket_in.bind("0.0.0.0", bind_port); socket_in.bind("0.0.0.0", bind_port);
printf("Waiting for XPlane data on UDP port %u and sending to port %u\n", printf("Waiting for XPlane data on UDP port %u and sending to port %u\n",
(unsigned)bind_port, (unsigned)xplane_port); (unsigned)bind_port, (unsigned)xplane_port);
// XPlane sensor data is not good enough for EKF. Use fake EKF by default // XPlane sensor data is not good enough for EKF. Use fake EKF by default
AP_Param::set_default_by_name("AHRS_EKF_TYPE", 10); AP_Param::set_default_by_name("AHRS_EKF_TYPE", 10);
AP_Param::set_default_by_name("GPS_TYPE", 100);
AP_Param::set_default_by_name("INS_GYR_CAL", 0); AP_Param::set_default_by_name("INS_GYR_CAL", 0);
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// default flaps to channel 5
AP_Param::set_default_by_name("SERVO5_FUNCTION", 3);
AP_Param::set_default_by_name("SERVO5_MIN", 1000);
AP_Param::set_default_by_name("SERVO5_MAX", 2000);
#endif
if (!load_dref_map(XPLANE_JSON)) {
AP_HAL::panic("%s failed to load\n", XPLANE_JSON);
}
}
/*
add one DRef to list
*/
void XPlane::add_dref(const char *name, DRefType type, const picojson::value &dref)
{
struct DRef *d = new struct DRef;
if (d == nullptr) {
AP_HAL::panic("out of memory for DRef %s", name);
}
d->name = strdup(name);
d->type = type;
if (d->name == nullptr) {
AP_HAL::panic("out of memory for DRef %s", name);
}
if (d->type == DRefType::FIXED) {
d->fixed_value = dref.get("value").get<double>();
} else {
d->range = dref.get("range").get<double>();
d->channel = dref.get("channel").get<double>();
}
// add to linked list
d->next = drefs;
drefs = d;
}
/*
add one joystick axis to list
*/
void XPlane::add_joyinput(const char *label, JoyType type, const picojson::value &d)
{
if (strncmp(label, "axis", 4) == 0) {
struct JoyInput *j = new struct JoyInput;
if (j == nullptr) {
AP_HAL::panic("out of memory for JoyInput %s", label);
}
j->axis = atoi(label+4);
j->type = JoyType::AXIS;
j->channel = d.get("channel").get<double>();
j->input_min = d.get("input_min").get<double>();
j->input_max = d.get("input_max").get<double>();
j->next = joyinputs;
joyinputs = j;
}
if (strncmp(label, "button", 6) == 0) {
struct JoyInput *j = new struct JoyInput;
if (j == nullptr) {
AP_HAL::panic("out of memory for JoyInput %s", label);
}
j->type = JoyType::BUTTON;
j->channel = d.get("channel").get<double>();
j->mask = d.get("mask").get<double>();
j->next = joyinputs;
joyinputs = j;
}
}
/*
handle a setting
*/
void XPlane::handle_setting(const picojson::value &d)
{
if (d.contains("debug")) {
dref_debug = d.get("debug").get<double>();
}
}
/*
load mapping of channels to datarefs from a json file
*/
bool XPlane::load_dref_map(const char *map_json)
{
char *fname = nullptr;
if (AP::FS().stat(map_json, &map_st) == 0) {
fname = strdup(map_json);
} else {
IGNORE_RETURN(asprintf(&fname, "@ROMFS/models/%s", map_json));
if (AP::FS().stat(fname, &map_st) != 0) {
return false;
}
}
if (fname == nullptr) {
return false;
}
picojson::value *obj = (picojson::value *)load_json(fname);
if (obj == nullptr) {
return false;
}
free(map_filename);
map_filename = fname;
// free old drefs
while (drefs) {
auto *d = drefs->next;
free(drefs->name);
delete drefs;
drefs = d;
}
// free old joystick
while (joyinputs) {
auto *j = joyinputs->next;
delete joyinputs;
joyinputs = j;
}
uint32_t count = 0;
// obtain a const reference to the map, and print the contents
const picojson::value::object& o = obj->get<picojson::object>();
for (picojson::value::object::const_iterator i = o.begin();
i != o.end();
++i) {
const char *label = i->first.c_str();
const auto &d = i->second;
if (strchr(label, '/') != nullptr) {
const char *type_s = d.get("type").to_str().c_str();
if (strcmp(type_s, "angle") == 0) {
add_dref(label, DRefType::ANGLE, d);
} else if (strcmp(type_s, "range") == 0) {
add_dref(label, DRefType::RANGE, d);
} else if (strcmp(type_s, "fixed") == 0) {
add_dref(label, DRefType::FIXED, d);
} else {
::printf("Invalid dref type %s for %s in %s", type_s, label, map_filename);
}
} else if (strcmp(label, "settings") == 0) {
handle_setting(d);
} else if (strncmp(label, "axis", 4) == 0) {
add_joyinput(label, JoyType::AXIS, d);
} else if (strncmp(label, "button", 6) == 0) {
add_joyinput(label, JoyType::BUTTON, d);
} else {
::printf("Invalid json type %s in %s", label, map_json);
continue;
}
count++;
}
delete obj;
::printf("Loaded %u DRefs from %s\n", unsigned(count), map_filename);
return true;
}
/*
load mapping of channels to datarefs from a json file
*/
void XPlane::check_reload_dref(void)
{
if (!hal.util->get_soft_armed()) {
struct stat st;
if (AP::FS().stat(map_filename, &st) == 0 && st.st_mtime != map_st.st_mtime) {
load_dref_map(map_filename);
}
}
}
int8_t XPlane::find_data_index(uint8_t code)
{
for (uint8_t i = 0; i<ARRAY_SIZE(required_data); i++) {
if (required_data[i] == code) {
return i;
}
}
return -1;
} }
/* /*
change what data is requested from XPlane. This saves the user from change what data is requested from XPlane. This saves the user from
having to setup the data screen correctly having to setup the data screen correctly
*/ */
void XPlane::select_data(uint64_t usel_mask, uint64_t sel_mask) void XPlane::select_data(void)
{ {
const uint64_t all_mask = (1U<<ARRAY_SIZE(required_data))-1;
if ((seen_mask & all_mask) == all_mask) {
// got it all
return;
}
struct PACKED { struct PACKED {
uint8_t marker[5] { 'D', 'S', 'E', 'L', '0' }; uint8_t marker[5] { 'D', 'S', 'E', 'L', '0' };
uint32_t data[8] {}; uint32_t data[8] {};
} dsel; } dsel;
uint8_t count = 0; uint8_t count = 0;
for (uint8_t i=0; i<64 && count<8; i++) { for (uint8_t i=0; i<ARRAY_SIZE(required_data); i++) {
if ((((uint64_t)1)<<i) & sel_mask) { if (seen_mask & (1U<<i)) {
dsel.data[count++] = i; // got this one
printf("i=%u\n", (unsigned)i); continue;
} }
dsel.data[count++] = required_data[i];
} }
if (count != 0) { if (count != 0) {
socket_out.send(&dsel, sizeof(dsel)); socket_out.send(&dsel, sizeof(dsel));
printf("Selecting %u data types 0x%llx\n", (unsigned)count, (unsigned long long)sel_mask); printf("Selecting %u data types\n", (unsigned)count);
} }
}
void XPlane::deselect_code(uint8_t code)
{
struct PACKED { struct PACKED {
uint8_t marker[5] { 'U', 'S', 'E', 'L', '0' }; uint8_t marker[5] { 'U', 'S', 'E', 'L', '0' };
uint32_t data[8] {}; uint32_t data[8] {};
} usel; } usel;
count = 0; usel.data[0] = code;
socket_out.send(&usel, sizeof(usel));
// only de-select an output once, so we don't fight the user printf("De-selecting code %u\n", code);
usel_mask &= ~unselected_mask;
unselected_mask |= usel_mask;
for (uint8_t i=0; i<64 && count<8; i++) {
if ((((uint64_t)1)<<i) & usel_mask) {
usel.data[count++] = i;
}
}
if (count != 0) {
socket_out.send(&usel, sizeof(usel));
printf("De-selecting %u data types 0x%llx\n", (unsigned)count, (unsigned long long)usel_mask);
}
} }
/* /*
receive data from X-Plane via UDP receive data from X-Plane via UDP
return true if we get a gyro frame
*/ */
bool XPlane::receive_data(void) bool XPlane::receive_data(void)
{ {
uint8_t pkt[10000]; uint8_t pkt[10000];
uint8_t *p = &pkt[5]; uint8_t *p = &pkt[5];
const uint8_t pkt_len = 36; const uint8_t pkt_len = 36;
uint64_t data_mask = 0;
const uint64_t one = 1U;
const uint64_t required_mask = (one<<Times | one<<LatLonAlt | one<<Speed | one<<PitchRollHeading |
one<<LocVelDistTraveled | one<<AngularVelocities | one<<Gload |
one << Joystick1 | one << ThrottleCommand | one << Trim |
one << PropPitch | one << EngineRPM | one << PropRPM | one << Generator |
one << Mixture);
Location loc {}; Location loc {};
Vector3d pos; Vector3d pos;
uint32_t wait_time_ms = 1; uint32_t wait_time_ms = 1;
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
bool ret = false;
// if we are about to get another frame from X-Plane then wait longer // if we are about to get another frame from X-Plane then wait longer
if (xplane_frame_time > wait_time_ms && if (xplane_frame_time > wait_time_ms &&
@ -127,12 +350,29 @@ bool XPlane::receive_data(void)
} }
ssize_t len = socket_in.recv(pkt, sizeof(pkt), wait_time_ms); ssize_t len = socket_in.recv(pkt, sizeof(pkt), wait_time_ms);
if (len < pkt_len+5 || memcmp(pkt, "DATA", 4) != 0) { if (len < 5) {
// bad packet
goto failed;
}
if (memcmp(pkt, "RREF", 4) == 0) {
handle_rref(pkt, len);
return false;
}
if (memcmp(pkt, "DATA", 4) != 0) {
// not a data packet we understand // not a data packet we understand
::printf("PACKET: %4.4s\n", (const char *)pkt);
goto failed; goto failed;
} }
len -= 5; len -= 5;
if (len < pkt_len) {
// bad packet
goto failed;
}
if (!connected) { if (!connected) {
// we now know the IP X-Plane is using // we now know the IP X-Plane is using
uint16_t port; uint16_t port;
@ -145,10 +385,15 @@ bool XPlane::receive_data(void)
while (len >= pkt_len) { while (len >= pkt_len) {
const float *data = (const float *)p; const float *data = (const float *)p;
uint8_t code = p[0]; uint8_t code = p[0];
// keep a mask of what codes we have received int8_t idx = find_data_index(code);
if (code < 64) { if (idx == -1) {
data_mask |= (((uint64_t)1) << code); deselect_code(code);
len -= pkt_len;
p += pkt_len;
continue;
} }
seen_mask |= (1U<<idx);
switch (code) { switch (code) {
case Times: { case Times: {
uint64_t tus = data[3] * 1.0e6f; uint64_t tus = data[3] * 1.0e6f;
@ -184,13 +429,6 @@ bool XPlane::receive_data(void)
// ignored // ignored
break; break;
case Trim:
if (heli_frame) {
// use flaps for collective as no direct collective data input
rcin[2] = data[4];
}
break;
case PitchRollHeading: { case PitchRollHeading: {
float roll, pitch, yaw; float roll, pitch, yaw;
pitch = radians(data[1]); pitch = radians(data[1]);
@ -214,9 +452,17 @@ bool XPlane::receive_data(void)
break; break;
case AngularVelocities: case AngularVelocities:
gyro.y = data[1]; if (is_xplane12()) {
gyro.x = data[2]; gyro.x = radians(data[1]);
gyro.z = data[3]; gyro.y = radians(data[2]);
gyro.z = radians(data[3]);
} else {
gyro.x = data[1];
gyro.y = data[2];
gyro.z = data[3];
}
// we only count gyro data towards data counts
ret = true;
break; break;
case Gload: case Gload:
@ -225,84 +471,56 @@ bool XPlane::receive_data(void)
accel_body.y = data[7] * GRAVITY_MSS; accel_body.y = data[7] * GRAVITY_MSS;
break; break;
case Joystick1:
rcin_chan_count = 4;
rcin[0] = (data[2] + 1)*0.5f;
rcin[1] = (data[1] + 1)*0.5f;
rcin[3] = (data[3] + 1)*0.5f;
break;
case ThrottleCommand: {
if (!heli_frame) {
/* getting joystick throttle input is very weird. The
* problem is that XPlane sends the ThrottleCommand packet
* both for joystick throttle input and for throttle that
* we have provided over the link. So we need some way to
* detect when we get our own values back. The trick used
* is to add throttle_magic to the values we send, then
* detect this offset in the data coming back. Very ugly,
* but I can't find a better way of allowing joystick
* input from XPlane10
*/
bool has_magic = ((uint32_t)(data[1] * throttle_magic_scale) % 1000U) == (uint32_t)(throttle_magic * throttle_magic_scale);
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wfloat-equal"
if (data[1] < 0 ||
data[1] == throttle_sent ||
has_magic) {
break;
}
#pragma GCC diagnostic pop
rcin[2] = data[1];
}
break;
}
case PropPitch: { case PropPitch: {
break; break;
} }
case EngineRPM: case EngineRPM:
rpm[0] = data[1]; rpm[0] = data[1];
motor_mask |= 1;
break; break;
case PropRPM: case PropRPM:
rpm[1] = data[1]; rpm[1] = data[1];
motor_mask |= 2;
break; break;
case Joystick2: case JoystickRaw: {
break; for (auto *j = joyinputs; j; j=j->next) {
switch (j->type) {
case Generator: case JoyType::AXIS: {
/* if (j->axis >= 1 && j->axis <= 6) {
in order to get interlock switch on helis we map the float v = (data[j->axis] - j->input_min) / (j->input_max - j->input_min);
"generator1 on/off" function of XPlane 10 to channel 8. rcin[j->channel-1] = v;
*/ rcin_chan_count = MAX(rcin_chan_count, j->channel);
rcin_chan_count = 8; }
rcin[7] = data[1]; break;
break; }
case JoyType::BUTTON: {
case Mixture: uint32_t m = uint32_t(data[7]) & j->mask;
// map channel 6 and 7 from Mixture3 and Mixture4 for extra channels float v = 0;
rcin_chan_count = MAX(7, rcin_chan_count); if (m == 0) {
rcin[5] = data[3]; v = 0;
rcin[6] = data[4]; } else if (1U<<(__builtin_ffs(j->mask)-1) != m) {
break; v = 0.5;
} else {
v = 1;
}
rcin[j->channel-1] = v;
rcin_chan_count = MAX(rcin_chan_count, j->channel);
break;
}
}
}
}
} }
len -= pkt_len; len -= pkt_len;
p += pkt_len; p += pkt_len;
} }
if (data_mask != required_mask) { // update data selection
// ask XPlane to change what data it sends select_data();
uint64_t usel = data_mask & ~required_mask;
uint64_t sel = required_mask & ~data_mask;
usel &= ~unselected_mask;
if (usel || sel) {
select_data(usel, sel);
goto failed;
}
}
position = pos + position_zero; position = pos + position_zero;
position.xy() += origin.get_distance_NE_double(home); position.xy() += origin.get_distance_NE_double(home);
update_position(); update_position();
@ -335,9 +553,12 @@ bool XPlane::receive_data(void)
} }
last_data_time_ms = AP_HAL::millis(); last_data_time_ms = AP_HAL::millis();
report.data_count++; if (ret) {
report.frame_count++; report.data_count++;
return true; report.frame_count++;
}
return ret;
failed: failed:
if (AP_HAL::millis() - last_data_time_ms > 200) { if (AP_HAL::millis() - last_data_time_ms > 200) {
@ -360,93 +581,56 @@ failed:
return false; return false;
} }
/*
receive RREF replies
*/
void XPlane::handle_rref(const uint8_t *pkt, uint32_t len)
{
const uint8_t *p = &pkt[5];
const struct PACKED RRefPacket {
uint32_t code;
union PACKED {
float value_f;
double value_d;
};
} *ref = (const struct RRefPacket *)p;
switch (ref->code) {
case RREF_VERSION:
if (xplane_version == 0) {
::printf("XPlane version %.0f\n", ref->value_f);
}
xplane_version = uint32_t(ref->value_f);
break;
}
}
/* /*
send data to X-Plane via UDP send DRef data to X-Plane via UDP
*/ */
void XPlane::send_data(const struct sitl_input &input) void XPlane::send_drefs(const struct sitl_input &input)
{ {
float aileron = (input.servos[0]-1500)/500.0f; for (const auto *d = drefs; d; d=d->next) {
float elevator = (input.servos[1]-1500)/500.0f; switch (d->type) {
float throttle = (input.servos[2]-1000)/1000.0;
float rudder = (input.servos[3]-1500)/500.0f;
struct PACKED {
uint8_t marker[5] { 'D', 'A', 'T', 'A', '0' };
uint32_t code;
float data[8];
} d {};
if (input.servos[0] == 0) { case DRefType::ANGLE: {
aileron = 0; float v = d->range * (input.servos[d->channel-1]-1500)/500.0;
} send_dref(d->name, v);
if (input.servos[1] == 0) { break;
elevator = 0; }
}
if (input.servos[2] == 0) {
throttle = 0;
}
if (input.servos[3] == 0) {
rudder = 0;
}
// we add the throttle_magic to the throttle value we send so we case DRefType::RANGE: {
// can detect when we get it back float v = d->range * (input.servos[d->channel-1]-1000)/1000.0;
throttle = ((uint32_t)(throttle * 1000)) * 1.0e-3f + throttle_magic; send_dref(d->name, v);
break;
}
uint8_t flap_chan; case DRefType::FIXED: {
if (SRV_Channels::find_channel(SRV_Channel::k_flap, flap_chan) || send_dref(d->name, d->fixed_value);
SRV_Channels::find_channel(SRV_Channel::k_flap_auto, flap_chan)) { break;
float flap = (input.servos[flap_chan]-1000)/1000.0; }
if (!is_equal(flap, last_flap)) {
send_dref("sim/flightmodel/controls/flaprqst", flap);
send_dref("sim/aircraft/overflow/acf_flap_arm", flap>0?1:0);
} }
} }
d.code = FlightCon;
d.data[0] = elevator;
d.data[1] = aileron;
d.data[2] = rudder;
d.data[4] = rudder;
socket_out.send(&d, sizeof(d));
if (!heli_frame) {
d.code = ThrottleCommand;
d.data[0] = throttle;
d.data[1] = throttle;
d.data[2] = throttle;
d.data[3] = throttle;
d.data[4] = 0;
socket_out.send(&d, sizeof(d));
} else {
// send chan3 as collective pitch, on scale from -10 to +10
float collective = 10*(input.servos[2]-1500)/500.0;
// and send throttle from channel 8
throttle = (input.servos[7]-1000)/1000.0;
// allow for extra throttle outputs for special aircraft
float throttle2 = (input.servos[5]-1000)/1000.0;
float throttle3 = (input.servos[6]-1000)/1000.0;
d.code = PropPitch;
d.data[0] = collective;
d.data[1] = -rudder*15; // reverse sense of rudder, 15 degrees pitch range
d.data[2] = 0;
d.data[3] = 0;
d.data[4] = 0;
socket_out.send(&d, sizeof(d));
d.code = ThrottleCommand;
d.data[0] = throttle;
d.data[1] = throttle;
d.data[2] = throttle2;
d.data[3] = throttle3;
d.data[4] = 0;
socket_out.send(&d, sizeof(d));
}
throttle_sent = throttle;
} }
@ -463,6 +647,31 @@ void XPlane::send_dref(const char *name, float value)
d.value = value; d.value = value;
strcpy(d.name, name); strcpy(d.name, name);
socket_out.send(&d, sizeof(d)); socket_out.send(&d, sizeof(d));
if (dref_debug > 0) {
::printf("-> %s : %.3f\n", name, value);
}
}
/*
request a dref
*/
void XPlane::request_dref(const char *name, uint8_t code, uint32_t rate)
{
struct PACKED {
uint8_t marker[5] { 'R', 'R', 'E', 'F', '0' };
uint32_t rate_hz;
uint32_t code;
char name[400];
} d {};
d.rate_hz = rate;
d.code = code; // given back in responses
strcpy(d.name, name);
socket_in.sendto(&d, sizeof(d), xplane_ip, xplane_port);
}
void XPlane::request_drefs(void)
{
request_dref("sim/version/xplane_internal_version", RREF_VERSION, 1);
} }
/* /*
@ -471,12 +680,13 @@ void XPlane::send_dref(const char *name, float value)
void XPlane::update(const struct sitl_input &input) void XPlane::update(const struct sitl_input &input)
{ {
if (receive_data()) { if (receive_data()) {
send_data(input); send_drefs(input);
} }
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if (report.last_report_ms == 0) { if (report.last_report_ms == 0) {
report.last_report_ms = now; report.last_report_ms = now;
request_drefs();
} }
if (now - report.last_report_ms > 5000) { if (now - report.last_report_ms > 5000) {
float dt = (now - report.last_report_ms) * 1.0e-3f; float dt = (now - report.last_report_ms) * 1.0e-3f;
@ -485,9 +695,9 @@ void XPlane::update(const struct sitl_input &input)
report.last_report_ms = now; report.last_report_ms = now;
report.data_count = 0; report.data_count = 0;
report.frame_count = 0; report.frame_count = 0;
request_drefs();
} }
check_reload_dref();
} }
} // namespace SITL
#endif // HAL_SIM_XPLANE_ENABLED #endif // HAL_SIM_XPLANE_ENABLED

View File

@ -27,8 +27,10 @@
#if HAL_SIM_XPLANE_ENABLED #if HAL_SIM_XPLANE_ENABLED
#include <AP_HAL/utility/Socket.h> #include <AP_HAL/utility/Socket.h>
#include <AP_Filesystem/AP_Filesystem.h>
#include "SIM_Aircraft.h" #include "SIM_Aircraft.h"
#include "picojson.h"
namespace SITL { namespace SITL {
@ -48,10 +50,21 @@ public:
} }
private: private:
bool receive_data(void); bool receive_data(void);
void send_dref(const char *name, float value); void send_dref(const char *name, float value);
void send_data(const struct sitl_input &input); void request_drefs(void);
void select_data(uint64_t usel_mask, uint64_t sel_mask); void request_dref(const char *name, uint8_t code, uint32_t rate_hz);
void send_drefs(const struct sitl_input &input);
void handle_rref(const uint8_t *p, uint32_t len);
void select_data(void);
void deselect_code(uint8_t code);
int8_t find_data_index(uint8_t id);
// return true if at least X
bool is_xplane12(void) const {
return xplane_version / 10000 >= 12;
}
const char *xplane_ip = "127.0.0.1"; const char *xplane_ip = "127.0.0.1";
uint16_t xplane_port = 49000; uint16_t xplane_port = 49000;
@ -64,58 +77,63 @@ private:
uint32_t last_data_time_ms; uint32_t last_data_time_ms;
Vector3d position_zero; Vector3d position_zero;
Vector3f accel_earth; Vector3f accel_earth;
float throttle_sent = -1;
bool connected = false; bool connected = false;
uint32_t xplane_frame_time; uint32_t xplane_frame_time;
uint64_t seen_mask;
struct { struct {
uint32_t last_report_ms; uint32_t last_report_ms;
uint32_t data_count; uint32_t data_count;
uint32_t frame_count; uint32_t frame_count;
} report; } report;
float last_flap;
// are we controlling a heli? enum class DRefType {
bool heli_frame; ANGLE = 0,
RANGE = 1,
FIXED = 2,
};
uint64_t unselected_mask; struct DRef {
struct DRef *next;
char *name;
DRefType type;
uint8_t channel;
float range;
float fixed_value;
};
// throttle joystick input is very weird. See comments in the main code // list of DRefs;
const float throttle_magic = 0.000123f; struct DRef *drefs;
const float throttle_magic_scale = 1.0e6; uint32_t dref_debug;
// DATA@ frame types. Thanks to TauLabs xplanesimulator.h enum class JoyType {
// (which strangely enough acknowledges APM as a source!) AXIS = 0,
enum { BUTTON = 1,
FramRate = 0, };
Times = 1,
SimStats = 2, // list of joystick inputs
Speed = 3, struct JoyInput {
Gload = 4, struct JoyInput *next;
AtmosphereWeather = 5, uint8_t axis;
AtmosphereAircraft = 6, uint8_t channel;
SystemPressures = 7, JoyType type;
Joystick1 = 8, float input_min, input_max;
Joystick2 = 9, uint32_t mask;
ArtStab = 10, };
FlightCon = 11, struct JoyInput *joyinputs;
WingSweep = 12,
Trim = 13, char *map_filename;
Brakes = 14, struct stat map_st;
AngularMoments = 15,
AngularVelocities = 16, bool load_dref_map(const char *map_json);
PitchRollHeading = 17, void add_dref(const char *name, DRefType type, const picojson::value &dref);
AoA = 18, void add_joyinput(const char *name, JoyType type, const picojson::value &d);
MagCompass = 19, void handle_setting(const picojson::value &d);
LatLonAlt = 20,
LocVelDistTraveled = 21, void check_reload_dref(void);
ThrottleCommand = 25,
Mixture = 29, uint32_t xplane_version;
CarbHeat = 30, bool have_ref_lat;
EngineRPM = 37,
PropRPM = 38,
PropPitch = 39,
Generator = 58,
};
}; };

View File

@ -9,6 +9,7 @@ def build(bld):
source = bld.path.ant_glob('*.cpp') source = bld.path.ant_glob('*.cpp')
source.append('../../../../libraries/SITL/SIM_Battery.cpp') source.append('../../../../libraries/SITL/SIM_Battery.cpp')
source.append('../../../../libraries/SITL/SIM_Frame.cpp') source.append('../../../../libraries/SITL/SIM_Frame.cpp')
source.append('../../../../libraries/SITL/picojson.cpp')
bld.env.DEFINES.append("USE_PICOJSON=1") bld.env.DEFINES.append("USE_PICOJSON=1")

View File

@ -9,6 +9,7 @@ def build(bld):
source = bld.path.ant_glob('*.cpp') source = bld.path.ant_glob('*.cpp')
source.append('../../../../libraries/SITL/SIM_Motor.cpp') source.append('../../../../libraries/SITL/SIM_Motor.cpp')
source.append('../../../../libraries/SITL/SIM_Frame.cpp') source.append('../../../../libraries/SITL/SIM_Frame.cpp')
source.append('../../../../libraries/SITL/picojson.cpp')
bld.env.DEFINES.append("USE_PICOJSON=1") bld.env.DEFINES.append("USE_PICOJSON=1")

View File

@ -0,0 +1,44 @@
# XPlane DREF map file for Alii quadplane
{
"settings" : { "debug" : 0 },
"sim/operation/override/override_engines" : { "type" : "fixed", "value" : 1 },
"sim/operation/override/override_control_surfaces" : { "type" : "fixed", "value" : 1 },
# forward throttle
"sim/flightmodel/engine/ENGN_TRQ[4]" : { "type" : "range", "range" : 3000, "channel" : 3 },
# VTOL motors
"sim/flightmodel/engine/ENGN_TRQ[0]" : { "type" : "range", "range" : 4000, "channel" : 5 },
"sim/flightmodel/engine/ENGN_TRQ[1]" : { "type" : "range", "range" : 4000, "channel" : 7 },
"sim/flightmodel/engine/ENGN_TRQ[2]" : { "type" : "range", "range" : 4000, "channel" : 6 },
"sim/flightmodel/engine/ENGN_TRQ[3]" : { "type" : "range", "range" : 4000, "channel" : 8 },
# ailerons
"sim/flightmodel2/wing/aileron1_deg[2]" : { "type" : "angle", "range" : 30, "channel" : 1 }, # left inner
"sim/flightmodel2/wing/aileron1_deg[3]" : { "type" : "angle", "range" : -30, "channel" : 9 }, # right inner
"sim/flightmodel2/wing/aileron2_deg[4]" : { "type" : "angle", "range" : 30, "channel" : 1 }, # left outer
"sim/flightmodel2/wing/aileron2_deg[5]" : { "type" : "angle", "range" : -30, "channel" : 9 }, # right outer
# elevators
"sim/flightmodel2/wing/elevator1_deg[8]" : { "type" : "angle", "range" : -30, "channel" : 2 }, # left inner
"sim/flightmodel2/wing/elevator1_deg[9]" : { "type" : "angle", "range" : -30, "channel" : 2 }, # right inner
"sim/flightmodel2/wing/elevator2_deg[8]" : { "type" : "angle", "range" : -30, "channel" : 2 }, # left outer
"sim/flightmodel2/wing/elevator2_deg[9]" : { "type" : "angle", "range" : -30, "channel" : 2 }, # right outer
# rudders
"sim/flightmodel2/wing/rudder1_deg[10]" : { "type" : "angle", "range" : -30, "channel" : 4 }, # left
"sim/flightmodel2/wing/rudder2_deg[11]" : { "type" : "angle", "range" : 30, "channel" : 4 }, # right
# joystick input
"axis6" : { "type" : "joyaxis", "channel" : 1, "input_min" : 0.2, "input_max" : 0.8 },
"axis5" : { "type" : "joyaxis", "channel" : 2, "input_min" : 0.2, "input_max" : 0.8 },
"axis4" : { "type" : "joyaxis", "channel" : 3, "input_min" : 0.8, "input_max" : 0.2 },
"axis2" : { "type" : "joyaxis", "channel" : 4, "input_min" : 0.2, "input_max" : 0.9 },
"axis3" : { "type" : "joyaxis", "channel" : 5, "input_min" : 0.2, "input_max" : 0.9 },
"button1" : { "channel" : 6, "mask" : 1 },
"button2" : { "channel" : 7, "mask" : 2 },
"button3" : { "type" : "joybutton", "channel" : 8, "mask" : 24 },
"button4" : { "type" : "joybutton", "channel" : 9, "mask" : 4 }
}

View File

@ -0,0 +1,62 @@
#include "SIM_Aircraft.h"
#include <AP_Filesystem/AP_Filesystem.h>
#if USE_PICOJSON
#include "picojson.h"
/*
load JSON file, returning a picojson object or nullptr on failure
*/
void *load_json(const char *filename)
{
struct stat st;
if (AP::FS().stat(filename, &st) != 0) {
::printf("No such json file %s\n", filename);
return nullptr;
}
int fd = AP::FS().open(filename, O_RDONLY);
if (fd == -1) {
::printf("failed to open json %s\n", filename);
return nullptr;
}
char buf[st.st_size+1];
memset(buf, '\0', sizeof(buf));
if (AP::FS().read(fd, buf, st.st_size) != st.st_size) {
::printf("failed to read json %s\n", filename);
AP::FS().close(fd);
return nullptr;
}
AP::FS().close(fd);
char *start = strchr(buf, '{');
if (!start) {
::printf("Invalid json %s", filename);
return nullptr;
}
/*
remove comments, as not allowed by the parser
*/
for (char *p = strchr(start,'#'); p; p=strchr(p+1, '#')) {
// clear to end of line
do {
*p++ = ' ';
} while (*p != '\n' && *p != '\r' && *p);
}
picojson::value *obj = new picojson::value;
if (obj == nullptr) {
::printf("Invalid allocate json for %s", filename);
return nullptr;
}
std::string err = picojson::parse(*obj, start);
if (!err.empty()) {
::printf("parse failed for json %s", filename);
delete obj;
return nullptr;
}
return obj;
}
#endif // USE_PICOJSON

View File

@ -31,6 +31,10 @@
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
*/ */
// load a json file, actual return type is picojson::value
void *load_json(const char *filename);
/* /*
use picojson to load optional frame files use picojson to load optional frame files
*/ */