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

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       xplanesimulator10.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @brief
   7  * @see        The GNU Public License (GPL) Version 3
   8  * @defgroup   hitlplugin
   9  * @{
  10  *
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27
  28 /**
  29  * Description of X-Plane Protocol:
  30  *
  31  * To see what data can be sended/recieved to/from X-Plane, launch X-Plane -> goto main menu
  32  * (cursor at top of main X-Plane window) -> Settings -> Data Input and Output -> Data Set.
  33  * Data Set shown all X-Plane params,
  34  * each row has four checkbox: 1st check - out to UDP; 4 check - show on screen
  35  * All the UDP messages for X-Plane have the same format, which is:
  36  * 5-character MESSAGE PROLOUGE (to indicate the type of message)
  37  * and then a DATA INPUT STRUCTURE (containing the message data that you want to send or receive)
  38  *
  39  * DATA INPUT/OUTPUT STRUCTURE is the following stuct:
  40  *
  41  *  struct data_struct
  42  *  {
  43  *      int index;     // data index, the index into the list of variables
  44                        // you can output from the Data Output screen in X-Plane.
  45  *      float data[8]; // the up to 8 numbers you see in the data output screen associated with that selection..
  46                        // many outputs do not use all 8, though.
  47  * };
  48  *
  49  * For Example, update of aileron/elevon/rudder in X-Plane (11 row in Data Set)
  50  * bytes     value     description
  51  * [0-3]     DATA      message type
  52  * [4]       none      no matter
  53  * [5-8]     11        code of setting param(row in Data Set)
  54  * [9-41]    data      message data (8 float values)
  55  * total size: 41 byte
  56  *
  57  */
  58
  59 #include "xplanesimulator10.h"
  60 #include "extensionsystem/pluginmanager.h"
  61 #include <coreplugin/icore.h>
  62 #include <coreplugin/threadmanager.h>
  63 #include <math.h>
  64
  65 void TraceBuf10(const char *buf, int len);
  66
  67 XplaneSimulator10::XplaneSimulator10(const SimulatorSettings & params) :
  68     Simulator(params)
  69 {
  70     resetInitialHomePosition();
  71 }
  72
  73
  74 XplaneSimulator10::~XplaneSimulator10()
  75 {}
  76
  77 void XplaneSimulator10::setupUdpPorts(const QString & host, int inPort, int outPort)
  78 {
  79     Q_UNUSED(outPort);
  80
  81     inSocket->bind(QHostAddress(host), inPort);
  82     // outSocket->bind(QHostAddress(host), outPort);
  83     resetInitialHomePosition();
  84 }
  85
  86 bool XplaneSimulator10::setupProcess()
  87 {
  88     emit processOutput(QString("Please start X-Plane 10 manually, and make sure it is setup to output its ") +
  89                        "data to host " + settings.hostAddress + " UDP port " + QString::number(settings.inPort));
  90
  91     return true;
  92 }
  93
  94 /**
  95  * update data in X-Plane simulator
  96  */
  97 void XplaneSimulator10::transmitUpdate()
  98 {
  99     if (settings.manualControlEnabled) {
 100         // Read ActuatorDesired from autopilot
 101         ActuatorDesired::DataFields actData = actDesired->getData();
 102         float ailerons = actData.Roll;
 103         float elevator = actData.Pitch;
 104         float rudder   = actData.Yaw;
 105         float throttle = actData.Thrust > 0 ? actData.Thrust : 0;
 106         float none     = -999;
 107         // quint32 none = *((quint32*)&tmp); // get float as 4 bytes
 108
 109         quint32 code;
 110         QByteArray buf;
 111         QDataStream stream(&buf, QIODevice::ReadWrite);
 112
 113         // !!! LAN byte order - Big Endian
 114 #if Q_BYTE_ORDER == Q_LITTLE_ENDIAN
 115         stream.setByteOrder(QDataStream::LittleEndian);
 116 #endif
 117
 118         // 11th data settings (flight con: ail/elv/rud)
 119         buf.clear();
 120         code = 11;
 121         // quint8 header[] = "DATA";
 122         /*
 123            stream << *((quint32*)header) <<
 124                   (quint8)0x30 <<
 125                   code <<
 126          *((quint32*)&elevator) <<
 127          *((quint32*)&ailerons) <<
 128          *((quint32*)&rudder)   <<
 129                   none <<
 130          *((quint32*)&ailerons) <<
 131                   none <<
 132                   none  <<
 133                   none;
 134          */
 135         buf.append("DATA0");
 136         buf.append(reinterpret_cast<const char *>(&code), sizeof(code));
 137         buf.append(reinterpret_cast<const char *>(&elevator), sizeof(elevator));
 138         buf.append(reinterpret_cast<const char *>(&ailerons), sizeof(ailerons));
 139         buf.append(reinterpret_cast<const char *>(&rudder), sizeof(rudder));
 140         buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
 141         buf.append(reinterpret_cast<const char *>(&rudder), sizeof(rudder));
 142         buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
 143         buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
 144         buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
 145 // TraceBuf10(buf.data(),41);
 146
 147         if (outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) {
 148             emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n");
 149         }
 150         // outSocket->write(buf);
 151
 152         // 25th data settings (throttle command)
 153         buf.clear();
 154         code = 25;
 155         // stream << *((quint32*)header) << (quint8)0x30 << code << *((quint32*)&throttle) << none  << none
 156         // << none  << none  << none << none  << none;
 157         buf.append("DATA0");
 158         buf.append(reinterpret_cast<const char *>(&code), sizeof(code));
 159         buf.append(reinterpret_cast<const char *>(&throttle), sizeof(throttle));
 160         buf.append(reinterpret_cast<const char *>(&throttle), sizeof(throttle));
 161         buf.append(reinterpret_cast<const char *>(&throttle), sizeof(throttle));
 162         buf.append(reinterpret_cast<const char *>(&throttle), sizeof(throttle));
 163         buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
 164         buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
 165         buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
 166         buf.append(reinterpret_cast<const char *>(&none), sizeof(none));
 167
 168         if (outSocket->writeDatagram(buf, QHostAddress(settings.remoteAddress), settings.outPort) == -1) {
 169             emit processOutput("Error sending UDP packet to XPlane: " + outSocket->errorString() + "\n");
 170         }
 171
 172         // outSocket->write(buf);
 173
 174
 175         /** !!! this settings was given from ardupilot X-Plane.pl, I comment them
 176            but if it needed comment should be removed !!!
 177
 178            // 8th data settings (joystick 1 ail/elv/rud)
 179            stream << "DATA0" << quint32(11) << elevator << ailerons << rudder
 180             << float(-999) << float(-999) << float(-999) << float(-999) << float(-999);
 181            outSocket->write(buf);
 182          */
 183     }
 184 }
 185
 186 /**
 187  * process data string from X-Plane simulator
 188  */
 189 void XplaneSimulator10::processUpdate(const QByteArray & dataBuf)
 190 {
 191     float altitude_msl     = 0;
 192     float altitude_agl     = 0;
 193     float latitude         = 0;
 194     float longitude        = 0;
 195     float airspeed_keas    = 0;
 196     float groundspeed_ktgs = 0;
 197     float pitch         = 0;
 198     float roll          = 0;
 199     float heading       = 0;
 200     float pressure      = 0;
 201     float temperature   = 0;
 202     float velX          = 0;
 203     float velY          = 0;
 204     float velZ          = 0;
 205     float dstX          = 0;
 206     float dstY          = 0;
 207     float dstZ          = 0;
 208     float accX          = 0;
 209     float accY          = 0;
 210     float accZ          = 0;
 211     float rollRate_rad  = 0;
 212     float pitchRate_rad = 0;
 213     float yawRate_rad   = 0;
 214
 215     // QString str;
 216     QByteArray & buf    = const_cast<QByteArray &>(dataBuf);
 217     QString data(buf);
 218
 219     if (data.left(4) == "DATA") { // check type of packet
 220         buf.remove(0, 5);
 221         if (dataBuf.size() % 36) {
 222             qDebug() << ("incorrect length of UDP packet: " + buf);
 223             return; // incorrect length of struct
 224         }
 225         // check correctness of data length, length must be multiple of (id_size+8*float_size)=4+8*4=36
 226         int channelCounter = dataBuf.size() / 36;
 227         do {
 228             switch (buf[0]) { // switch by id
 229             case XplaneSimulator10::LatitudeLongitude:
 230                 latitude     = *((float *)(buf.data() + 4 * 1));
 231                 longitude    = *((float *)(buf.data() + 4 * 2));
 232                 altitude_msl = *((float *)(buf.data() + 4 * 3)) * FT2M;
 233                 altitude_agl = *((float *)(buf.data() + 4 * 4)) * FT2M;
 234                 break;
 235
 236             case XplaneSimulator10::Speed:
 237                 airspeed_keas    = *((float *)(buf.data() + 4 * 2));
 238                 groundspeed_ktgs = *((float *)(buf.data() + 4 * 4));
 239                 break;
 240
 241             case XplaneSimulator10::PitchRollHeading:
 242                 pitch   = *((float *)(buf.data() + 4 * 1));
 243                 roll    = *((float *)(buf.data() + 4 * 2));
 244                 heading = *((float *)(buf.data() + 4 * 3));
 245                 break;
 246
 247             /*
 248                    case XplaneSimulator10::SystemPressures:
 249                pressure = *((float*)(buf.data()+4*1));
 250                break;
 251              */
 252
 253             case XplaneSimulator10::AtmosphereWeather:
 254                 pressure    = *((float *)(buf.data() + 4 * 1)) * INHG2KPA;
 255                 temperature = *((float *)(buf.data() + 4 * 2));
 256                 break;
 257
 258             case XplaneSimulator10::LocVelDistTraveled:
 259                 dstX = *((float *)(buf.data() + 4 * 1));
 260                 dstY = -*((float *)(buf.data() + 4 * 3));
 261                 dstZ = *((float *)(buf.data() + 4 * 2));
 262                 velX = *((float *)(buf.data() + 4 * 4));
 263                 velY = -*((float *)(buf.data() + 4 * 6));
 264                 velZ = *((float *)(buf.data() + 4 * 5));
 265                 break;
 266
 267             case XplaneSimulator10::AngularVelocities: // In [rad/s]
 268                 pitchRate_rad = *((float *)(buf.data() + 4 * 1));
 269                 rollRate_rad  = *((float *)(buf.data() + 4 * 2));
 270                 yawRate_rad   = *((float *)(buf.data() + 4 * 3));
 271                 break;
 272
 273             case XplaneSimulator10::Gload:
 274                 accX = *((float *)(buf.data() + 4 * 6)) * GEE;
 275                 accY = *((float *)(buf.data() + 4 * 7)) * GEE;
 276                 accZ = *((float *)(buf.data() + 4 * 5)) * GEE;
 277                 break;
 278
 279             default:
 280                 break;
 281             }
 282             channelCounter--;
 283             buf.remove(0, 36);
 284         } while (channelCounter);
 285
 286
 287         ///////
 288         // Output formatting
 289         ///////
 290         Output2Hardware out;
 291         memset(&out, 0, sizeof(Output2Hardware));
 292
 293         // Update GPS Position objects
 294         out.latitude    = latitude * 1e7;
 295         out.longitude   = longitude * 1e7;
 296         out.altitude    = altitude_msl;
 297         out.agl = altitude_agl;
 298         out.groundspeed = groundspeed_ktgs * 1.15 * 1.6089 / 3.6; // Convert from [kts] to [m/s]
 299
 300         out.calibratedAirspeed = airspeed_keas * 1.15 * 1.6089 / 3.6; // Convert from [kts] to [m/s]
 301
 302         // Update BaroSensor object
 303         out.temperature = temperature;
 304         out.pressure    = pressure;
 305
 306         // Update attState object
 307         out.roll      = roll;       // roll;
 308         out.pitch     = pitch;     // pitch
 309         out.heading   = heading; // yaw
 310
 311
 312         out.dstN      = dstY;
 313         out.dstE      = dstX;
 314         out.dstD      = -dstZ;
 315
 316         // Update VelocityState.{North,East,Down}
 317         out.velNorth  = velY;
 318         out.velEast   = velX;
 319         out.velDown   = -velZ;
 320
 321         // Update gyroscope sensor data - convert from rad/s to deg/s
 322         out.rollRate  = rollRate_rad * (180.0 / M_PI);
 323         out.pitchRate = pitchRate_rad * (180.0 / M_PI);
 324         out.yawRate   = yawRate_rad * (180.0 / M_PI);
 325
 326         // Update accelerometer sensor data
 327         out.accX      = accX;
 328         out.accY      = accY;
 329         out.accZ      = -accZ;
 330
 331         updateUAVOs(out);
 332     }
 333     // issue manual update
 334     // attState->updated();
 335     // altState->updated();
 336     // posState->updated();
 337 }
 338
 339
 340 void TraceBuf10(const char *buf, int len)
 341 {
 342     QString str;
 343     bool reminder = true;
 344
 345     for (int i = 0; i < len; i++) {
 346         if (!(i % 16)) {
 347             if (i > 0) {
 348                 qDebug() << str;
 349                 str.clear();
 350                 reminder = false;
 351             }
 352             reminder = true;
 353         }
 354         str += QString(" 0x%1").arg((quint8)buf[i], 2, 16, QLatin1Char('0'));
 355     }
 356
 357     if (reminder) {
 358         qDebug() << str;
 359     }
 360 }