QGCXPlaneLink.cc 30.5 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
#include <QHostInfo>

38
#include <iostream>
39
40
#include <Eigen/Eigen>

41
42
#include "QGCXPlaneLink.h"
#include "QGC.h"
43
#include "UAS.h"
Lorenz Meier's avatar
Lorenz Meier committed
44
#include "UASInterface.h"
45
#include "QGCMessageBox.h"
Don Gagne's avatar
Don Gagne committed
46
#include "HomePositionManager.h"
47

48
49
QGCXPlaneLink::QGCXPlaneLink(Vehicle* vehicle, QString remoteHost, QHostAddress localHost, quint16 localPort) :
    _vehicle(vehicle),
Lorenz Meier's avatar
Lorenz Meier committed
50
51
    remoteHost(QHostAddress("127.0.0.1")),
    remotePort(49000),
52
    socket(NULL),
53
    process(NULL),
54
    terraSync(NULL),
Lorenz Meier's avatar
Lorenz Meier committed
55
    barometerOffsetkPa(-8.0f),
56
    airframeID(QGCXPlaneLink::AIRFRAME_UNKNOWN),
Lorenz Meier's avatar
Lorenz Meier committed
57
    xPlaneConnected(false),
58
59
    xPlaneVersion(0),
    simUpdateLast(QGC::groundTimeMilliseconds()),
60
    simUpdateFirst(0),
61
    simUpdateLastText(QGC::groundTimeMilliseconds()),
62
    simUpdateLastGroundTruth(QGC::groundTimeMilliseconds()),
Lorenz Meier's avatar
Lorenz Meier committed
63
    simUpdateHz(0),
64
65
    _sensorHilEnabled(true),
    _should_exit(false)
66
{
67
68
69
70
    // 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);

71
72
    setTerminationEnabled(false);

Lorenz Meier's avatar
Lorenz Meier committed
73
74
    this->localHost = localHost;
    this->localPort = localPort/*+mav->getUASID()*/;
75
76
    connectState = false;

Lorenz Meier's avatar
Lorenz Meier committed
77
    this->name = tr("X-Plane Link (localPort:%1)").arg(localPort);
78
    setRemoteHost(remoteHost);
Lorenz Meier's avatar
Lorenz Meier committed
79
    loadSettings();
80
81
82
}

QGCXPlaneLink::~QGCXPlaneLink()
83
{
Lorenz Meier's avatar
Lorenz Meier committed
84
    storeSettings();
Lorenz Meier's avatar
Lorenz Meier committed
85
    // Tell the thread to exit
86
    _should_exit = true;
Lorenz Meier's avatar
Lorenz Meier committed
87

88
89
90
91
92
    if (socket) {
        socket->close();
        socket->deleteLater();
        socket = NULL;
    }
93
94
}

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

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

Lorenz Meier's avatar
Lorenz Meier committed
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
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
145
146
147
148
149
150
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
151

Lorenz Meier's avatar
Lorenz Meier committed
152

153
154
155
156
157
158
/**
 * @brief Runs the thread
 *
 **/
void QGCXPlaneLink::run()
{
159
    if (!_vehicle) {
160
161
162
163
164
165
166
167
        emit statusMessage("No MAV present");
        return;
    }

    if (connectState) {
        emit statusMessage("Already connected");
        return;
    }
168
169

    socket = new QUdpSocket(this);
Lorenz Meier's avatar
Lorenz Meier committed
170
    socket->moveToThread(this);
171
    connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint);
172
173
174
175
    if (!connectState) {

        emit statusMessage("Binding socket failed!");

176
        socket->deleteLater();
177
178
179
        socket = NULL;
        return;
    }
180

181
182
    emit statusMessage(tr("Waiting for XPlane.."));

183
    QObject::connect(socket, &QUdpSocket::readyRead, this, &QGCXPlaneLink::readBytes);
184

185
    connect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCXPlaneLink::updateControls, Qt::QueuedConnection);
186

187
188
189
190
    connect(this, &QGCXPlaneLink::hilGroundTruthChanged, _vehicle->uas(), &UAS::sendHilGroundTruth, Qt::QueuedConnection);
    connect(this, &QGCXPlaneLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState, Qt::QueuedConnection);
    connect(this, &QGCXPlaneLink::sensorHilGpsChanged, _vehicle->uas(), &UAS::sendHilGps, Qt::QueuedConnection);
    connect(this, &QGCXPlaneLink::sensorHilRawImuChanged, _vehicle->uas(), &UAS::sendHilSensors, Qt::QueuedConnection);
191

192
    _vehicle->uas()->startHil();
193
194
195
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

#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
225
    }
226
227

    ip.index = 0;
228
229
    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));
230
231
232
233
    ip.use_ip = 1;

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

234
235
    _should_exit = false;

236
237
238
239
    while(!_should_exit) {
        QCoreApplication::processEvents();
        QGC::SLEEP::msleep(5);
    }
240

241
    disconnect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCXPlaneLink::updateControls);
242

243
244
245
246
    disconnect(this, &QGCXPlaneLink::hilGroundTruthChanged, _vehicle->uas(), &UAS::sendHilGroundTruth);
    disconnect(this, &QGCXPlaneLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState);
    disconnect(this, &QGCXPlaneLink::sensorHilGpsChanged, _vehicle->uas(), &UAS::sendHilGps);
    disconnect(this, &QGCXPlaneLink::sensorHilRawImuChanged, _vehicle->uas(), &UAS::sendHilSensors);
247
248
    connectState = false;

249
    disconnect(socket, &QUdpSocket::readyRead, this, &QGCXPlaneLink::readBytes);
250

251
    socket->close();
252
    socket->deleteLater();
253
254
255
256
    socket = NULL;

    emit simulationDisconnected();
    emit simulationConnected(false);
257
258
}

Lorenz Meier's avatar
Lorenz Meier committed
259
void QGCXPlaneLink::setPort(int localPort)
260
{
Lorenz Meier's avatar
Lorenz Meier committed
261
    this->localPort = localPort;
262
263
264
265
266
267
    disconnectSimulation();
    connectSimulation();
}

void QGCXPlaneLink::processError(QProcess::ProcessError err)
{
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
    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;
292
    }
293
294
295
    
    
    QGCMessageBox::critical(tr("X-Plane HIL"), msg);
296
297
}

Lorenz Meier's avatar
Lorenz Meier committed
298
299
QString QGCXPlaneLink::getRemoteHost()
{
Lorenz Meier's avatar
Lorenz Meier committed
300
    return QString("%1:%2").arg(remoteHost.toString()).arg(remotePort);
Lorenz Meier's avatar
Lorenz Meier committed
301
302
}

303
/**
Lorenz Meier's avatar
Lorenz Meier committed
304
 * @param newHost Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551
305
 */
Lorenz Meier's avatar
Lorenz Meier committed
306
void QGCXPlaneLink::setRemoteHost(const QString& newHost)
307
{
Lorenz Meier's avatar
Lorenz Meier committed
308
309
310
    if (newHost.length() == 0)
        return;

Lorenz Meier's avatar
Lorenz Meier committed
311
    if (newHost.contains(":"))
312
    {
Lorenz Meier's avatar
Lorenz Meier committed
313
        QHostInfo info = QHostInfo::fromName(newHost.split(":").first());
314
315
        if (info.error() == QHostInfo::NoError)
        {
Lorenz Meier's avatar
Lorenz Meier committed
316
317
            // Add newHost
            QList<QHostAddress> newHostAddresses = info.addresses();
318
            QHostAddress address;
Lorenz Meier's avatar
Lorenz Meier committed
319
            for (int i = 0; i < newHostAddresses.size(); i++)
320
321
            {
                // Exclude loopback IPv4 and all IPv6 addresses
Lorenz Meier's avatar
Lorenz Meier committed
322
                if (!newHostAddresses.at(i).toString().contains(":"))
323
                {
Lorenz Meier's avatar
Lorenz Meier committed
324
                    address = newHostAddresses.at(i);
325
326
                }
            }
Lorenz Meier's avatar
Lorenz Meier committed
327
328
            remoteHost = address;
            // Set localPort according to user input
Lorenz Meier's avatar
Lorenz Meier committed
329
            remotePort = newHost.split(":").last().toInt();
330
331
332
333
        }
    }
    else
    {
Lorenz Meier's avatar
Lorenz Meier committed
334
        QHostInfo info = QHostInfo::fromName(newHost);
335
336
        if (info.error() == QHostInfo::NoError)
        {
Lorenz Meier's avatar
Lorenz Meier committed
337
            // Add newHost
Lorenz Meier's avatar
Lorenz Meier committed
338
            remoteHost = info.addresses().first();
Lorenz Meier's avatar
Lorenz Meier committed
339
            if (remotePort == 0) remotePort = 49000;
340
341
342
        }
    }

Lorenz Meier's avatar
Lorenz Meier committed
343
344
345
346
347
    if (isConnected())
    {
        disconnectSimulation();
        connectSimulation();
    }
348
349
350
351

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

352
void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode)
353
{
354
355
356
357
358
359
360
361
362
363
364
365
366
367
    #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';

368
369
370
371
372
    Q_UNUSED(time);
    Q_UNUSED(systemMode);
    Q_UNUSED(navMode);

    bool isFixedWing = true;
Lorenz Meier's avatar
Lorenz Meier committed
373

374
    if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR)
375
376
377
378
    {
        qDebug() << "MAV_TYPE_QUADROTOR";

        // Individual effort will be provided directly to the actuators on Xplane quadrotor.
379
        p.f[0] = yawRudder;
380
        p.f[1] = rollAilerons;
381
382
383
        p.f[2] = throttle;
        p.f[3] = pitchElevator;

384
385
        isFixedWing = false;
    }
Lorenz Meier's avatar
Lorenz Meier committed
386
387
    else
    {
388
        // direct pass-through, normal fixed-wing.
Lorenz Meier's avatar
Lorenz Meier committed
389
390
391
392
        p.f[0] = -pitchElevator;
        p.f[1] = rollAilerons;
        p.f[2] = yawRudder;
    }
393

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
    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));
    }
419
420
421

}

422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
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;
}

451
452
void QGCXPlaneLink::writeBytes(const char* data, qint64 size)
{
453
    if (!data) return;
454
455
456

    // If socket exists and is connected, transmit the data
    if (socket && connectState)
457
    {
458
        socket->writeDatagram(data, size, remoteHost, remotePort);
459
460
461
462
    }
}

/**
463
 * @brief Read all pending packets from the interface.
464
465
466
 **/
void QGCXPlaneLink::readBytes()
{
Lorenz Meier's avatar
Lorenz Meier committed
467
468
    // Only emit updates on attitude message
    bool emitUpdate = false;
Lorenz Meier's avatar
Lorenz Meier committed
469
    quint16 fields_changed = 0;
Lorenz Meier's avatar
Lorenz Meier committed
470

471
    const qint64 maxLength = 65536;
472
473
474
475
476
    char data[maxLength];
    QHostAddress sender;
    quint16 senderPort;

    unsigned int s = socket->pendingDatagramSize();
477
    if (s > maxLength) std::cerr << __FILE__ << __LINE__ << " UDP datagram overflow, allowed to read less bytes than datagram size: " << s << std::endl;
478
    socket->readDatagram(data, maxLength, &sender, &senderPort);
479
480
481
482
    if (s > maxLength) {
    	std::string headStr = std::string(data, data+5);
    	std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl;
    }
483

Lorenz Meier's avatar
Lorenz Meier committed
484
485
486
    // Calculate the number of data segments a 36 bytes
    // XPlane always has 5 bytes header: 'DATA@'
    unsigned nsegs = (s-5)/36;
487

488
    //qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
Lorenz Meier's avatar
Lorenz Meier committed
489

490
491
492
493
494
495
    #pragma pack(push, 1)
    struct payload {
        int index;
        float f[8];
    } p;
    #pragma pack(pop)
Lorenz Meier's avatar
Lorenz Meier committed
496

497
498
    bool oldConnectionState = xPlaneConnected;

499
500
501
502
503
    if (data[0] == 'D' &&
            data[1] == 'A' &&
            data[2] == 'T' &&
            data[3] == 'A')
    {
504
        xPlaneConnected = true;
505

506
507
508
509
        if (oldConnectionState != xPlaneConnected) {
            simUpdateFirst = QGC::groundTimeMilliseconds();
        }

510
511
512
513
514
515
516
517
        for (unsigned i = 0; i < nsegs; i++)
        {
            // Get index
            unsigned ioff = (5+i*36);;
            memcpy(&(p), data+ioff, sizeof(p));

            if (p.index == 3)
            {
Lorenz Meier's avatar
Lorenz Meier committed
518
519
                ind_airspeed = p.f[5] * 0.44704f;
                true_airspeed = p.f[6] * 0.44704f;
520
521
                groundspeed = p.f[7] * 0.44704;

522
523
                //qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
            }
524
525
            if (p.index == 4)
            {
526
527
528
529
530
531
532
				// 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.
533
                    Eigen::Vector3f g(0, 0, -9.80665f);
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
					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);
555
                emitUpdate = true;
556
            }
557
            // atmospheric pressure aircraft for XPlane 9 and 10
558
            else if (p.index == 6)
Lorenz Meier's avatar
Lorenz Meier committed
559
            {
560
561
                // inHg to hPa (hecto Pascal / millibar)
                abs_pressure = p.f[0] * 33.863886666718317f;
Lorenz Meier's avatar
Lorenz Meier committed
562
                temperature = p.f[1];
Lorenz Meier's avatar
Lorenz Meier committed
563
                fields_changed |= (1 << 9) | (1 << 12);
Lorenz Meier's avatar
Lorenz Meier committed
564
            }
565
566
567
568
569
570
571
572
573
574
575
            // 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);
//            }
576
            else if ((xPlaneVersion == 10 && p.index == 16) || (xPlaneVersion == 9 && p.index == 17))
577
            {
578
579
580
581
                // Cross checked with XPlane flight
                pitchspeed = p.f[0];
                rollspeed = p.f[1];
                yawspeed = p.f[2];
Lorenz Meier's avatar
Lorenz Meier committed
582
                fields_changed |= (1 << 3) | (1 << 4) | (1 << 5);
583
584

                emitUpdate = true;
585
            }
Lorenz Meier's avatar
Lorenz Meier committed
586
            else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18))
587
            {
588
                //qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
589
590
                pitch = p.f[0] / 180.0f * M_PI;
                roll = p.f[1] / 180.0f * M_PI;
591
                yaw = p.f[2] / 180.0f * M_PI;
592

593
594
                // X-Plane expresses yaw as 0..2 PI
                if (yaw > M_PI) {
Don Gagne's avatar
Don Gagne committed
595
                    yaw -= 2.0f * static_cast<float>(M_PI);
596
597
                }
                if (yaw < -M_PI) {
Don Gagne's avatar
Don Gagne committed
598
                    yaw += 2.0f * static_cast<float>(M_PI);
599
                }
Lorenz Meier's avatar
Lorenz Meier committed
600

601
                float yawmag = p.f[3] / 180.0f * M_PI;
Lorenz Meier's avatar
Lorenz Meier committed
602
603

                if (yawmag > M_PI) {
Don Gagne's avatar
Don Gagne committed
604
                    yawmag -= 2.0f * static_cast<float>(M_PI);
Lorenz Meier's avatar
Lorenz Meier committed
605
606
                }
                if (yawmag < -M_PI) {
Don Gagne's avatar
Don Gagne committed
607
                    yawmag += 2.0f * static_cast<float>(M_PI);
Lorenz Meier's avatar
Lorenz Meier committed
608
609
                }

610
611
612
613
614
615
616
                // 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;
Lorenz Meier's avatar
Lorenz Meier committed
617
                fields_changed |= (1 << 6) | (1 << 7) | (1 << 8);
Lorenz Meier's avatar
Lorenz Meier committed
618

619
620
621
622
                double cosPhi = cos(roll);
                double sinPhi = sin(roll);
                double cosThe = cos(pitch);
                double sinThe = sin(pitch);
Lorenz Meier's avatar
Lorenz Meier committed
623
624
                double cosPsi = cos(0.0);
                double sinPsi = sin(0.0);
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645

                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;

646
647
//                qDebug() << "yaw mag:" << p.f[2] << "x" << xmag << "y" << ymag;
//                qDebug() << "yaw mag in body:" << magbody(0) << magbody(1) << magbody(2);
648
649
650
651
652
653
654
655

                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
656
                emitUpdate = true;
657
            }
658

659
660
661
662
//            else if (p.index == 19)
//            {
//                qDebug() << "ATT:" << p.f[0] << p.f[1] << p.f[2];
//            }
663
664
665
666
667
668
669
			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
670
            }
671
            else if (p.index == 21)
Lorenz Meier's avatar
Lorenz Meier committed
672
            {
673
674
                vy = p.f[3];
                vx = -p.f[5];
675
676
677
                // moving 'up' in XPlane is positive, but its negative in NED
                // for us.
                vz = -p.f[4];
Lorenz Meier's avatar
Lorenz Meier committed
678
            }
679
680
            else if (p.index == 12)
            {
681
                //qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2];
682
683
684
            }
            else if (p.index == 25)
            {
685
                //qDebug() << "THROTTLE" << p.f[0] << p.f[1] << p.f[2] << p.f[3];
686
687
688
            }
            else if (p.index == 0)
            {
689
                //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];
690
691
692
            }
            else if (p.index == 11)
            {
693
                //qDebug() << "CONTROLS" << "ail" << p.f[0] << "elev" << p.f[1] << "rudder" << p.f[2] << "nwheel" << p.f[3];
694
695
696
            }
            else
            {
697
                //qDebug() << "UNKNOWN #" << p.index << p.f[0] << p.f[1] << p.f[2] << p.f[3];
698
699
            }
        }
700
701
702
703
704
705
706
707
708
709
710
711
712
713
    }
    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')
    {

714
715
716
717
718
    }
    else
    {
        qDebug() << "UNKNOWN PACKET:" << data;
    }
719

720
721
722
723
724
    // Wait for 0.5s before actually using the data, so that all fields are filled
    if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) {
        return;
    }

725
    // Send updated state
726
    if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 2)
Lorenz Meier's avatar
Lorenz Meier committed
727
    {
728
729
730
731
732
733
734
735
736
        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();
        }

Lorenz Meier's avatar
Lorenz Meier committed
737
738
        simUpdateLast = QGC::groundTimeMilliseconds();

739
        if (_sensorHilEnabled)
Lorenz Meier's avatar
Lorenz Meier committed
740
        {
741
742
743
744
745
746
747
748
749
750
751
            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;

752
753
            /* measured pressure in hPa, plus offset to simulate weather effects / offsets */
            double p = abs_pressure / 10.0 + barometerOffsetkPa;
754
755
756
757
758
759
760
761
762
763
764
765

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

Lorenz Meier's avatar
Lorenz Meier committed
766
767
            // set pressure alt to changed
            fields_changed |= (1 << 11);
Lorenz Meier's avatar
Lorenz Meier committed
768
769

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

Lorenz Meier's avatar
Lorenz Meier committed
772
773
            // XXX make these GUI-configurable and add randomness
            int gps_fix_type = 3;
Don Gagne's avatar
Don Gagne committed
774
775
            float eph = 0.3f;
            float epv = 0.6f;
Lorenz Meier's avatar
Lorenz Meier committed
776
777
778
779
780
781
782
            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,
783
784
785
786
787
                                 pitchspeed, yawspeed, lat, lon, alt,
                                 vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
        }

        // Limit ground truth to 25 Hz
788
        if (QGC::groundTimeMilliseconds() - simUpdateLastGroundTruth > 40) {
789
790
791
            emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
                                       pitchspeed, yawspeed, lat, lon, alt,
                                       vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
Lorenz Meier's avatar
Lorenz Meier committed
792

793
794
            simUpdateLastGroundTruth = QGC::groundTimeMilliseconds();
        }
Lorenz Meier's avatar
Lorenz Meier committed
795
    }
796

797
798
    if (!oldConnectionState && xPlaneConnected)
    {
799
        emit statusMessage(tr("Receiving from XPlane."));
800
801
    }

802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
    //    // 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()
{
831
    if (connectState)
832
    {
833
834
835
836
        _should_exit = true;
    } else {
        emit simulationDisconnected();
        emit simulationConnected(false);
837
    }
838
839
840
841

    return !connectState;
}

Lorenz Meier's avatar
Lorenz Meier committed
842
void QGCXPlaneLink::selectAirframe(const QString& plane)
843
{
844
    airframeName = plane;
845

846
847
    if (plane.contains("QRO"))
    {
Lorenz Meier's avatar
Lorenz Meier committed
848
        if (plane.contains("MK") && airframeID != AIRFRAME_QUAD_X_MK_10INCH_I2C)
849
850
        {
            airframeID = AIRFRAME_QUAD_X_MK_10INCH_I2C;
Lorenz Meier's avatar
Lorenz Meier committed
851
            emit airframeChanged("QRO_X / MK");
852
        }
Lorenz Meier's avatar
Lorenz Meier committed
853
        else if (plane.contains("ARDRONE") && airframeID != AIRFRAME_QUAD_X_ARDRONE)
854
855
        {
            airframeID = AIRFRAME_QUAD_X_ARDRONE;
Lorenz Meier's avatar
Lorenz Meier committed
856
            emit airframeChanged("QRO_X / ARDRONE");
857
858
859
        }
        else
        {
Lorenz Meier's avatar
Lorenz Meier committed
860
            bool changed = (airframeID != AIRFRAME_QUAD_DJI_F450_PWM);
861
            airframeID = AIRFRAME_QUAD_DJI_F450_PWM;
Lorenz Meier's avatar
Lorenz Meier committed
862
            if (changed) emit airframeChanged("QRO_X / DJI-F450 / PWM");
863
864
        }
    }
Lorenz Meier's avatar
Lorenz Meier committed
865
866
    else
    {
Lorenz Meier's avatar
Lorenz Meier committed
867
        bool changed = (airframeID != AIRFRAME_UNKNOWN);
Lorenz Meier's avatar
Lorenz Meier committed
868
        airframeID = AIRFRAME_UNKNOWN;
Lorenz Meier's avatar
Lorenz Meier committed
869
        if (changed) emit airframeChanged("X Plane default");
Lorenz Meier's avatar
Lorenz Meier committed
870
    }
871
872
873
874
}

void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw)
{
875
    #pragma pack(push, 1)
876
877
    struct VEH1_struct
    {
878
        char header[5];
879
880
881
882
883
        quint32 p;
        double lat_lon_ele[3];
        float psi_the_phi[3];
        float gear_flap_vect[3];
    } pos;
884
    #pragma pack(pop)
885

886
887
888
889
890
    pos.header[0] = 'V';
    pos.header[1] = 'E';
    pos.header[2] = 'H';
    pos.header[3] = '1';
    pos.header[4] = '0';
891
892
893
894
    pos.p = 0;
    pos.lat_lon_ele[0] = lat;
    pos.lat_lon_ele[1] = lon;
    pos.lat_lon_ele[2] = alt;
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
    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));
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
}

/**
 * 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;

936
    if (_vehicle->altitudeAMSL() + offAlt < 0)
937
938
939
940
    {
        offAlt *= -1.0;
    }

941
942
943
944
945
946
    setPositionAttitude(_vehicle->latitude() + offLat,
                        _vehicle->longitude() + offLon,
                        _vehicle->altitudeAMSL() + offAlt,
                        _vehicle->roll(),
                        _vehicle->pitch(),
                        _vehicle->uas()->getYaw());
947
948
949
950
951
952
953
954
955
956
957
}

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;

958
959
960
    setPositionAttitude(_vehicle->latitude(),
                        _vehicle->longitude(),
                        _vehicle->altitudeAMSL(),
961
962
963
964
965
                        roll,
                        pitch,
                        yaw);
}

966
967
968
969
970
971
972
/**
 * @brief Connect the connection.
 *
 * @return True if connection has been established, false if connection couldn't be established.
 **/
bool QGCXPlaneLink::connectSimulation()
{
973
974
975
976
977
978
979
980
981
    if (connectState) {
        qDebug() << "Simulation already active";
    } else {
        qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
        // XXX Hack
        storeSettings();

        start(HighPriority);
    }
982

983
    return true;
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
}

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

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

For faster browsing, not all history is shown. View entire blame