QGCXPlaneLink.cc 34.8 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34
/*=====================================================================

QGroundControl Open Source Ground Control Station

(c) 2009 - 2011 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

This file is part of the QGROUNDCONTROL project

    QGROUNDCONTROL 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.

    QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file QGCXPlaneLink.cc
 *   Implementation of X-Plane interface
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <QTimer>
#include <QList>
#include <QDebug>
#include <QMutexLocker>
35
#include <QNetworkInterface>
36 37 38 39
#include <iostream>
#include "QGCXPlaneLink.h"
#include "QGC.h"
#include <QHostInfo>
40
#include "UAS.h"
Lorenz Meier's avatar
Lorenz Meier committed
41
#include "UASInterface.h"
42
#include "QGCMessageBox.h"
43

44
QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress localHost, quint16 localPort) :
45
    mav(mav),
Lorenz Meier's avatar
Lorenz Meier committed
46 47
    remoteHost(QHostAddress("127.0.0.1")),
    remotePort(49000),
48
    socket(NULL),
49
    process(NULL),
50
    terraSync(NULL),
51
    barometerOffsetkPa(-8.0f),
52
    airframeID(QGCXPlaneLink::AIRFRAME_UNKNOWN),
Lorenz Meier's avatar
Lorenz Meier committed
53
    xPlaneConnected(false),
54 55
    xPlaneVersion(0),
    simUpdateLast(QGC::groundTimeMilliseconds()),
56
    simUpdateFirst(0),
57
    simUpdateLastText(QGC::groundTimeMilliseconds()),
58
    simUpdateLastGroundTruth(QGC::groundTimeMilliseconds()),
Lorenz Meier's avatar
Lorenz Meier committed
59
    simUpdateHz(0),
60 61
    _sensorHilEnabled(true),
    _should_exit(false)
62
{
63 64 65 66
    // We're doing it wrong - because the Qt folks got the API wrong:
    // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/
    moveToThread(this);

67 68
    setTerminationEnabled(false);

69 70
    this->localHost = localHost;
    this->localPort = localPort/*+mav->getUASID()*/;
71 72
    connectState = false;

73
    this->name = tr("X-Plane Link (localPort:%1)").arg(localPort);
74
    setRemoteHost(remoteHost);
Lorenz Meier's avatar
Lorenz Meier committed
75
    loadSettings();
76 77 78
}

QGCXPlaneLink::~QGCXPlaneLink()
79
{
Lorenz Meier's avatar
Lorenz Meier committed
80
    storeSettings();
Lorenz Meier's avatar
Lorenz Meier committed
81
    // Tell the thread to exit
82
    _should_exit = true;
Lorenz Meier's avatar
Lorenz Meier committed
83

84 85 86 87 88
    if (socket) {
        socket->close();
        socket->deleteLater();
        socket = NULL;
    }
89 90
}

Lorenz Meier's avatar
Lorenz Meier committed
91 92 93 94 95
void QGCXPlaneLink::loadSettings()
{
    // Load defaults from settings
    QSettings settings;
    settings.beginGroup("QGC_XPLANE_LINK");
96
    setRemoteHost(settings.value("REMOTE_HOST", QString("%1:%2").arg(remoteHost.toString()).arg(remotePort)).toString());
Lorenz Meier's avatar
Lorenz Meier committed
97
    setVersion(settings.value("XPLANE_VERSION", 10).toInt());
Lorenz Meier's avatar
Lorenz Meier committed
98
    selectAirframe(settings.value("AIRFRAME", "default").toString());
99
    _sensorHilEnabled = settings.value("SENSOR_HIL", _sensorHilEnabled).toBool();
Lorenz Meier's avatar
Lorenz Meier committed
100 101 102 103 104 105 106 107
    settings.endGroup();
}

void QGCXPlaneLink::storeSettings()
{
    // Store settings
    QSettings settings;
    settings.beginGroup("QGC_XPLANE_LINK");
Lorenz Meier's avatar
Lorenz Meier committed
108
    settings.setValue("REMOTE_HOST", QString("%1:%2").arg(remoteHost.toString()).arg(remotePort));
Lorenz Meier's avatar
Lorenz Meier committed
109 110
    settings.setValue("XPLANE_VERSION", xPlaneVersion);
    settings.setValue("AIRFRAME", airframeName);
111
    settings.setValue("SENSOR_HIL", _sensorHilEnabled);
Lorenz Meier's avatar
Lorenz Meier committed
112 113 114
    settings.endGroup();
}

Lorenz Meier's avatar
Lorenz Meier committed
115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140
void QGCXPlaneLink::setVersion(const QString& version)
{
    unsigned int oldVersion = xPlaneVersion;
    if (version.contains("9"))
    {
        xPlaneVersion = 9;
    }
    else if (version.contains("10"))
    {
        xPlaneVersion = 10;
    }
    else if (version.contains("11"))
    {
        xPlaneVersion = 11;
    }
    else if (version.contains("12"))
    {
        xPlaneVersion = 12;
    }

    if (oldVersion != xPlaneVersion)
    {
        emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion));
    }
}

Lorenz Meier's avatar
Lorenz Meier committed
141 142 143 144 145 146
void QGCXPlaneLink::setVersion(unsigned int version)
{
    bool changed = (xPlaneVersion != version);
    xPlaneVersion = version;
    if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion));
}
Lorenz Meier's avatar
Lorenz Meier committed
147

Lorenz Meier's avatar
Lorenz Meier committed
148

149 150 151 152 153 154
/**
 * @brief Runs the thread
 *
 **/
void QGCXPlaneLink::run()
{
155 156 157 158 159 160 161 162 163
    if (!mav) {
        emit statusMessage("No MAV present");
        return;
    }

    if (connectState) {
        emit statusMessage("Already connected");
        return;
    }
164 165

    socket = new QUdpSocket(this);
Lorenz Meier's avatar
Lorenz Meier committed
166
    socket->moveToThread(this);
167
    connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint);
168 169 170 171
    if (!connectState) {

        emit statusMessage("Binding socket failed!");

172
        socket->deleteLater();
173 174 175
        socket = NULL;
        return;
    }
176

177 178
    emit statusMessage(tr("Waiting for XPlane.."));

179 180 181 182 183
    QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));

    UAS* uas = dynamic_cast<UAS*>(mav);
    if (uas)
    {
184 185 186 187 188 189 190 191
        connect(uas, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8)), Qt::QueuedConnection);
        connect(uas, SIGNAL(hilActuatorsChanged(quint64, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(quint64,float,float,float,float,float,float,float,float)), Qt::QueuedConnection);

        connect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), uas, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), Qt::QueuedConnection);
        connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), uas, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), Qt::QueuedConnection);
        connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), uas, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), Qt::QueuedConnection);
        connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), uas, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), Qt::QueuedConnection);

192
        uas->startHil();
193 194 195
    } else {
        emit statusMessage(tr("Failed to connect to drone instance"));
        return;
196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228
    }

#pragma pack(push, 1)
    struct iset_struct
    {
        char b[5];
        int index; // (0->20 in the lsit below)
        char str_ipad_them[16];
        char str_port_them[6];
        char padding[2];
        int use_ip;
    } ip; // to use this option, 0 not to.
#pragma pack(pop)

    ip.b[0] = 'I';
    ip.b[1] = 'S';
    ip.b[2] = 'E';
    ip.b[3] = 'T';
    ip.b[4] = '0';

    QList<QHostAddress> hostAddresses = QNetworkInterface::allAddresses();

    QString localAddrStr;
    QString localPortStr = QString("%1").arg(localPort);

    for (int i = 0; i < hostAddresses.size(); i++)
    {
        // Exclude loopback IPv4 and all IPv6 addresses
        if (hostAddresses.at(i) != QHostAddress("127.0.0.1") && !hostAddresses.at(i).toString().contains(":"))
        {
            localAddrStr = hostAddresses.at(i).toString();
            break;
        }
Lorenz Meier's avatar
Lorenz Meier committed
229
    }
230 231

    ip.index = 0;
232 233
    strncpy(ip.str_ipad_them, localAddrStr.toLatin1(), qMin((int)sizeof(ip.str_ipad_them), 16));
    strncpy(ip.str_port_them, localPortStr.toLatin1(), qMin((int)sizeof(ip.str_port_them), 6));
234 235 236 237
    ip.use_ip = 1;

    writeBytes((const char*)&ip, sizeof(ip));

238 239
    _should_exit = false;

240 241 242 243
    while(!_should_exit) {
        QCoreApplication::processEvents();
        QGC::SLEEP::msleep(5);
    }
244

245 246
    uas = dynamic_cast<UAS*>(mav);
    if (uas)
247
    {
248 249
        disconnect(uas, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8)));
        disconnect(uas, SIGNAL(hilActuatorsChanged(quint64, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(quint64,float,float,float,float,float,float,float,float)));
250

251 252 253 254
        disconnect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), uas, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
        disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), uas, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
        disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), uas, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int)));
        disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), uas, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)));
255 256 257 258 259 260 261

        // Do not toggle HIL state on the UAS - this is not the job of this link, but of the
        // UAS object
    }

    connectState = false;

262 263
    QObject::disconnect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));

264
    socket->close();
265
    socket->deleteLater();
266 267 268 269
    socket = NULL;

    emit simulationDisconnected();
    emit simulationConnected(false);
270 271
}

272
void QGCXPlaneLink::setPort(int localPort)
273
{
274
    this->localPort = localPort;
275 276 277 278 279 280
    disconnectSimulation();
    connectSimulation();
}

void QGCXPlaneLink::processError(QProcess::ProcessError err)
{
281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304
    QString msg;
    
    switch(err) {
        case QProcess::FailedToStart:
            msg = tr("X-Plane Failed to start. Please check if the path and command is correct");
            break;
            
        case QProcess::Crashed:
            msg = tr("X-Plane crashed. This is an X-Plane-related problem, check for X-Plane upgrade.");
            break;
            
        case QProcess::Timedout:
            msg = tr("X-Plane start timed out. Please check if the path and command is correct");
            break;
            
        case QProcess::ReadError:
        case QProcess::WriteError:
            msg = tr("Could not communicate with X-Plane. Please check if the path and command are correct");
            break;
            
        case QProcess::UnknownError:
        default:
            msg = tr("X-Plane error occurred. Please check if the path and command is correct.");
            break;
305
    }
306 307 308
    
    
    QGCMessageBox::critical(tr("X-Plane HIL"), msg);
309 310
}

Lorenz Meier's avatar
Lorenz Meier committed
311 312
QString QGCXPlaneLink::getRemoteHost()
{
Lorenz Meier's avatar
Lorenz Meier committed
313
    return QString("%1:%2").arg(remoteHost.toString()).arg(remotePort);
Lorenz Meier's avatar
Lorenz Meier committed
314 315
}

316
/**
Lorenz Meier's avatar
Lorenz Meier committed
317
 * @param newHost Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551
318
 */
Lorenz Meier's avatar
Lorenz Meier committed
319
void QGCXPlaneLink::setRemoteHost(const QString& newHost)
320
{
Lorenz Meier's avatar
Lorenz Meier committed
321 322 323
    if (newHost.length() == 0)
        return;

Lorenz Meier's avatar
Lorenz Meier committed
324
    if (newHost.contains(":"))
325
    {
Lorenz Meier's avatar
Lorenz Meier committed
326
        QHostInfo info = QHostInfo::fromName(newHost.split(":").first());
327 328
        if (info.error() == QHostInfo::NoError)
        {
Lorenz Meier's avatar
Lorenz Meier committed
329 330
            // Add newHost
            QList<QHostAddress> newHostAddresses = info.addresses();
331
            QHostAddress address;
Lorenz Meier's avatar
Lorenz Meier committed
332
            for (int i = 0; i < newHostAddresses.size(); i++)
333 334
            {
                // Exclude loopback IPv4 and all IPv6 addresses
Lorenz Meier's avatar
Lorenz Meier committed
335
                if (!newHostAddresses.at(i).toString().contains(":"))
336
                {
Lorenz Meier's avatar
Lorenz Meier committed
337
                    address = newHostAddresses.at(i);
338 339
                }
            }
340 341
            remoteHost = address;
            // Set localPort according to user input
Lorenz Meier's avatar
Lorenz Meier committed
342
            remotePort = newHost.split(":").last().toInt();
343 344 345 346
        }
    }
    else
    {
Lorenz Meier's avatar
Lorenz Meier committed
347
        QHostInfo info = QHostInfo::fromName(newHost);
348 349
        if (info.error() == QHostInfo::NoError)
        {
Lorenz Meier's avatar
Lorenz Meier committed
350
            // Add newHost
351
            remoteHost = info.addresses().first();
Lorenz Meier's avatar
Lorenz Meier committed
352
            if (remotePort == 0) remotePort = 49000;
353 354 355
        }
    }

Lorenz Meier's avatar
Lorenz Meier committed
356 357 358 359 360
    if (isConnected())
    {
        disconnectSimulation();
        connectSimulation();
    }
361 362 363 364

    emit remoteChanged(QString("%1:%2").arg(remoteHost.toString()).arg(remotePort));
}

365
void QGCXPlaneLink::updateActuators(quint64 time, float act1, float act2, float act3, float act4, float act5, float act6, float act7, float act8)
366
{
Lorenz Meier's avatar
Lorenz Meier committed
367
    if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393
    // Only update this for multirotors
    {

        Q_UNUSED(time);
        Q_UNUSED(act5);
        Q_UNUSED(act6);
        Q_UNUSED(act7);
        Q_UNUSED(act8);

    #pragma pack(push, 1)
        struct payload {
            char b[5];
            int index;
            float f[8];
        } p;
    #pragma pack(pop)

        p.b[0] = 'D';
        p.b[1] = 'A';
        p.b[2] = 'T';
        p.b[3] = 'A';
        p.b[4] = '\0';

        p.index = 25;
        memset(p.f, 0, sizeof(p.f));

Lorenz Meier's avatar
Lorenz Meier committed
394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421
        p.f[0] = act1;
        p.f[1] = act2;
        p.f[2] = act3;
        p.f[3] = act4;

        // XXX the system corrects for the scale onboard, do not scale again

//        if (airframeID == AIRFRAME_QUAD_X_MK_10INCH_I2C)
//        {
//            p.f[0] = act1 / 255.0f;
//            p.f[1] = act2 / 255.0f;
//            p.f[2] = act3 / 255.0f;
//            p.f[3] = act4 / 255.0f;
//        }
//        else if (airframeID == AIRFRAME_QUAD_X_ARDRONE)
//        {
//            p.f[0] = act1 / 500.0f;
//            p.f[1] = act2 / 500.0f;
//            p.f[2] = act3 / 500.0f;
//            p.f[3] = act4 / 500.0f;
//        }
//        else
//        {
//            p.f[0] = (act1 - 1000.0f) / 1000.0f;
//            p.f[1] = (act2 - 1000.0f) / 1000.0f;
//            p.f[2] = (act3 - 1000.0f) / 1000.0f;
//            p.f[3] = (act4 - 1000.0f) / 1000.0f;
//        }
422 423 424
        // Throttle
        writeBytes((const char*)&p, sizeof(p));
    }
425 426
}

427
void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode)
428
{
429 430 431 432 433 434 435 436 437 438 439 440 441 442
    #pragma pack(push, 1)
    struct payload {
        char b[5];
        int index;
        float f[8];
    } p;
    #pragma pack(pop)

    p.b[0] = 'D';
    p.b[1] = 'A';
    p.b[2] = 'T';
    p.b[3] = 'A';
    p.b[4] = '\0';

443 444 445 446 447
    Q_UNUSED(time);
    Q_UNUSED(systemMode);
    Q_UNUSED(navMode);

    bool isFixedWing = true;
Lorenz Meier's avatar
Lorenz Meier committed
448 449 450 451 452 453 454 455 456 457 458 459 460

    if (mav->getAirframe() == UASInterface::QGC_AIRFRAME_X8 ||
            mav->getAirframe() == UASInterface::QGC_AIRFRAME_VIPER_2_0 ||
            mav->getAirframe() == UASInterface::QGC_AIRFRAME_CAMFLYER_Q)
    {
        // de-mix delta-mixed inputs
        // pitch input - mixed roll and pitch channels
        p.f[0] = 0.5f * (rollAilerons - pitchElevator);
        // roll input - mixed roll and pitch channels
        p.f[1] = 0.5f * (rollAilerons + pitchElevator);
        // yaw
        p.f[2] = 0.0f;
    }
461 462 463 464 465
    else if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
    {
        qDebug() << "MAV_TYPE_QUADROTOR";

        // Individual effort will be provided directly to the actuators on Xplane quadrotor.
466
        p.f[0] = yawRudder;
467
        p.f[1] = rollAilerons;
468 469 470
        p.f[2] = throttle;
        p.f[3] = pitchElevator;

471 472
        isFixedWing = false;
    }
Lorenz Meier's avatar
Lorenz Meier committed
473 474
    else
    {
475
        // direct pass-through, normal fixed-wing.
Lorenz Meier's avatar
Lorenz Meier committed
476 477 478 479
        p.f[0] = -pitchElevator;
        p.f[1] = rollAilerons;
        p.f[2] = yawRudder;
    }
480

481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505
    if(isFixedWing)
    {
        // Ail / Elevon / Rudder
        p.index = 12;   // XPlane, wing sweep
        writeBytes((const char*)&p, sizeof(p));

        p.index = 8;    // XPlane, joystick? why?
        writeBytes((const char*)&p, sizeof(p));

        p.index = 25;   // Thrust
        memset(p.f, 0, sizeof(p.f));
        p.f[0] = throttle;
        p.f[1] = throttle;
        p.f[2] = throttle;
        p.f[3] = throttle;

        // Throttle
        writeBytes((const char*)&p, sizeof(p));
    }
    else
    {
        qDebug() << "Transmitting p.index = 25";
        p.index = 25;   // XPlane, throttle command.
        writeBytes((const char*)&p, sizeof(p));
    }
506 507 508

}

509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537
Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) {
  double c__ = cos(yaw);
  double _c_ = cos(pitch);
  double __c = cos(roll);
  double s__ = sin(yaw);
  double _s_ = sin(pitch);
  double __s = sin(roll);
  double cc_ = c__ * _c_;
  double cs_ = c__ * _s_;
  double sc_ = s__ * _c_;
  double ss_ = s__ * _s_;
  double c_c = c__ * __c;
  double c_s = c__ * __s;
  double s_c = s__ * __c;
  double s_s = s__ * __s;
  double _cc = _c_ * __c;
  double _cs = _c_ * __s;
  double csc = cs_ * __c;
  double css = cs_ * __s;
  double ssc = ss_ * __c;
  double sss = ss_ * __s;
  Eigen::Matrix3f wRo;
  wRo <<
    cc_  , css-s_c,  csc+s_s,
    sc_  , sss+c_c,  ssc-c_s,
    -_s_  ,     _cs,      _cc;
  return wRo;
}

538 539
void QGCXPlaneLink::writeBytes(const char* data, qint64 size)
{
540
    if (!data) return;
541 542 543

    // If socket exists and is connected, transmit the data
    if (socket && connectState)
544
    {
545
        socket->writeDatagram(data, size, remoteHost, remotePort);
546 547 548 549
    }
}

/**
550
 * @brief Read all pending packets from the interface.
551 552 553
 **/
void QGCXPlaneLink::readBytes()
{
Lorenz Meier's avatar
Lorenz Meier committed
554 555
    // Only emit updates on attitude message
    bool emitUpdate = false;
Lorenz Meier's avatar
Lorenz Meier committed
556
    quint16 fields_changed = 0;
Lorenz Meier's avatar
Lorenz Meier committed
557

558
    const qint64 maxLength = 1000;
559 560 561 562 563
    char data[maxLength];
    QHostAddress sender;
    quint16 senderPort;

    unsigned int s = socket->pendingDatagramSize();
564
    if (s > maxLength) std::cerr << __FILE__ << __LINE__ << " UDP datagram overflow, allowed to read less bytes than datagram size: " << s << std::endl;
565
    socket->readDatagram(data, maxLength, &sender, &senderPort);
566 567 568 569
    if (s > maxLength) {
    	std::string headStr = std::string(data, data+5);
    	std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl;
    }
570

571 572 573
    // Calculate the number of data segments a 36 bytes
    // XPlane always has 5 bytes header: 'DATA@'
    unsigned nsegs = (s-5)/36;
574

575
    //qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
576

577 578 579 580 581 582
    #pragma pack(push, 1)
    struct payload {
        int index;
        float f[8];
    } p;
    #pragma pack(pop)
583

584 585
    bool oldConnectionState = xPlaneConnected;

586 587 588 589 590
    if (data[0] == 'D' &&
            data[1] == 'A' &&
            data[2] == 'T' &&
            data[3] == 'A')
    {
591
        xPlaneConnected = true;
592

593 594 595 596
        if (oldConnectionState != xPlaneConnected) {
            simUpdateFirst = QGC::groundTimeMilliseconds();
        }

597 598 599 600 601 602 603 604
        for (unsigned i = 0; i < nsegs; i++)
        {
            // Get index
            unsigned ioff = (5+i*36);;
            memcpy(&(p), data+ioff, sizeof(p));

            if (p.index == 3)
            {
605 606
                ind_airspeed = p.f[5] * 0.44704f;
                true_airspeed = p.f[6] * 0.44704f;
607 608
                groundspeed = p.f[7] * 0.44704;

609 610
                //qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
            }
611 612
            if (p.index == 4)
            {
613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641
				// WORKAROUND: IF ground speed <<1m/s and altitude-above-ground <1m, do NOT use the X-Plane data, because X-Plane (tested 
				// with v10.3 and earlier) delivers yacc=0 and zacc=0 when the ground speed is very low, which gives e.g. wrong readings 
				// before launch when waiting on the runway. This might pose a problem for initial state estimation/calibration. 
				// Instead, we calculate our own accelerations.
				if (fabsf(groundspeed)<0.1f && alt_agl<1.0) 
				{
					// TODO: Add centrip. acceleration to the current static acceleration implementation.
					Eigen::Vector3f g(0, 0, -9.81f);
					Eigen::Matrix3f R = euler_to_wRo(yaw, pitch, roll);
					Eigen::Vector3f gr = R.transpose().eval() * g;

					xacc = gr[0];
					yacc = gr[1];
					zacc = gr[2];

					//qDebug() << "Calculated values" << gr[0] << gr[1] << gr[2];
				}
				else
				{
					// Accelerometer readings, directly from X-Plane and including centripetal forces. 
					const float one_g = 9.80665f;
					xacc = p.f[5] * one_g;
					yacc = p.f[6] * one_g;
					zacc = -p.f[4] * one_g;

					//qDebug() << "X-Plane values" << xacc << yacc << zacc;
				}

				fields_changed |= (1 << 0) | (1 << 1) | (1 << 2);
642
            }
643
            // atmospheric pressure aircraft for XPlane 9 and 10
644
            else if (p.index == 6)
Lorenz Meier's avatar
Lorenz Meier committed
645
            {
646 647
                // inHg to hPa (hecto Pascal / millibar)
                abs_pressure = p.f[0] * 33.863886666718317f;
Lorenz Meier's avatar
Lorenz Meier committed
648
                temperature = p.f[1];
649
                fields_changed |= (1 << 9) | (1 << 12);
Lorenz Meier's avatar
Lorenz Meier committed
650
            }
651 652 653 654 655 656 657 658 659 660 661
            // Forward controls from X-Plane to MAV, not very useful
            // better: Connect Joystick to QGroundControl
//            else if (p.index == 8)
//            {
//                //qDebug() << "MAN:" << p.f[0] << p.f[3] << p.f[7];
//                man_roll = p.f[0];
//                man_pitch = p.f[1];
//                man_yaw = p.f[2];
//                UAS* uas = dynamic_cast<UAS*>(mav);
//                if (uas) uas->setManualControlCommands(man_roll, man_pitch, man_yaw, 0.6);
//            }
662
            else if ((xPlaneVersion == 10 && p.index == 16) || (xPlaneVersion == 9 && p.index == 17))
663
            {
664 665 666 667
                // Cross checked with XPlane flight
                pitchspeed = p.f[0];
                rollspeed = p.f[1];
                yawspeed = p.f[2];
668
                fields_changed |= (1 << 3) | (1 << 4) | (1 << 5);
669
            }
Lorenz Meier's avatar
Lorenz Meier committed
670
            else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18))
671
            {
672
                //qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
673 674
                pitch = p.f[0] / 180.0f * M_PI;
                roll = p.f[1] / 180.0f * M_PI;
675
                yaw = p.f[2] / 180.0f * M_PI;
676

677 678
                // X-Plane expresses yaw as 0..2 PI
                if (yaw > M_PI) {
679
                    yaw -= 2.0f * static_cast<float>(M_PI);
680 681
                }
                if (yaw < -M_PI) {
682
                    yaw += 2.0f * static_cast<float>(M_PI);
683
                }
Lorenz Meier's avatar
Lorenz Meier committed
684

685
                float yawmag = p.f[3] / 180.0f * M_PI;
Lorenz Meier's avatar
Lorenz Meier committed
686 687

                if (yawmag > M_PI) {
688
                    yawmag -= 2.0f * static_cast<float>(M_PI);
Lorenz Meier's avatar
Lorenz Meier committed
689 690
                }
                if (yawmag < -M_PI) {
691
                    yawmag += 2.0f * static_cast<float>(M_PI);
Lorenz Meier's avatar
Lorenz Meier committed
692 693
                }

694 695 696 697 698 699 700
                // Normal rotation matrix, but since we rotate the
                // vector [0.25 0 0.45]', we end up with these relevant
                // matrix parts.

                xmag = cos(-yawmag) * 0.25f;
                ymag = sin(-yawmag) * 0.25f;
                zmag = 0.45f;
701
                fields_changed |= (1 << 6) | (1 << 7) | (1 << 8);
Lorenz Meier's avatar
Lorenz Meier committed
702

703 704 705 706
                double cosPhi = cos(roll);
                double sinPhi = sin(roll);
                double cosThe = cos(pitch);
                double sinThe = sin(pitch);
Lorenz Meier's avatar
Lorenz Meier committed
707 708
                double cosPsi = cos(0.0);
                double sinPsi = sin(0.0);
709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729

                float dcm[3][3];

                dcm[0][0] = cosThe * cosPsi;
                dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
                dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;

                dcm[1][0] = cosThe * sinPsi;
                dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
                dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;

                dcm[2][0] = -sinThe;
                dcm[2][1] = sinPhi * cosThe;
                dcm[2][2] = cosPhi * cosThe;

                Eigen::Matrix3f m = Eigen::Map<Eigen::Matrix3f>((float*)dcm).eval();

                Eigen::Vector3f mag(xmag, ymag, zmag);

                Eigen::Vector3f magbody = m * mag;

730 731
//                qDebug() << "yaw mag:" << p.f[2] << "x" << xmag << "y" << ymag;
//                qDebug() << "yaw mag in body:" << magbody(0) << magbody(1) << magbody(2);
732 733 734 735 736 737 738 739

                xmag = magbody(0);
                ymag = magbody(1);
                zmag = magbody(2);

                // Rotate the measurement vector into the body frame using roll and pitch


Lorenz Meier's avatar
Lorenz Meier committed
740
                emitUpdate = true;
741
            }
742

743 744 745 746
//            else if (p.index == 19)
//            {
//                qDebug() << "ATT:" << p.f[0] << p.f[1] << p.f[2];
//            }
747 748 749 750 751 752 753
			else if (p.index == 20)
			{
				//qDebug() << "LAT/LON/ALT:" << p.f[0] << p.f[1] << p.f[2];
				lat = p.f[0];
				lon = p.f[1];
				alt = p.f[2] * 0.3048f; // convert feet (MSL) to meters
				alt_agl = p.f[3] * 0.3048f; //convert feet (AGL) to meters
754
            }
755
            else if (p.index == 21)
Lorenz Meier's avatar
Lorenz Meier committed
756
            {
757 758
                vy = p.f[3];
                vx = -p.f[5];
759 760 761
                // moving 'up' in XPlane is positive, but its negative in NED
                // for us.
                vz = -p.f[4];
Lorenz Meier's avatar
Lorenz Meier committed
762
            }
763 764
            else if (p.index == 12)
            {
765
                //qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2];
766 767 768
            }
            else if (p.index == 25)
            {
769
                //qDebug() << "THROTTLE" << p.f[0] << p.f[1] << p.f[2] << p.f[3];
770 771 772
            }
            else if (p.index == 0)
            {
773
                //qDebug() << "STATS" << "fgraphics/s" << p.f[0] << "fsim/s" << p.f[2] << "t frame" << p.f[3] << "cpu load" << p.f[4] << "grnd ratio" << p.f[5] << "filt ratio" << p.f[6];
774 775 776
            }
            else if (p.index == 11)
            {
777
                //qDebug() << "CONTROLS" << "ail" << p.f[0] << "elev" << p.f[1] << "rudder" << p.f[2] << "nwheel" << p.f[3];
778 779 780
            }
            else
            {
781
                //qDebug() << "UNKNOWN #" << p.index << p.f[0] << p.f[1] << p.f[2] << p.f[3];
782 783
            }
        }
784 785 786 787 788 789 790 791 792 793 794 795 796 797
    }
    else if (data[0] == 'S' &&
             data[1] == 'N' &&
             data[2] == 'A' &&
             data[3] == 'P')
    {

    }
    else if (data[0] == 'S' &&
               data[1] == 'T' &&
               data[2] == 'A' &&
               data[3] == 'T')
    {

798 799 800 801 802
    }
    else
    {
        qDebug() << "UNKNOWN PACKET:" << data;
    }
803

804 805 806 807 808
    // Wait for 0.5s before actually using the data, so that all fields are filled
    if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) {
        return;
    }

809
    // Send updated state
810
    if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 3)
Lorenz Meier's avatar
Lorenz Meier committed
811
    {
812 813 814 815 816 817 818 819 820
        simUpdateHz = simUpdateHz * 0.9f + 0.1f * (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast));
        if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000) {
            emit statusMessage(tr("Receiving from XPlane at %1 Hz").arg(static_cast<int>(simUpdateHz)));
            // Reset lowpass with current value
            simUpdateHz = (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast));
            // Set state
            simUpdateLastText = QGC::groundTimeMilliseconds();
        }

821 822
        simUpdateLast = QGC::groundTimeMilliseconds();

823
        if (_sensorHilEnabled)
Lorenz Meier's avatar
Lorenz Meier committed
824
        {
825 826 827 828 829 830 831 832 833 834 835
            diff_pressure = (ind_airspeed * ind_airspeed * 1.225f) / 2.0f;

            /* tropospheric properties (0-11km) for standard atmosphere */
            const double T1 = 15.0 + 273.15;	/* temperature at base height in Kelvin */
            const double a  = -6.5 / 1000;	/* temperature gradient in degrees per metre */
            const double g  = 9.80665;	/* gravity constant in m/s/s */
            const double R  = 287.05;	/* ideal gas constant in J/kg/K */

            /* current pressure at MSL in kPa */
            double p1 = 1013.25 / 10.0;

836 837
            /* measured pressure in hPa, plus offset to simulate weather effects / offsets */
            double p = abs_pressure / 10.0 + barometerOffsetkPa;
838 839 840 841 842 843 844 845 846 847 848 849

            /*
             * Solve:
             *
             *     /        -(aR / g)     \
             *    | (p / p1)          . T1 | - T1
             *     \                      /
             * h = -------------------------------  + h1
             *                   a
             */
            pressure_alt = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;

850 851
            // set pressure alt to changed
            fields_changed |= (1 << 11);
Lorenz Meier's avatar
Lorenz Meier committed
852 853

            emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed,
854
                                        xmag, ymag, zmag, abs_pressure, diff_pressure / 100.0, pressure_alt, temperature, fields_changed);
855

856 857
            // XXX make these GUI-configurable and add randomness
            int gps_fix_type = 3;
858 859
            float eph = 0.3f;
            float epv = 0.6f;
860 861 862 863 864 865 866
            float vel = sqrt(vx*vx + vy*vy + vz*vz);
            float cog = atan2(vy, vx);
            int satellites = 8;

            emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites);
        } else {
            emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
867 868 869 870 871
                                 pitchspeed, yawspeed, lat, lon, alt,
                                 vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
        }

        // Limit ground truth to 25 Hz
872
        if (QGC::groundTimeMilliseconds() - simUpdateLastGroundTruth > 40) {
873 874 875
            emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
                                       pitchspeed, yawspeed, lat, lon, alt,
                                       vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
876

877 878
            simUpdateLastGroundTruth = QGC::groundTimeMilliseconds();
        }
Lorenz Meier's avatar
Lorenz Meier committed
879
    }
880

881 882
    if (!oldConnectionState && xPlaneConnected)
    {
883
        emit statusMessage(tr("Receiving from XPlane."));
884 885
    }

886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903 904 905 906 907 908 909 910 911 912 913 914
    //    // Echo data for debugging purposes
    //    std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl;
    //    int i;
    //    for (i=0; i<s; i++)
    //    {
    //        unsigned int v=data[i];
    //        fprintf(stderr,"%02x ", v);
    //    }
    //    std::cerr << std::endl;
}


/**
 * @brief Get the number of bytes to read.
 *
 * @return The number of bytes to read
 **/
qint64 QGCXPlaneLink::bytesAvailable()
{
    return socket->pendingDatagramSize();
}

/**
 * @brief Disconnect the connection.
 *
 * @return True if connection has been disconnected, false if connection couldn't be disconnected.
 **/
bool QGCXPlaneLink::disconnectSimulation()
{
915
    if (connectState)
916
    {
917 918 919 920
        _should_exit = true;
    } else {
        emit simulationDisconnected();
        emit simulationConnected(false);
921
    }
922 923 924 925

    return !connectState;
}

Lorenz Meier's avatar
Lorenz Meier committed
926
void QGCXPlaneLink::selectAirframe(const QString& plane)
927
{
928
    airframeName = plane;
929

930 931
    if (plane.contains("QRO"))
    {
Lorenz Meier's avatar
Lorenz Meier committed
932
        if (plane.contains("MK") && airframeID != AIRFRAME_QUAD_X_MK_10INCH_I2C)
933 934
        {
            airframeID = AIRFRAME_QUAD_X_MK_10INCH_I2C;
Lorenz Meier's avatar
Lorenz Meier committed
935
            emit airframeChanged("QRO_X / MK");
936
        }
Lorenz Meier's avatar
Lorenz Meier committed
937
        else if (plane.contains("ARDRONE") && airframeID != AIRFRAME_QUAD_X_ARDRONE)
938 939
        {
            airframeID = AIRFRAME_QUAD_X_ARDRONE;
Lorenz Meier's avatar
Lorenz Meier committed
940
            emit airframeChanged("QRO_X / ARDRONE");
941 942 943
        }
        else
        {
Lorenz Meier's avatar
Lorenz Meier committed
944
            bool changed = (airframeID != AIRFRAME_QUAD_DJI_F450_PWM);
945
            airframeID = AIRFRAME_QUAD_DJI_F450_PWM;
Lorenz Meier's avatar
Lorenz Meier committed
946
            if (changed) emit airframeChanged("QRO_X / DJI-F450 / PWM");
947 948
        }
    }
Lorenz Meier's avatar
Lorenz Meier committed
949 950
    else
    {
Lorenz Meier's avatar
Lorenz Meier committed
951
        bool changed = (airframeID != AIRFRAME_UNKNOWN);
Lorenz Meier's avatar
Lorenz Meier committed
952
        airframeID = AIRFRAME_UNKNOWN;
Lorenz Meier's avatar
Lorenz Meier committed
953
        if (changed) emit airframeChanged("X Plane default");
Lorenz Meier's avatar
Lorenz Meier committed
954
    }
955 956 957 958
}

void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw)
{
959
    #pragma pack(push, 1)
960 961
    struct VEH1_struct
    {
962
        char header[5];
963 964 965 966 967
        quint32 p;
        double lat_lon_ele[3];
        float psi_the_phi[3];
        float gear_flap_vect[3];
    } pos;
968
    #pragma pack(pop)
969

970 971 972 973 974
    pos.header[0] = 'V';
    pos.header[1] = 'E';
    pos.header[2] = 'H';
    pos.header[3] = '1';
    pos.header[4] = '0';
975 976 977 978
    pos.p = 0;
    pos.lat_lon_ele[0] = lat;
    pos.lat_lon_ele[1] = lon;
    pos.lat_lon_ele[2] = alt;
979 980 981 982 983 984 985 986 987 988 989 990 991 992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004
    pos.psi_the_phi[0] = roll;
    pos.psi_the_phi[1] = pitch;
    pos.psi_the_phi[2] = yaw;
    pos.gear_flap_vect[0] = 0.0f;
    pos.gear_flap_vect[1] = 0.0f;
    pos.gear_flap_vect[2] = 0.0f;

    writeBytes((const char*)&pos, sizeof(pos));

//    pos.header[0] = 'V';
//    pos.header[1] = 'E';
//    pos.header[2] = 'H';
//    pos.header[3] = '1';
//    pos.header[4] = '0';
//    pos.p = 0;
//    pos.lat_lon_ele[0] = -999;
//    pos.lat_lon_ele[1] = -999;
//    pos.lat_lon_ele[2] = -999;
//    pos.psi_the_phi[0] = -999;
//    pos.psi_the_phi[1] = -999;
//    pos.psi_the_phi[2] = -999;
//    pos.gear_flap_vect[0] = -999;
//    pos.gear_flap_vect[1] = -999;
//    pos.gear_flap_vect[2] = -999;

//    writeBytes((const char*)&pos, sizeof(pos));
1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019
}

/**
 * Sets a random position with an offset of max 1/1000 degree
 * and max 100 m altitude
 */
void QGCXPlaneLink::setRandomPosition()
{
    // Initialize generator
    srand(0);

    double offLat = rand() / static_cast<double>(RAND_MAX) / 500.0 + 1.0/500.0;
    double offLon = rand() / static_cast<double>(RAND_MAX) / 500.0 + 1.0/500.0;
    double offAlt = rand() / static_cast<double>(RAND_MAX) * 200.0 + 100.0;

1020
    if (mav->getAltitudeAMSL() + offAlt < 0)
1021 1022 1023 1024 1025 1026
    {
        offAlt *= -1.0;
    }

    setPositionAttitude(mav->getLatitude() + offLat,
                        mav->getLongitude() + offLon,
1027
                        mav->getAltitudeAMSL() + offAlt,
1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039 1040 1041 1042 1043
                        mav->getRoll(),
                        mav->getPitch(),
                        mav->getYaw());
}

void QGCXPlaneLink::setRandomAttitude()
{
    // Initialize generator
    srand(0);

    double roll = rand() / static_cast<double>(RAND_MAX) * 2.0 - 1.0;
    double pitch = rand() / static_cast<double>(RAND_MAX) * 2.0 - 1.0;
    double yaw = rand() / static_cast<double>(RAND_MAX) * 2.0 - 1.0;

    setPositionAttitude(mav->getLatitude(),
                        mav->getLongitude(),
1044
                        mav->getAltitudeAMSL(),
1045 1046 1047 1048 1049
                        roll,
                        pitch,
                        yaw);
}

1050 1051 1052 1053 1054 1055 1056
/**
 * @brief Connect the connection.
 *
 * @return True if connection has been established, false if connection couldn't be established.
 **/
bool QGCXPlaneLink::connectSimulation()
{
1057 1058 1059 1060 1061 1062 1063 1064 1065
    if (connectState) {
        qDebug() << "Simulation already active";
    } else {
        qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
        // XXX Hack
        storeSettings();

        start(HighPriority);
    }
1066

1067
    return true;
1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089
}

/**
 * @brief Check if connection is active.
 *
 * @return True if link is connected, false otherwise.
 **/
bool QGCXPlaneLink::isConnected()
{
    return connectState;
}

QString QGCXPlaneLink::getName()
{
    return name;
}

void QGCXPlaneLink::setName(QString name)
{
    this->name = name;
    //    emit nameChanged(this->name);
}