From 0f9218d3afff5da2186d15591b512afb96b985a1 Mon Sep 17 00:00:00 2001 From: Kevin Finisterre Date: Mon, 27 Apr 2015 15:45:45 -0400 Subject: [PATCH] X-Plane 10 HITL & SITL protocol changes that should have been in place long ago! --- .../src/plugins/hitl/hitlnoisegeneration.h | 3 +- .../src/plugins/hitl/hitlplugin.cpp | 3 +- .../openpilotgcs/src/plugins/hitl/plugin.pro | 6 +- .../src/plugins/hitl/xplanesimulator10.cpp | 360 ++++++++++++++++++ .../src/plugins/hitl/xplanesimulator10.h | 95 +++++ ...lanesimulator.cpp => xplanesimulator9.cpp} | 0 .../{xplanesimulator.h => xplanesimulator9.h} | 16 +- 7 files changed, 471 insertions(+), 12 deletions(-) create mode 100644 ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.cpp create mode 100644 ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.h rename ground/openpilotgcs/src/plugins/hitl/{xplanesimulator.cpp => xplanesimulator9.cpp} (100%) rename ground/openpilotgcs/src/plugins/hitl/{xplanesimulator.h => xplanesimulator9.h} (88%) diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h index 22df94b72..1e828f450 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h +++ b/ground/openpilotgcs/src/plugins/hitl/hitlnoisegeneration.h @@ -30,7 +30,8 @@ // #include // #include -#include "xplanesimulator.h" +#include "xplanesimulator9.h" +#include "xplanesimulator10.h" #include "hitlnoisegeneration.h" #include "extensionsystem/pluginmanager.h" #include diff --git a/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp b/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp index 0ce9e9141..adefb920a 100644 --- a/ground/openpilotgcs/src/plugins/hitl/hitlplugin.cpp +++ b/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 HITLPlugin::typeSimulators; diff --git a/ground/openpilotgcs/src/plugins/hitl/plugin.pro b/ground/openpilotgcs/src/plugins/hitl/plugin.pro index 066c60ec0..3eb167d74 100644 --- a/ground/openpilotgcs/src/plugins/hitl/plugin.pro +++ b/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 diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.cpp new file mode 100644 index 000000000..6ca79f4f4 --- /dev/null +++ b/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 +#include +#include + +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(&code), sizeof(code)); + buf.append(reinterpret_cast(&elevator), sizeof(elevator)); + buf.append(reinterpret_cast(&ailerons), sizeof(ailerons)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&rudder), sizeof(rudder)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&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(&code), sizeof(code)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&throttle), sizeof(throttle)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&none), sizeof(none)); + buf.append(reinterpret_cast(&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(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; + } +} diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.h b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator10.h new file mode 100644 index 000000000..7a7632296 --- /dev/null +++ b/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 +#include + +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 diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator9.cpp similarity index 100% rename from ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp rename to ground/openpilotgcs/src/plugins/hitl/xplanesimulator9.cpp diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator9.h similarity index 88% rename from ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h rename to ground/openpilotgcs/src/plugins/hitl/xplanesimulator9.h index b1c33f36f..f785e425b 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.h +++ b/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 #include -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