/** ****************************************************************************** * * @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 void TraceBuf(const char *buf, int len); XplaneSimulator::XplaneSimulator(const SimulatorSettings & params) : Simulator(params) { resetInitialHomePosition(); } XplaneSimulator::~XplaneSimulator() {} void XplaneSimulator::setupUdpPorts(const QString & host, int inPort, int outPort) { Q_UNUSED(outPort); inSocket->bind(QHostAddress(host), inPort); // outSocket->bind(QHostAddress(host), outPort); resetInitialHomePosition(); } bool XplaneSimulator::setupProcess() { emit processOutput(QString("Please start X-Plane 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 XplaneSimulator::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 XplaneSimulator::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 XplaneSimulator::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 XplaneSimulator::Speed: airspeed_keas = *((float *)(buf.data() + 4 * 2)); groundspeed_ktgs = *((float *)(buf.data() + 4 * 4)); 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: pressure = *((float *)(buf.data() + 4 * 1)) * INHG2KPA; temperature = *((float *)(buf.data() + 4 * 2)); break; case XplaneSimulator::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 XplaneSimulator::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 XplaneSimulator::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; } }