diff --git a/ground/src/plugins/hitlnew/fgsimulator.cpp b/ground/src/plugins/hitlnew/fgsimulator.cpp index 7bb3a7f92..24d265c62 100644 --- a/ground/src/plugins/hitlnew/fgsimulator.cpp +++ b/ground/src/plugins/hitlnew/fgsimulator.cpp @@ -139,7 +139,6 @@ void FGSimulator::transmitUpdate() float elevator = -actData.Pitch; float rudder = actData.Yaw; float throttle = actData.Throttle; - // Send update to FlightGear QString cmd; cmd = QString("%1,%2,%3,%4\n") @@ -152,9 +151,10 @@ void FGSimulator::transmitUpdate() } -void FGSimulator::processUpdate(QString& data) +void FGSimulator::processUpdate(const QByteArray& inp) { // Split + QString data(inp); QStringList fields = data.split(","); // Get xRate (deg/s) // float xRate = fields[0].toFloat() * 180.0/M_PI; diff --git a/ground/src/plugins/hitlnew/fgsimulator.h b/ground/src/plugins/hitlnew/fgsimulator.h index 770bb22f3..31583b549 100644 --- a/ground/src/plugins/hitlnew/fgsimulator.h +++ b/ground/src/plugins/hitlnew/fgsimulator.h @@ -51,7 +51,7 @@ private: static const float KT2MPS; static const float INHG2KPA; - void processUpdate(QString& data); + void processUpdate(const QByteArray& data); }; class FGSimulatorCreator : public SimulatorCreator diff --git a/ground/src/plugins/hitlnew/hitlnew.pro b/ground/src/plugins/hitlnew/hitlnew.pro index fd3dde820..70e46e939 100644 --- a/ground/src/plugins/hitlnew/hitlnew.pro +++ b/ground/src/plugins/hitlnew/hitlnew.pro @@ -11,7 +11,8 @@ HEADERS += hitlplugin.h \ hitlgadget.h \ simulator.h \ fgsimulator.h \ - il2simulator.h + il2simulator.h \ + xplanesimulator.h SOURCES += hitlplugin.cpp \ hitlwidget.cpp \ hitloptionspage.cpp \ @@ -20,7 +21,8 @@ SOURCES += hitlplugin.cpp \ hitlgadget.cpp \ simulator.cpp \ il2simulator.cpp \ - fgsimulator.cpp + fgsimulator.cpp \ + xplanesimulator.cpp OTHER_FILES += hitlnew.pluginspec FORMS += hitloptionspage.ui \ hitlwidget.ui diff --git a/ground/src/plugins/hitlnew/hitlplugin.cpp b/ground/src/plugins/hitlnew/hitlplugin.cpp index 70f6a0e26..dbfb27ba5 100644 --- a/ground/src/plugins/hitlnew/hitlplugin.cpp +++ b/ground/src/plugins/hitlnew/hitlplugin.cpp @@ -31,6 +31,7 @@ #include #include "fgsimulator.h" #include "il2simulator.h" +#include "xplanesimulator.h" QList HITLPlugin::typeSimulators; @@ -54,6 +55,7 @@ bool HITLPlugin::initialize(const QStringList& args, QString *errMsg) addSimulator(new FGSimulatorCreator("FG","FlightGear")); addSimulator(new IL2SimulatorCreator("IL2","IL2")); + addSimulator(new XplaneSimulatorCreator("X-Plane","X-Plane")); return true; } diff --git a/ground/src/plugins/hitlnew/il2simulator.cpp b/ground/src/plugins/hitlnew/il2simulator.cpp index f43c486cf..bd7aa6c53 100644 --- a/ground/src/plugins/hitlnew/il2simulator.cpp +++ b/ground/src/plugins/hitlnew/il2simulator.cpp @@ -59,6 +59,8 @@ * * unfortunately angular acceleration provided is very limited, too */ + + #include "il2simulator.h" #include "extensionsystem/pluginmanager.h" #include @@ -150,11 +152,11 @@ float IL2Simulator::TAS(float IAS, float alt) { /** * process data string from flight simulator */ -void IL2Simulator::processUpdate(QString& data) +void IL2Simulator::processUpdate(const QByteArray& inp) { // save old flight data to calculate delta's later old=current; - + QString data(inp); // Split QStringList fields = data.split("/"); @@ -208,17 +210,17 @@ void IL2Simulator::processUpdate(QString& data) current.Y = old.Y + (current.dY*current.dT); // Update AltitudeActual object - BaroAltitude::DataFields altActualData; - memset(&altActualData, 0, sizeof(BaroAltitude::DataFields)); - altActualData.Altitude = current.Z; + BaroAltitude::DataFields altActualData; + memset(&altActualData, 0, sizeof(BaroAltitude::DataFields)); + altActualData.Altitude = current.Z; altActualData.Temperature = TEMP_GROUND + (current.Z * TEMP_LAPSE_RATE) - 273.0; altActualData.Pressure = PRESSURE(current.Z)/1000.0; // kpa altActual->setData(altActualData); // Update attActual object AttitudeActual::DataFields attActualData; - memset(&attActualData, 0, sizeof(AttitudeActual::DataFields)); - attActualData.Roll = current.roll; + memset(&attActualData, 0, sizeof(AttitudeActual::DataFields)); + attActualData.Roll = current.roll; attActualData.Pitch = current.pitch; attActualData.Yaw = current.azimuth; attActualData.q1 = 0; @@ -229,8 +231,8 @@ void IL2Simulator::processUpdate(QString& data) // Update gps objects PositionActual::DataFields gpsData; - memset(&gpsData, 0, sizeof(PositionActual::DataFields)); - gpsData.Altitude = current.Z; + memset(&gpsData, 0, sizeof(PositionActual::DataFields)); + gpsData.Altitude = current.Z; gpsData.Heading = current.azimuth; gpsData.Groundspeed = current.groundspeed; gpsData.Latitude = settings.latitude.toFloat() + current.Y * DEG2M; diff --git a/ground/src/plugins/hitlnew/il2simulator.h b/ground/src/plugins/hitlnew/il2simulator.h index 725fdefa1..d2382396f 100644 --- a/ground/src/plugins/hitlnew/il2simulator.h +++ b/ground/src/plugins/hitlnew/il2simulator.h @@ -63,7 +63,8 @@ private: float DENSITY(float pressure); float PRESSURE(float alt); float TAS(float ias,float alt); - void processUpdate(QString& data); + + void processUpdate(const QByteArray& data); }; diff --git a/ground/src/plugins/hitlnew/simulator.cpp b/ground/src/plugins/hitlnew/simulator.cpp index c07283fc5..086368361 100644 --- a/ground/src/plugins/hitlnew/simulator.cpp +++ b/ground/src/plugins/hitlnew/simulator.cpp @@ -191,9 +191,9 @@ void Simulator::receiveUpdate() quint16 senderPort; inSocket->readDatagram(datagram.data(), datagram.size(), &sender, &senderPort); - QString datastr(datagram); + //QString datastr(datagram); // Process incomming data - processUpdate(datastr); + processUpdate(datagram); } } diff --git a/ground/src/plugins/hitlnew/simulator.h b/ground/src/plugins/hitlnew/simulator.h index 31c50714c..c7eb4592d 100644 --- a/ground/src/plugins/hitlnew/simulator.h +++ b/ground/src/plugins/hitlnew/simulator.h @@ -134,7 +134,7 @@ private slots: Q_INVOKABLE void onDeleteSimulator(void); virtual void transmitUpdate() = 0; - virtual void processUpdate(QString& data) = 0; + virtual void processUpdate(const QByteArray& data) = 0; protected: QProcess* simProcess; diff --git a/ground/src/plugins/hitlnew/xplanesimulator.cpp b/ground/src/plugins/hitlnew/xplanesimulator.cpp new file mode 100644 index 000000000..e54aae6f2 --- /dev/null +++ b/ground/src/plugins/hitlnew/xplanesimulator.cpp @@ -0,0 +1,260 @@ +/** + ****************************************************************************** + * + * @file xplanesimulator.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 "xplanesimulator.h" +#include "extensionsystem/pluginmanager.h" +#include +#include +#include +#include + +void TraceBuf(const char* buf,int len); + +XplaneSimulator::XplaneSimulator(const SimulatorSettings& params) : + Simulator(params) +{ +} + + +XplaneSimulator::~XplaneSimulator() +{ +} + +void XplaneSimulator::setupUdpPorts(const QString& host, int inPort, int outPort) +{ + inSocket->bind(QHostAddress(host), inPort); + outSocket->bind(QHostAddress(host), outPort); +} + +/** + * update data in X-Plane simulator + */ +void XplaneSimulator::transmitUpdate() +{ + //Read ActuatorDesired from autopilot + ActuatorDesired::DataFields actData = actDesired->getData(); + float ailerons = actData.Roll; + float elevator = actData.Pitch; + float rudder = actData.Yaw; + float throttle = actData.Throttle*2-1.0; + float tmp = -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; + TraceBuf(buf.data(),41); + 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; + 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 XplaneSimulator::processUpdate(const QByteArray& dataBuf) +{ + float altitude = 0; + float latitude = 0; + float longitude = 0; + float airspeed = 0; + float speed = 0; + float pitch = 0; + float roll = 0; + float heading = 0; + float pressure = 0; + float weather = 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) + { + qxtLog->info("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 XplaneSimulator::LatitudeLongitude: + latitude = *((float*)(buf.data()+4*1)); + longitude = *((float*)(buf.data()+4*2)); + altitude = *((float*)(buf.data()+4*3)) /* * 3.048 */; + break; + + case XplaneSimulator::Speed: + airspeed = *((float*)(buf.data()+4*6)); + speed = *((float*)(buf.data()+4*7)); + break; + + case XplaneSimulator::PitchRollHeading: + pitch = *((float*)(buf.data()+4*1)); + roll = *((float*)(buf.data()+4*2)); + heading = *((float*)(buf.data()+4*3)); + break; + + case XplaneSimulator::SystemPressures: + pressure = *((float*)(buf.data()+4*1)); + break; + + case XplaneSimulator::AtmosphereWeather: + weather = *((float*)(buf.data()+4*1)); + break; + + default: + break; + } + channelCounter--; + buf.remove(0,36); + } while (channelCounter); + + // Update AltitudeActual object + BaroAltitude::DataFields altActualData; + memset(&altActualData, 0, sizeof(BaroAltitude::DataFields)); + altActualData.Altitude = altitude; + altActualData.Temperature = weather; + altActualData.Pressure = pressure; + altActual->setData(altActualData); + + // Update attActual object + AttitudeActual::DataFields attActualData; + memset(&attActualData, 0, sizeof(AttitudeActual::DataFields)); + attActualData.Roll = roll; //roll; + attActualData.Pitch = pitch; // pitch + // attActualData.Yaw = yaw; + // attActualData.q1 = 0; + // attActualData.q2 = 0; + // attActualData.q3 = 0; + // attActualData.q4 = 0; + attActual->setData(attActualData); + + // Update gps objects + PositionActual::DataFields gpsData; + memset(&gpsData, 0, sizeof(PositionActual::DataFields)); + gpsData.Altitude = altitude; +// gpsData.Heading = pitch[2]; +// gpsData.Groundspeed = speed[0]; + gpsData.Latitude = latitude; + gpsData.Longitude = longitude; + posActual->setData(gpsData); + } + // issue manual update + //attActual->updated(); + //altActual->updated(); + //posActual->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/src/plugins/hitlnew/xplanesimulator.h b/ground/src/plugins/hitlnew/xplanesimulator.h new file mode 100644 index 000000000..c5c468fa3 --- /dev/null +++ b/ground/src/plugins/hitlnew/xplanesimulator.h @@ -0,0 +1,90 @@ +/** + ****************************************************************************** + * + * @file xplanesimulator.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 XPLANESIMULATOR_H +#define XPLANESIMULATOR_H + +#include +#include + +class XplaneSimulator: public Simulator +{ + Q_OBJECT +public: + XplaneSimulator(const SimulatorSettings& params); + ~XplaneSimulator(); + + void setupUdpPorts(const QString& host, int inPort, int outPort); + +private slots: + void transmitUpdate(); + +private: + enum XplaneOutputData + { + FramRate, + Times, + SimStats, + Speed, + Gload, + AtmosphereWeather, + AtmosphereAircraft, + SystemPressures, + Joystick1, + Joystick2, + ArtStab, + FlightCon, + WingSweep, + Trim, + Brakes, + AngularMoments, + AngularAccelerations, + AngularVelociies, + PitchRollHeading, + AoA, + LatitudeLongitude, + LocVelDistTraveled + }; + + void processUpdate(const QByteArray& data); + +}; + +class XplaneSimulatorCreator : public SimulatorCreator +{ +public: + XplaneSimulatorCreator(const QString& classId, const QString& description) + : SimulatorCreator (classId,description) + {} + + Simulator* createSimulator(const SimulatorSettings& params) + { + return new XplaneSimulator(params); + } +}; + +#endif // XPLANESIMULATOR_H