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

Tools/autotest/models/xplane_heli.json

@@ -2,7 +2,7 @@
 # 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_flightcontrol" : { "type" : "fixed", "value" : 0 },

libraries/SITL/SIM_Aircraft.cpp

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

libraries/SITL/SIM_Aircraft.h

@@ -37,6 +37,10 @@
 #include <Filter/Filter.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 {
 
 /*

libraries/SITL/SIM_Frame.cpp

@@ -20,11 +20,11 @@
 #include <AP_Motors/AP_Motors.h>
 #include <AP_Baro/AP_Baro.h>
 #include <AP_Filesystem/AP_Filesystem.h>
+#include "SIM_Aircraft.h"
 
 #include <stdio.h>
 #include <sys/stat.h>
 
-
 using namespace SITL;
 
 static Motor quad_plus_motors[] =
@@ -341,40 +341,10 @@ void Frame::load_frame_params(const char *model_json)
     if (fname == nullptr) {
         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);
     }
-    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 {
         FLOAT,
@@ -415,7 +385,7 @@ void Frame::load_frame_params(const char *model_json)
     };
 
     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>()) {
             // use default value
             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 j=0; j<12; j++) {
             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>()) {
                 // use default value
                 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);
 }
 
@@ -473,10 +445,11 @@ void Frame::parse_vector3(picojson::value val, const char* label, Vector3f &para
 
 #endif
 
+#if AP_SIM_ENABLED
+
 /*
   initialise the frame
  */
-#if AP_SIM_ENABLED
 void Frame::init(const char *frame_str, Battery *_battery)
 {
     model = default_model;

libraries/SITL/SIM_Frame.h

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

libraries/SITL/SIM_GPS.cpp

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

libraries/SITL/SIM_XPlane.cpp

@@ -28,14 +28,68 @@
 #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;
 
-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) :
     Aircraft(frame_str)
@@ -46,79 +100,248 @@ XPlane::XPlane(const char *frame_str) :
         xplane_ip = colon+1;
     }
 
-    heli_frame = (strstr(frame_str, "-heli") != nullptr);
-
     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(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 {
         uint8_t  marker[5] { 'D', 'S', 'E', 'L', '0' };
         uint32_t data[8] {};
     } dsel;
     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) {
         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 {
         uint8_t  marker[5] { 'U', 'S', 'E', 'L', '0' };
         uint32_t data[8] {};
     } 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
+  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;
-    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 {};
     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 &&
@@ -127,12 +350,29 @@ bool XPlane::receive_data(void)
     }
     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
+        ::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;
@@ -145,10 +385,15 @@ bool XPlane::receive_data(void)
     while (len >= pkt_len) {
         const float *data = (const float *)p;
         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) {
         case Times: {
             uint64_t tus = data[3] * 1.0e6f;
@@ -184,13 +429,6 @@ bool XPlane::receive_data(void)
             // ignored
             break;
 
-        case Trim:
-            if (heli_frame) {
-                // use flaps for collective as no direct collective data input
-                rcin[2] = data[4];
-            }
-            break;
-            
         case PitchRollHeading: {
             float roll, pitch, yaw;
             pitch = radians(data[1]);
@@ -214,9 +452,17 @@ bool XPlane::receive_data(void)
             break;
 
         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;
 
         case Gload:
@@ -225,84 +471,56 @@ bool XPlane::receive_data(void)
             accel_body.y = data[7] * GRAVITY_MSS;
             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: {
             break;
         }
 
         case EngineRPM:
             rpm[0] = data[1];
+            motor_mask |= 1;
             break;
 
         case PropRPM:
             rpm[1] = data[1];
+            motor_mask |= 2;
             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;
         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.xy() += origin.get_distance_NE_double(home);
     update_position();
@@ -335,9 +553,12 @@ bool XPlane::receive_data(void)
     }
     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:
     if (AP_HAL::millis() - last_data_time_ms > 200) {
@@ -359,94 +580,57 @@ failed:
     report.frame_count++;
     return false;
 }
-    
 
 /*
-  send data to X-Plane via UDP
+  receive RREF replies
 */
-void XPlane::send_data(const struct sitl_input &input)
+void XPlane::handle_rref(const uint8_t *pkt, uint32_t len)
 {
-    float aileron  = (input.servos[0]-1500)/500.0f;
+    const uint8_t *p = &pkt[5];
-    float elevator = (input.servos[1]-1500)/500.0f;
+    const struct PACKED RRefPacket {
-    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];
+        union PACKED {
-    } d {};
+            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;
+    }
+}
 
-    if (input.servos[0] == 0) {
+
-        aileron = 0;
+/*
-    }
+  send DRef data to X-Plane via UDP
-    if (input.servos[1] == 0) {
+*/
-        elevator = 0;
+void XPlane::send_drefs(const struct sitl_input &input)
-    }
+{
-    if (input.servos[2] == 0) {
+    for (const auto *d = drefs; d; d=d->next) {
-        throttle = 0;
+        switch (d->type) {
-    }
+
-    if (input.servos[3] == 0) {
+        case DRefType::ANGLE: {
-        rudder = 0;
+            float v  = d->range * (input.servos[d->channel-1]-1500)/500.0;
-    }
+            send_dref(d->name, v);
-    
+            break;
-    // we add the throttle_magic to the throttle value we send so we
+        }
-    // can detect when we get it back
+
-    throttle = ((uint32_t)(throttle * 1000)) * 1.0e-3f + throttle_magic;
+        case DRefType::RANGE: {
-    
+            float v  = d->range * (input.servos[d->channel-1]-1000)/1000.0;
-    uint8_t flap_chan;
+            send_dref(d->name, v);
-    if (SRV_Channels::find_channel(SRV_Channel::k_flap, flap_chan) ||
+            break;
-        SRV_Channels::find_channel(SRV_Channel::k_flap_auto, flap_chan)) {
+        }
-        float flap = (input.servos[flap_chan]-1000)/1000.0;
+
-        if (!is_equal(flap, last_flap)) {
+        case DRefType::FIXED: {
-            send_dref("sim/flightmodel/controls/flaprqst", flap);
+            send_dref(d->name, d->fixed_value);
-            send_dref("sim/aircraft/overflow/acf_flap_arm", flap>0?1:0);
+            break;
+        }
         }
     }
-
-    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;
 }
 
 
@@ -462,21 +646,47 @@ void XPlane::send_dref(const char *name, float value)
     } d {};
     d.value = value;
     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);
+}
+
 /*
   update the XPlane simulation by one time step
  */
 void XPlane::update(const struct sitl_input &input)
 {
     if (receive_data()) {
-        send_data(input);
+        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;
@@ -485,9 +695,9 @@ void XPlane::update(const struct sitl_input &input)
         report.last_report_ms = now;
         report.data_count = 0;
         report.frame_count = 0;
+        request_drefs();
     }
+    check_reload_dref();
 }
 
-} // namespace SITL
-
 #endif  // HAL_SIM_XPLANE_ENABLED

libraries/SITL/SIM_XPlane.h

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

libraries/SITL/examples/EvaluateBatteryModel/wscript

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

libraries/SITL/examples/EvaluateMotorModel/wscript

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

libraries/SITL/examples/XPlane/xplane_alia.json

@@ -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 }
+}

libraries/SITL/picojson.cpp

@@ -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

libraries/SITL/picojson.h

@@ -31,6 +31,10 @@
  * 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
  */