X-Plane 10 HITL & SITL protocol changes that should have been in place long ago!

ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h

@@ -30,7 +30,8 @@
 
 // #include <QObject>
 // #include <simulator.h>
-#include "xplanesimulator.h"
+#include "xplanesimulator9.h"
+#include "xplanesimulator10.h"
 #include "hitlnoisegeneration.h"
 #include "extensionsystem/pluginmanager.h"
 #include <coreplugin/icore.h>

ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp

@@ -32,7 +32,8 @@
 #include "aerosimrcsimulator.h"
 #include "fgsimulator.h"
 #include "il2simulator.h"
-#include "xplanesimulator.h"
+#include "xplanesimulator9.h"
+#include "xplanesimulator10.h"
 
 QList<SimulatorCreator * > HITLPlugin::typeSimulators;
 

ground/openpilotgcs/src/plugins/hitl/plugin.pro

@@ -16,7 +16,8 @@ HEADERS += hitlplugin.h \
     aerosimrcsimulator.h \
     fgsimulator.h \
     il2simulator.h \
-    xplanesimulator.h
+    xplanesimulator9.h
+    xplanesimulator10.h
 SOURCES += hitlplugin.cpp \
     hitlwidget.cpp \
     hitloptionspage.cpp \
@@ -28,7 +29,8 @@ SOURCES += hitlplugin.cpp \
     aerosimrcsimulator.cpp \
     fgsimulator.cpp \
     il2simulator.cpp \
-    xplanesimulator.cpp
+    xplanesimulator9.cpp
+    xplanesimulator10.cpp
 OTHER_FILES += hitl.pluginspec
 FORMS += hitloptionspage.ui \
     hitlwidget.ui

ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.cpp

@@ -0,0 +1,360 @@
+/**
+ ******************************************************************************
+ *
+ * @file       xplanesimulator10.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief
+ * @see        The GNU Public License (GPL) Version 3
+ * @defgroup   hitlplugin
+ * @{
+ *
+ *****************************************************************************/
+/*
+ * 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, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**
+ * Description of X-Plane Protocol:
+ *
+ * To see what data can be sended/recieved to/from X-Plane, launch X-Plane -> goto main menu
+ * (cursor at top of main X-Plane window) -> Settings -> Data Input and Output -> Data Set.
+ * Data Set shown all X-Plane params,
+ * each row has four checkbox: 1st check - out to UDP; 4 check - show on screen
+ * All the UDP messages for X-Plane have the same format, which is:
+ * 5-character MESSAGE PROLOUGE (to indicate the type of message)
+ * and then a DATA INPUT STRUCTURE (containing the message data that you want to send or receive)
+ *
+ * DATA INPUT/OUTPUT STRUCTURE is the following stuct:
+ *
+ *  struct data_struct
+ *  {
+ *      int index;     // data index, the index into the list of variables
+                       // you can output from the Data Output screen in X-Plane.
+ *      float data[8]; // the up to 8 numbers you see in the data output screen associated with that selection..
+                       // many outputs do not use all 8, though.
+ * };
+ *
+ * For Example, update of aileron/elevon/rudder in X-Plane (11 row in Data Set)
+ * bytes     value     description
+ * [0-3]     DATA      message type
+ * [4]       none      no matter
+ * [5-8]     11        code of setting param(row in Data Set)
+ * [9-41]    data      message data (8 float values)
+ * total size: 41 byte
+ *
+ */
+
+#include "xplanesimulator10.h"
+#include "extensionsystem/pluginmanager.h"
+#include <coreplugin/icore.h>
+#include <coreplugin/threadmanager.h>
+#include <math.h>
+
+void TraceBuf(const char *buf, int len);
+
+XplaneSimulator10::XplaneSimulator10(const SimulatorSettings & params) :
+    Simulator(params)
+{
+    resetInitialHomePosition();
+}
+
+
+XplaneSimulator10::~XplaneSimulator10()
+{}
+
+void XplaneSimulator10::setupUdpPorts(const QString & host, int inPort, int outPort)
+{
+    Q_UNUSED(outPort);
+
+    inSocket->bind(QHostAddress(host), inPort);
+    // outSocket->bind(QHostAddress(host), outPort);
+    resetInitialHomePosition();
+}
+
+bool XplaneSimulator10::setupProcess()
+{
+    emit processOutput(QString("Please start X-Plane 10 manually, and make sure it is setup to output its ") +
+                       "data to host " + settings.hostAddress + " UDP port " + QString::number(settings.inPort));
+
+    return true;
+}
+
+/**
+ * update data in X-Plane simulator
+ */
+void XplaneSimulator10::transmitUpdate()
+{
+    if (settings.manualControlEnabled) {
+        // Read ActuatorDesired from autopilot
+        ActuatorDesired::DataFields actData = actDesired->getData();
+        float ailerons = actData.Roll;
+        float elevator = actData.Pitch;
+        float rudder   = actData.Yaw;
+        float throttle = actData.Thrust > 0 ? actData.Thrust : 0;
+        float none     = -999;
+        // quint32 none = *((quint32*)&tmp); // get float as 4 bytes
+
+        quint32 code;
+        QByteArray buf;
+        QDataStream stream(&buf, QIODevice::ReadWrite);
+
+        // !!! LAN byte order - Big Endian
+#if Q_BYTE_ORDER == Q_LITTLE_ENDIAN
+        stream.setByteOrder(QDataStream::LittleEndian);
+#endif
+
+        // 11th data settings (flight con: ail/elv/rud)
+        buf.clear();
+        code = 11;
+        // quint8 header[] = "DATA";
+        /*
+           stream << *((quint32*)header) <<
+                  (quint8)0x30 <<
+                  code <<
+         *((quint32*)&elevator) <<
+         *((quint32*)&ailerons) <<
+         *((quint32*)&rudder)   <<
+                  none <<
+         *((quint32*)&ailerons) <<
+                  none <<
+                  none  <<
+                  none;
+         */
+        buf.append("DATA0");
+        buf.append(reinterpret_cast<const char *>(&code), sizeof(code));
+        buf.append(reinterpret_cast<const char *>(&elevator), sizeof(elevator));
+        buf.append(reinterpret_cast<const char *>(&ailerons), sizeof(ailerons));
+        buf.append(reinterpret_cast<const char *>(&rudder), sizeof(rudder));
+        buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
+        buf.append(reinterpret_cast<const char *>(&rudder), sizeof(rudder));
+        buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
+        buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
+        buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
+// TraceBuf(buf.data(),41);
+
+        if (outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) {
+            emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n");
+        }
+        // outSocket->write(buf);
+
+        // 25th data settings (throttle command)
+        buf.clear();
+        code = 25;
+        // stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none  << none
+        // << none  << none  << none << none  << none;
+        buf.append("DATA0");
+        buf.append(reinterpret_cast<const char *>(&code), sizeof(code));
+        buf.append(reinterpret_cast<const char *>(&throttle), sizeof(throttle));
+        buf.append(reinterpret_cast<const char *>(&throttle), sizeof(throttle));
+        buf.append(reinterpret_cast<const char *>(&throttle), sizeof(throttle));
+        buf.append(reinterpret_cast<const char *>(&throttle), sizeof(throttle));
+        buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
+        buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
+        buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
+        buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
+
+        if (outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) {
+            emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n");
+        }
+
+        // outSocket->write(buf);
+
+
+        /** !!! this settings was given from ardupilot X-Plane.pl, I comment them
+           but if it needed comment should be removed !!!
+
+           // 8th data settings (joystick 1 ail/elv/rud)
+           stream << "DATA0" << quint32(11) << elevator << ailerons << rudder
+            << float(-999) << float(-999) << float(-999) << float(-999) << float(-999);
+           outSocket->write(buf);
+         */
+    }
+}
+
+/**
+ * process data string from X-Plane simulator
+ */
+void XplaneSimulator10::processUpdate(const QByteArray & dataBuf)
+{
+    float altitude_msl     = 0;
+    float altitude_agl     = 0;
+    float latitude         = 0;
+    float longitude        = 0;
+    float airspeed_keas    = 0;
+    float groundspeed_ktgs = 0;
+    float pitch         = 0;
+    float roll          = 0;
+    float heading       = 0;
+    float pressure      = 0;
+    float temperature   = 0;
+    float velX          = 0;
+    float velY          = 0;
+    float velZ          = 0;
+    float dstX          = 0;
+    float dstY          = 0;
+    float dstZ          = 0;
+    float accX          = 0;
+    float accY          = 0;
+    float accZ          = 0;
+    float rollRate_rad  = 0;
+    float pitchRate_rad = 0;
+    float yawRate_rad   = 0;
+
+    // QString str;
+    QByteArray & buf    = const_cast<QByteArray &>(dataBuf);
+    QString data(buf);
+
+    if (data.left(4) == "DATA") { // check type of packet
+        buf.remove(0, 5);
+        if (dataBuf.size() % 36) {
+            qDebug() << ("incorrect length of UDP packet: " + buf);
+            return; // incorrect length of struct
+        }
+        // check correctness of data length, length must be multiple of (id_size+8*float_size)=4+8*4=36
+        int channelCounter = dataBuf.size() / 36;
+        do {
+            switch (buf[0]) { // switch by id
+            case XplaneSimulator10::LatitudeLongitudeAltitude:
+                latitude     = *((float *)(buf.data() + 4 * 1));
+                longitude    = *((float *)(buf.data() + 4 * 2));
+                altitude_msl = *((float *)(buf.data() + 4 * 3)) * FT2M;
+                altitude_agl = *((float *)(buf.data() + 4 * 4)) * FT2M;
+                break;
+
+            case XplaneSimulator10::Speed:
+                airspeed_keas    = *((float *)(buf.data() + 4 * 2));
+                groundspeed_ktgs = *((float *)(buf.data() + 4 * 4));
+                break;
+
+            case XplaneSimulator10::PitchRollHeading:
+                pitch   = *((float *)(buf.data() + 4 * 1));
+                roll    = *((float *)(buf.data() + 4 * 2));
+                heading = *((float *)(buf.data() + 4 * 3));
+                break;
+
+            /*
+                   case XplaneSimulator10::SystemPressures:
+               pressure = *((float*)(buf.data()+4*1));
+               break;
+             */
+
+            case XplaneSimulator10::AtmosphereWeather:
+                pressure    = *((float *)(buf.data() + 4 * 1)) * INHG2KPA;
+                temperature = *((float *)(buf.data() + 4 * 2));
+                break;
+
+            case XplaneSimulator10::LocVelDistTraveled:
+                dstX = *((float *)(buf.data() + 4 * 1));
+                dstY = -*((float *)(buf.data() + 4 * 3));
+                dstZ = *((float *)(buf.data() + 4 * 2));
+                velX = *((float *)(buf.data() + 4 * 4));
+                velY = -*((float *)(buf.data() + 4 * 6));
+                velZ = *((float *)(buf.data() + 4 * 5));
+                break;
+
+            case XplaneSimulator10::AngularVelocities: // In [rad/s]
+                pitchRate_rad = *((float *)(buf.data() + 4 * 1));
+                rollRate_rad  = *((float *)(buf.data() + 4 * 2));
+                yawRate_rad   = *((float *)(buf.data() + 4 * 3));
+                break;
+
+            case XplaneSimulator10::Gload:
+                accX = *((float *)(buf.data() + 4 * 6)) * GEE;
+                accY = *((float *)(buf.data() + 4 * 7)) * GEE;
+                accZ = *((float *)(buf.data() + 4 * 5)) * GEE;
+                break;
+
+            default:
+                break;
+            }
+            channelCounter--;
+            buf.remove(0, 36);
+        } while (channelCounter);
+
+
+        ///////
+        // Output formatting
+        ///////
+        Output2Hardware out;
+        memset(&out, 0, sizeof(Output2Hardware));
+
+        // Update GPS Position objects
+        out.latitude    = latitude * 1e7;
+        out.longitude   = longitude * 1e7;
+        out.altitude    = altitude_msl;
+        out.agl = altitude_agl;
+        out.groundspeed = groundspeed_ktgs * 1.15 * 1.6089 / 3.6; // Convert from [kts] to [m/s]
+
+        out.calibratedAirspeed = airspeed_keas * 1.15 * 1.6089 / 3.6; // Convert from [kts] to [m/s]
+
+        // Update BaroSensor object
+        out.temperature = temperature;
+        out.pressure    = pressure;
+
+        // Update attState object
+        out.roll      = roll;       // roll;
+        out.pitch     = pitch;     // pitch
+        out.heading   = heading; // yaw
+
+
+        out.dstN      = dstY;
+        out.dstE      = dstX;
+        out.dstD      = -dstZ;
+
+        // Update VelocityState.{North,East,Down}
+        out.velNorth  = velY;
+        out.velEast   = velX;
+        out.velDown   = -velZ;
+
+        // Update gyroscope sensor data - convert from rad/s to deg/s
+        out.rollRate  = rollRate_rad * (180.0 / M_PI);
+        out.pitchRate = pitchRate_rad * (180.0 / M_PI);
+        out.yawRate   = yawRate_rad * (180.0 / M_PI);
+
+        // Update accelerometer sensor data
+        out.accX      = accX;
+        out.accY      = accY;
+        out.accZ      = -accZ;
+
+        updateUAVOs(out);
+    }
+    // issue manual update
+    // attState->updated();
+    // altState->updated();
+    // posState->updated();
+}
+
+
+void TraceBuf(const char *buf, int len)
+{
+    QString str;
+    bool reminder = true;
+
+    for (int i = 0; i < len; i++) {
+        if (!(i % 16)) {
+            if (i > 0) {
+                qDebug() << str;
+                str.clear();
+                reminder = false;
+            }
+            reminder = true;
+        }
+        str += QString(" 0x%1").arg((quint8)buf[i], 2, 16, QLatin1Char('0'));
+    }
+
+    if (reminder) {
+        qDebug() << str;
+    }
+}

ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.h

@@ -0,0 +1,95 @@
+/**
+ ******************************************************************************
+ *
+ * @file       xplanesimulator10.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief
+ * @see        The GNU Public License (GPL) Version 3
+ * @defgroup   hitlplugin
+ * @{
+ *
+ *****************************************************************************/
+/*
+ * 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, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef XPLANESIMULATOR10_H
+#define XPLANESIMULATOR10_H
+
+#include <QObject>
+#include <simulator.h>
+
+class XplaneSimulator10 : public Simulator {
+    Q_OBJECT
+public:
+    XplaneSimulator10(const SimulatorSettings & params);
+    ~XplaneSimulator10();
+    bool setupProcess();
+
+    void setupUdpPorts(const QString & host, int inPort, int outPort);
+
+private slots:
+    void transmitUpdate();
+
+private:
+    enum XplaneOutputData // ***WARNING***: Elements in this enum are in a precise order, do
+    { // not change. Cf. http://www.nuclearprojects.com/xplane/info.shtml (outdated version 9 info)
+	// These fields have been updated for X-Plane version 10.x
+	/* 0 */ FramRate,
+	/* 1 */ Times,
+	/* 2 */ SimStats,
+	/* 3 */ Speed,
+	/* 4 */ Gload,
+	/* 5 */ AtmosphereWeather,
+	/* 6 */ AtmosphereAircraft,
+	/* 7 */ SystemPressures,
+	/* 8 */ Joystick1,
+	/* 9 */ Joystick2,
+	/* 10 */ ArtStab,
+	/* 11 */ FlightCon,
+	/* 12 */ WingSweep,
+	/* 13 */ Trim,
+	/* 14 */ Brakes,
+	/* 15 */ AngularMoments,
+	/* 16 */ AngularVelocities,
+	/* 17 */ PitchRollHeading,
+	/* 18 */ AoA,
+	/* 19 */ MagCompass,
+	/* 20 */ LatitudeLongitude,
+	/* 21 */ LocVelDistTraveled,
+	/* 22 */ AllPlanesLat,
+	/* 23 */ AllPlanesLon,
+	/* 24 */ AllPlanesAlt,
+	/* 25 */ ThrottleCommand
+	/* .. */
+
+    };
+
+    void processUpdate(const QByteArray & data);
+};
+
+class XplaneSimulatorCreator10 : public SimulatorCreator {
+public:
+    XplaneSimulatorCreator10(const QString & classId, const QString & description)
+        :  SimulatorCreator(classId, description)
+    {}
+
+    Simulator *createSimulator(const SimulatorSettings & params)
+    {
+        return new XplaneSimulator10(params);
+    }
+};
+
+#endif // XPLANESIMULATOR10_H

ground/openpilotgcs/src/plugins/hitl/xplanesimulator9.h

@@ -1,7 +1,7 @@
 /**
  ******************************************************************************
  *
- * @file       xplanesimulator.h
+ * @file       xplanesimulator9.h
  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  * @brief
  * @see        The GNU Public License (GPL) Version 3
@@ -25,17 +25,17 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#ifndef XPLANESIMULATOR_H
-#define XPLANESIMULATOR_H
+#ifndef XPLANESIMULATOR9_H
+#define XPLANESIMULATOR9_H
 
 #include <QObject>
 #include <simulator.h>
 
-class XplaneSimulator : public Simulator {
+class XplaneSimulator9 : public Simulator {
     Q_OBJECT
 public:
-    XplaneSimulator(const SimulatorSettings & params);
-    ~XplaneSimulator();
+    XplaneSimulator9(const SimulatorSettings & params);
+    ~XplaneSimulator9();
     bool setupProcess();
 
     void setupUdpPorts(const QString & host, int inPort, int outPort);
@@ -81,8 +81,8 @@ public:
 
     Simulator *createSimulator(const SimulatorSettings & params)
     {
-        return new XplaneSimulator(params);
+        return new XplaneSimulator9(params);
     }
 };
 
-#endif // XPLANESIMULATOR_H
+#endif // XPLANESIMULATOR9_H