Skip to content
Snippets Groups Projects
QGCXPlaneLink.cc 34.5 KiB
Newer Older
  • Learn to ignore specific revisions
  • /*=====================================================================
    
    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>
    
    #include <iostream>
    #include "QGCXPlaneLink.h"
    #include "QGC.h"
    #include <QHostInfo>
    
    Lorenz Meier's avatar
    Lorenz Meier committed
    #include "UASInterface.h"
    
    #include "QGCMessageBox.h"
    
    QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress localHost, quint16 localPort) :
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        remoteHost(QHostAddress("127.0.0.1")),
        remotePort(49000),
    
        barometerOffsetkPa(-8.0f),
    
        airframeID(QGCXPlaneLink::AIRFRAME_UNKNOWN),
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        xPlaneConnected(false),
    
        xPlaneVersion(0),
        simUpdateLast(QGC::groundTimeMilliseconds()),
    
        simUpdateFirst(0),
    
        simUpdateLastText(QGC::groundTimeMilliseconds()),
    
        simUpdateLastGroundTruth(QGC::groundTimeMilliseconds()),
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        simUpdateHz(0),
    
        _sensorHilEnabled(true),
        _should_exit(false)
    
        // 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);
    
    
        this->localHost = localHost;
        this->localPort = localPort/*+mav->getUASID()*/;
    
        this->name = tr("X-Plane Link (localPort:%1)").arg(localPort);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        loadSettings();
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        storeSettings();
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        // Tell the thread to exit
    
        _should_exit = true;
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        // Wait for it to exit
        wait();
    
    
    //    if(connectState) {
    //       disconnectSimulation();
    //    }
    
    Lorenz Meier's avatar
    Lorenz Meier committed
    void QGCXPlaneLink::loadSettings()
    {
        // Load defaults from settings
        QSettings settings;
        settings.beginGroup("QGC_XPLANE_LINK");
    
        setRemoteHost(settings.value("REMOTE_HOST", QString("%1:%2").arg(remoteHost.toString()).arg(remotePort)).toString());
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        setVersion(settings.value("XPLANE_VERSION", 10).toInt());
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        selectAirframe(settings.value("AIRFRAME", "default").toString());
    
        _sensorHilEnabled = settings.value("SENSOR_HIL", _sensorHilEnabled).toBool();
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        settings.endGroup();
    }
    
    void QGCXPlaneLink::storeSettings()
    {
        // Store settings
        QSettings settings;
        settings.beginGroup("QGC_XPLANE_LINK");
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        settings.setValue("REMOTE_HOST", QString("%1:%2").arg(remoteHost.toString()).arg(remotePort));
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        settings.setValue("XPLANE_VERSION", xPlaneVersion);
        settings.setValue("AIRFRAME", airframeName);
    
        settings.setValue("SENSOR_HIL", _sensorHilEnabled);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        settings.endGroup();
    }
    
    
    Lorenz Meier's avatar
    Lorenz Meier committed
    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
    void QGCXPlaneLink::setVersion(unsigned int version)
    {
        bool changed = (xPlaneVersion != version);
        xPlaneVersion = version;
        if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion));
    }
    
        if (!mav) {
            emit statusMessage("No MAV present");
            return;
        }
    
        if (connectState) {
            emit statusMessage("Already connected");
            return;
        }
    
    
        socket = new QUdpSocket(this);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        socket->moveToThread(this);
    
        connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint);
    
        if (!connectState) {
    
            emit statusMessage("Binding socket failed!");
    
            delete socket;
            socket = NULL;
            return;
        }
    
    
        QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
    
        UAS* uas = dynamic_cast<UAS*>(mav);
        if (uas)
        {
    
            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);
    
    
            uas->startHil();
        }
    
    #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
        }
    
        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));
    
        ip.use_ip = 1;
    
        writeBytes((const char*)&ip, sizeof(ip));
    
    
        while(!_should_exit) {
            QCoreApplication::processEvents();
            QGC::SLEEP::msleep(5);
        }
    
        uas = dynamic_cast<UAS*>(mav);
        if (uas)
    
            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)));
    
            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)));
    
    
            // Do not toggle HIL state on the UAS - this is not the job of this link, but of the
            // UAS object
        }
    
        connectState = false;
    
        socket->close();
    
        socket->deleteLater();
    
        socket = NULL;
    
        emit simulationDisconnected();
        emit simulationConnected(false);
    
    void QGCXPlaneLink::setPort(int localPort)
    
        this->localPort = localPort;
    
        disconnectSimulation();
        connectSimulation();
    }
    
    void QGCXPlaneLink::processError(QProcess::ProcessError err)
    {
    
        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;
    
        
        
        QGCMessageBox::critical(tr("X-Plane HIL"), msg);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
    QString QGCXPlaneLink::getRemoteHost()
    {
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        return QString("%1:%2").arg(remoteHost.toString()).arg(remotePort);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
     * @param newHost Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551
    
    Lorenz Meier's avatar
    Lorenz Meier committed
    void QGCXPlaneLink::setRemoteHost(const QString& newHost)
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        if (newHost.length() == 0)
            return;
    
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        if (newHost.contains(":"))
    
    Lorenz Meier's avatar
    Lorenz Meier committed
            QHostInfo info = QHostInfo::fromName(newHost.split(":").first());
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                // Add newHost
                QList<QHostAddress> newHostAddresses = info.addresses();
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                for (int i = 0; i < newHostAddresses.size(); i++)
    
                {
                    // Exclude loopback IPv4 and all IPv6 addresses
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                    if (!newHostAddresses.at(i).toString().contains(":"))
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                        address = newHostAddresses.at(i);
    
                remoteHost = address;
                // Set localPort according to user input
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                remotePort = newHost.split(":").last().toInt();
    
    Lorenz Meier's avatar
    Lorenz Meier committed
            QHostInfo info = QHostInfo::fromName(newHost);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                // Add newHost
    
                remoteHost = info.addresses().first();
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                if (remotePort == 0) remotePort = 49000;
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        if (isConnected())
        {
            disconnectSimulation();
            connectSimulation();
        }
    
    
        emit remoteChanged(QString("%1:%2").arg(remoteHost.toString()).arg(remotePort));
    }
    
    
    void QGCXPlaneLink::updateActuators(quint64 time, float act1, float act2, float act3, float act4, float act5, float act6, float act7, float act8)
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
    
        // 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
            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;
    //        }
    
            // Throttle
            writeBytes((const char*)&p, sizeof(p));
        }
    
    void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode)
    
        #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';
    
    
        Q_UNUSED(time);
        Q_UNUSED(systemMode);
        Q_UNUSED(navMode);
    
        bool isFixedWing = true;
    
    Lorenz Meier's avatar
    Lorenz Meier committed
    
        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;
        }
    
        else if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
        {
            qDebug() << "MAV_TYPE_QUADROTOR";
    
            // Individual effort will be provided directly to the actuators on Xplane quadrotor.
    
            p.f[0] = yawRudder;
    
            p.f[2] = throttle;
            p.f[3] = pitchElevator;
    
    
    Lorenz Meier's avatar
    Lorenz Meier committed
            p.f[0] = -pitchElevator;
            p.f[1] = rollAilerons;
            p.f[2] = yawRudder;
        }
    
        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));
        }
    
    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;
    }
    
    
    void QGCXPlaneLink::writeBytes(const char* data, qint64 size)
    {
    
    
        // If socket exists and is connected, transmit the data
        if (socket && connectState)
    
            socket->writeDatagram(data, size, remoteHost, remotePort);
    
     * @brief Read all pending packets from the interface.
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        // Only emit updates on attitude message
        bool emitUpdate = false;
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        quint16 fields_changed = 0;
    
        char data[maxLength];
        QHostAddress sender;
        quint16 senderPort;
    
        unsigned int s = socket->pendingDatagramSize();
    
        if (s > maxLength) std::cerr << __FILE__ << __LINE__ << " UDP datagram overflow, allowed to read less bytes than datagram size: " << s << std::endl;
    
        socket->readDatagram(data, maxLength, &sender, &senderPort);
    
        if (s > maxLength) {
        	std::string headStr = std::string(data, data+5);
        	std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl;
        }
    
        // Calculate the number of data segments a 36 bytes
        // XPlane always has 5 bytes header: 'DATA@'
        unsigned nsegs = (s-5)/36;
    
        //qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
    
        #pragma pack(push, 1)
        struct payload {
            int index;
            float f[8];
        } p;
        #pragma pack(pop)
    
        bool oldConnectionState = xPlaneConnected;
    
    
        if (data[0] == 'D' &&
                data[1] == 'A' &&
                data[2] == 'T' &&
                data[3] == 'A')
        {
    
            if (oldConnectionState != xPlaneConnected) {
                simUpdateFirst = QGC::groundTimeMilliseconds();
            }
    
    
            for (unsigned i = 0; i < nsegs; i++)
            {
                // Get index
                unsigned ioff = (5+i*36);;
                memcpy(&(p), data+ioff, sizeof(p));
    
                if (p.index == 3)
                {
    
                    ind_airspeed = p.f[5] * 0.44704f;
                    true_airspeed = p.f[6] * 0.44704f;
    
                    //qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
                }
    
                if (p.index == 4)
                {
    
    				// 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);
    
                // atmospheric pressure aircraft for XPlane 9 and 10
    
                else if (p.index == 6)
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                {
    
                    // inHg to hPa (hecto Pascal / millibar)
                    abs_pressure = p.f[0] * 33.863886666718317f;
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                    temperature = p.f[1];
    
                    fields_changed |= (1 << 9) | (1 << 12);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                }
    
                // 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);
    //            }
    
                else if ((xPlaneVersion == 10 && p.index == 16) || (xPlaneVersion == 9 && p.index == 17))
    
                    // Cross checked with XPlane flight
                    pitchspeed = p.f[0];
                    rollspeed = p.f[1];
                    yawspeed = p.f[2];
    
                    fields_changed |= (1 << 3) | (1 << 4) | (1 << 5);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18))
    
                    //qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
    
                    pitch = p.f[0] / 180.0f * M_PI;
                    roll = p.f[1] / 180.0f * M_PI;
    
                    yaw = p.f[2] / 180.0f * M_PI;
    
                    // X-Plane expresses yaw as 0..2 PI
                    if (yaw > M_PI) {
    
    Don Gagne's avatar
    Don Gagne committed
                        yaw -= 2.0f * static_cast<float>(M_PI);
    
    Don Gagne's avatar
    Don Gagne committed
                        yaw += 2.0f * static_cast<float>(M_PI);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
    
                    if (yawmag > M_PI) {
    
    Don Gagne's avatar
    Don Gagne committed
                        yawmag -= 2.0f * static_cast<float>(M_PI);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                    }
                    if (yawmag < -M_PI) {
    
    Don Gagne's avatar
    Don Gagne committed
                        yawmag += 2.0f * static_cast<float>(M_PI);
    
                    // 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;
    
                    fields_changed |= (1 << 6) | (1 << 7) | (1 << 8);
    
                    double cosPhi = cos(roll);
                    double sinPhi = sin(roll);
                    double cosThe = cos(pitch);
                    double sinThe = sin(pitch);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                    double cosPsi = cos(0.0);
                    double sinPsi = sin(0.0);
    
    
                    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;
    
    
    //                qDebug() << "yaw mag:" << p.f[2] << "x" << xmag << "y" << ymag;
    //                qDebug() << "yaw mag in body:" << magbody(0) << magbody(1) << magbody(2);
    
    
                    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
                    emitUpdate = true;
    
    //            else if (p.index == 19)
    //            {
    //                qDebug() << "ATT:" << p.f[0] << p.f[1] << p.f[2];
    //            }
    
    			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
    
                else if (p.index == 21)
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                {
    
                    // moving 'up' in XPlane is positive, but its negative in NED
                    // for us.
                    vz = -p.f[4];
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                }
    
                    //qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2];
    
                    //qDebug() << "THROTTLE" << p.f[0] << p.f[1] << p.f[2] << p.f[3];
    
                    //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];
    
                    //qDebug() << "CONTROLS" << "ail" << p.f[0] << "elev" << p.f[1] << "rudder" << p.f[2] << "nwheel" << p.f[3];
    
                    //qDebug() << "UNKNOWN #" << p.index << p.f[0] << p.f[1] << p.f[2] << p.f[3];
    
        }
        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')
        {
    
    
        // Wait for 0.5s before actually using the data, so that all fields are filled
        if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) {
            return;
        }
    
    
        if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 3)
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        {
    
            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();
            }
    
    
            simUpdateLast = QGC::groundTimeMilliseconds();
    
    
    Lorenz Meier's avatar
    Lorenz Meier committed
            {
    
                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;
    
    
                /* measured pressure in hPa, plus offset to simulate weather effects / offsets */
                double p = abs_pressure / 10.0 + barometerOffsetkPa;
    
    
                /*
                 * Solve:
                 *
                 *     /        -(aR / g)     \
                 *    | (p / p1)          . T1 | - T1
                 *     \                      /
                 * h = -------------------------------  + h1
                 *                   a
                 */
                pressure_alt = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
    
    
                // set pressure alt to changed
                fields_changed |= (1 << 11);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
    
                emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed,
    
                                            xmag, ymag, zmag, abs_pressure, diff_pressure / 100.0, pressure_alt, temperature, fields_changed);
    
                // XXX make these GUI-configurable and add randomness
                int gps_fix_type = 3;
    
    Don Gagne's avatar
    Don Gagne committed
                float eph = 0.3f;
                float epv = 0.6f;
    
                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,
    
                                     pitchspeed, yawspeed, lat, lon, alt,
                                     vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
            }
    
            // Limit ground truth to 25 Hz
    
            if (QGC::groundTimeMilliseconds() - simUpdateLastGroundTruth > 40) {
    
                emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
                                           pitchspeed, yawspeed, lat, lon, alt,
                                           vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
    
                simUpdateLastGroundTruth = QGC::groundTimeMilliseconds();
            }
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        }
    
        if (!oldConnectionState && xPlaneConnected)
        {
    
            emit statusMessage(tr("Receiving from XPlane."));
    
        //    // 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()
    {
    
            _should_exit = true;
            wait();
        } else {
            emit simulationDisconnected();
            emit simulationConnected(false);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
    void QGCXPlaneLink::selectAirframe(const QString& plane)
    
    Lorenz Meier's avatar
    Lorenz Meier committed
            if (plane.contains("MK") && airframeID != AIRFRAME_QUAD_X_MK_10INCH_I2C)
    
            {
                airframeID = AIRFRAME_QUAD_X_MK_10INCH_I2C;
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                emit airframeChanged("QRO_X / MK");
    
    Lorenz Meier's avatar
    Lorenz Meier committed
            else if (plane.contains("ARDRONE") && airframeID != AIRFRAME_QUAD_X_ARDRONE)
    
            {
                airframeID = AIRFRAME_QUAD_X_ARDRONE;
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                emit airframeChanged("QRO_X / ARDRONE");
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                bool changed = (airframeID != AIRFRAME_QUAD_DJI_F450_PWM);
    
                airframeID = AIRFRAME_QUAD_DJI_F450_PWM;
    
    Lorenz Meier's avatar
    Lorenz Meier committed
                if (changed) emit airframeChanged("QRO_X / DJI-F450 / PWM");
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        else
        {
    
    Lorenz Meier's avatar
    Lorenz Meier committed
            bool changed = (airframeID != AIRFRAME_UNKNOWN);
    
    Lorenz Meier's avatar
    Lorenz Meier committed
            airframeID = AIRFRAME_UNKNOWN;
    
    Lorenz Meier's avatar
    Lorenz Meier committed
            if (changed) emit airframeChanged("X Plane default");
    
    Lorenz Meier's avatar
    Lorenz Meier committed
        }
    
    }
    
    void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw)
    {
    
        #pragma pack(push, 1)
    
            char header[5];
    
            quint32 p;
            double lat_lon_ele[3];
            float psi_the_phi[3];
            float gear_flap_vect[3];
        } pos;
    
        #pragma pack(pop)
    
        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] = lat;
        pos.lat_lon_ele[1] = lon;
        pos.lat_lon_ele[2] = alt;
    
        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));