Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity
d76077b921
ardupilot/libraries/SITL/SIM_XPlane.cpp
Andrew Tridgell 7a4483b091 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
2023-01-31 11:22:08 +11:00

704 lines
19 KiB
C++
Raw Blame History

/*
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 XPlane
*/
#include "SIM_XPlane.h"
#if HAL_SIM_XPLANE_ENABLED
#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>
#include <AP_Filesystem/AP_Filesystem.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
#pragma GCC diagnostic ignored "-Wcast-align"
extern const AP_HAL::HAL& hal;
#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) :
Aircraft(frame_str)
{
use_time_sync = false;
const char *colon = strchr(frame_str, ':');
if (colon) {
xplane_ip = colon+1;
}
socket_in.bind("0.0.0.0", bind_port);
printf("Waiting for XPlane data on UDP port %u and sending to port %u\n",
(unsigned)bind_port, (unsigned)xplane_port);
// 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("GPS_TYPE", 100);
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
having to setup the data screen correctly
*/
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 {
uint8_t marker[5] { 'D', 'S', 'E', 'L', '0' };
uint32_t data[8] {};
} dsel;
uint8_t count = 0;
for (uint8_t i=0; i<ARRAY_SIZE(required_data); i++) {
if (seen_mask & (1U<<i)) {
// got this one
continue;
}
dsel.data[count++] = required_data[i];
}
if (count != 0) {
socket_out.send(&dsel, sizeof(dsel));
printf("Selecting %u data types\n", (unsigned)count);
}
}
void XPlane::deselect_code(uint8_t code)
{
struct PACKED {
uint8_t marker[5] { 'U', 'S', 'E', 'L', '0' };
uint32_t data[8] {};
} usel;
usel.data[0] = code;
socket_out.send(&usel, sizeof(usel));
printf("De-selecting code %u\n", code);
}
/*
receive data from X-Plane via UDP
return true if we get a gyro frame
*/
bool XPlane::receive_data(void)
{
uint8_t pkt[10000];
uint8_t *p = &pkt[5];
const uint8_t pkt_len = 36;
Location loc {};
Vector3d pos;
uint32_t wait_time_ms = 1;
uint32_t now = AP_HAL::millis();
bool ret = false;
// if we are about to get another frame from X-Plane then wait longer
if (xplane_frame_time > wait_time_ms &&
now+1 >= last_data_time_ms + xplane_frame_time) {
wait_time_ms = 10;
}
ssize_t len = socket_in.recv(pkt, sizeof(pkt), wait_time_ms);
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
::printf("PACKET: %4.4s\n", (const char *)pkt);
goto failed;
}
len -= 5;
if (len < pkt_len) {
// bad packet
goto failed;
}
if (!connected) {
// we now know the IP X-Plane is using
uint16_t port;
socket_in.last_recv_address(xplane_ip, port);
socket_out.connect(xplane_ip, xplane_port);
connected = true;
printf("Connected to %s:%u\n", xplane_ip, (unsigned)xplane_port);
}
while (len >= pkt_len) {
const float *data = (const float *)p;
uint8_t code = p[0];
int8_t idx = find_data_index(code);
if (idx == -1) {
deselect_code(code);
len -= pkt_len;
p += pkt_len;
continue;
}
seen_mask |= (1U<<idx);
switch (code) {
case Times: {
uint64_t tus = data[3] * 1.0e6f;
if (tus + time_base_us <= time_now_us) {
uint64_t tdiff = time_now_us - (tus + time_base_us);
if (tdiff > 1e6f) {
printf("X-Plane time reset %lu\n", (unsigned long)tdiff);
}
time_base_us = time_now_us - tus;
}
uint64_t tnew = time_base_us + tus;
//uint64_t dt = tnew - time_now_us;
//printf("dt %u\n", (unsigned)dt);
time_now_us = tnew;
break;
}
case LatLonAlt: {
loc.lat = data[1] * 1e7;
loc.lng = data[2] * 1e7;
loc.alt = data[3] * FEET_TO_METERS * 100.0f;
const float altitude_above_ground = data[4] * FEET_TO_METERS;
ground_level = loc.alt * 0.01f - altitude_above_ground;
break;
}
case Speed:
airspeed = data[2] * KNOTS_TO_METERS_PER_SECOND;
airspeed_pitot = airspeed;
break;
case AoA:
// ignored
break;
case PitchRollHeading: {
float roll, pitch, yaw;
pitch = radians(data[1]);
roll = radians(data[2]);
yaw = radians(data[3]);
dcm.from_euler(roll, pitch, yaw);
break;
}
case AtmosphereWeather:
// ignored
break;
case LocVelDistTraveled:
pos.y = data[1];
pos.z = -data[2];
pos.x = -data[3];
velocity_ef.y = data[4];
velocity_ef.z = -data[5];
velocity_ef.x = -data[6];
break;
case AngularVelocities:
if (is_xplane12()) {
gyro.x = radians(data[1]);
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;
case Gload:
accel_body.z = -data[5] * GRAVITY_MSS;
accel_body.x = data[6] * GRAVITY_MSS;
accel_body.y = data[7] * GRAVITY_MSS;
break;
case PropPitch: {
break;
}
case EngineRPM:
rpm[0] = data[1];
motor_mask |= 1;
break;
case PropRPM:
rpm[1] = data[1];
motor_mask |= 2;
break;
case JoystickRaw: {
for (auto *j = joyinputs; j; j=j->next) {
switch (j->type) {
case JoyType::AXIS: {
if (j->axis >= 1 && j->axis <= 6) {
float v = (data[j->axis] - j->input_min) / (j->input_max - j->input_min);
rcin[j->channel-1] = v;
rcin_chan_count = MAX(rcin_chan_count, j->channel);
}
break;
}
case JoyType::BUTTON: {
uint32_t m = uint32_t(data[7]) & j->mask;
float v = 0;
if (m == 0) {
v = 0;
} else if (1U<<(__builtin_ffs(j->mask)-1) != m) {
v = 0.5;
} else {
v = 1;
}
rcin[j->channel-1] = v;
rcin_chan_count = MAX(rcin_chan_count, j->channel);
break;
}
}
}
}
}
len -= pkt_len;
p += pkt_len;
}
// update data selection
select_data();
position = pos + position_zero;
position.xy() += origin.get_distance_NE_double(home);
update_position();
time_advance();
accel_earth = dcm * accel_body;
accel_earth.z += GRAVITY_MSS;
// the position may slowly deviate due to float accuracy and longitude scaling
if (loc.get_distance(location) > 4 || abs(loc.alt - location.alt)*0.01f > 2.0f) {
printf("X-Plane home reset dist=%f alt=%.1f/%.1f\n",
loc.get_distance(location), loc.alt*0.01f, location.alt*0.01f);
// reset home location
position_zero = {-pos.x, -pos.y, -pos.z};
home.lat = loc.lat;
home.lng = loc.lng;
home.alt = loc.alt;
origin = home;
position.x = 0;
position.y = 0;
position.z = 0;
update_position();
time_advance();
}
update_mag_field_bf();
if (now > last_data_time_ms && now - last_data_time_ms < 100) {
xplane_frame_time = now - last_data_time_ms;
}
last_data_time_ms = AP_HAL::millis();
if (ret) {
report.data_count++;
report.frame_count++;
}
return ret;
failed:
if (AP_HAL::millis() - last_data_time_ms > 200) {
// don't extrapolate beyond 0.2s
return false;
}
// advance time by 1ms
frame_time_us = 1000;
float delta_time = frame_time_us * 1e-6f;
time_now_us += frame_time_us;
extrapolate_sensors(delta_time);
update_position();
time_advance();
update_mag_field_bf();
report.frame_count++;
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 DRef data to X-Plane via UDP
*/
void XPlane::send_drefs(const struct sitl_input &input)
{
for (const auto *d = drefs; d; d=d->next) {
switch (d->type) {
case DRefType::ANGLE: {
float v = d->range * (input.servos[d->channel-1]-1500)/500.0;
send_dref(d->name, v);
break;
}
case DRefType::RANGE: {
float v = d->range * (input.servos[d->channel-1]-1000)/1000.0;
send_dref(d->name, v);
break;
}
case DRefType::FIXED: {
send_dref(d->name, d->fixed_value);
break;
}
}
}
}
/*
send DREF to X-Plane via UDP
*/
void XPlane::send_dref(const char *name, float value)
{
struct PACKED {
uint8_t marker[5] { 'D', 'R', 'E', 'F', '0' };
float value;
char name[500];
} d {};
d.value = value;
strcpy(d.name, name);
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);
}
/*
update the XPlane simulation by one time step
*/
void XPlane::update(const struct sitl_input &input)
{
if (receive_data()) {
send_drefs(input);
}
uint32_t now = AP_HAL::millis();
if (report.last_report_ms == 0) {
report.last_report_ms = now;
request_drefs();
}
if (now - report.last_report_ms > 5000) {
float dt = (now - report.last_report_ms) * 1.0e-3f;
printf("Data rate: %.1f FPS Frame rate: %.1f FPS\n",
report.data_count/dt, report.frame_count/dt);
report.last_report_ms = now;
report.data_count = 0;
report.frame_count = 0;
request_drefs();
}
check_reload_dref();
}
#endif // HAL_SIM_XPLANE_ENABLED
Reference in New Issue View Git Blame Copy Permalink