Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/SITL/SIM_XPlane.cpp

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