QGCFlightGearLink.cc

/****************************************************************************
 *
 *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/


/**
 * @file
 *   @brief Definition of UDP connection (server) for unmanned vehicles
 *   @see Flightgear Manual http://mapserver.flightgear.org/getstart.pdf
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *   @author Thomas Gubler <thomasgubler@student.ethz.ch>
 *
 */

#include <QTimer>
#include <QList>
#include <QDebug>
#include <QMutexLocker>
#include <QHostInfo>
#include <QMessageBox>
#include <QClipboard>

#include <iostream>
#include <Eigen/Eigen>

#include "QGCFlightGearLink.h"
#include "QGC.h"
#include "QGCQFileDialog.h"
#include "QGCMessageBox.h"
#include "QGCApplication.h"
#include "Vehicle.h"
#include "UAS.h"
#include "QGroundControlQmlGlobal.h"

// FlightGear _fgProcess start and connection is quite fragile. Uncomment the define below to get higher level of debug output
// for tracking down problems.
//#define DEBUG_FLIGHTGEAR_CONNECT

QGCFlightGearLink::QGCFlightGearLink(Vehicle* vehicle, QString startupArguments, QString remoteHost, QHostAddress host, quint16 port)
    : _vehicle(vehicle)
    , _udpCommSocket(NULL)
    , _fgProcess(NULL)
    , flightGearVersion(3)
    , startupArguments(startupArguments)
    , _sensorHilEnabled(true)
    , barometerOffsetkPa(0.0f)
{
    // 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->host = host;
    this->port = port + _vehicle->id();
    this->connectState = false;
    this->currentPort = 49000 + _vehicle->id();
    this->name = tr("FlightGear 3.0+ Link (port:%1)").arg(port);
    setRemoteHost(remoteHost);

    // We need a mechanism so show error message from our FGLink thread on the UI thread. This signal connection will do that for us.
    connect(this, &QGCFlightGearLink::showCriticalMessageFromThread, qgcApp(), &QGCApplication::criticalMessageBoxOnMainThread);
    connect(this, &QGCFlightGearLink::disconnectSim, this, &QGCFlightGearLink::disconnectSimulation);
}

QGCFlightGearLink::~QGCFlightGearLink()
{   //do not disconnect unless it is connected.
    //disconnectSimulation will delete the memory that was allocated for process, terraSync and _udpCommSocket
    if(connectState){
       disconnectSimulation();
    }
}

/// @brief Runs the simulation thread. We do setup work here which needs to happen in the separate thread.
void QGCFlightGearLink::run()
{
    Q_ASSERT(_vehicle);
    Q_ASSERT(!_fgProcessName.isEmpty());

    // We communicate with FlightGear over a UDP _udpCommSocket
    _udpCommSocket = new QUdpSocket(this);
    Q_CHECK_PTR(_udpCommSocket);
    _udpCommSocket->moveToThread(this);
    _udpCommSocket->bind(host, port, QAbstractSocket::ReuseAddressHint);
    QObject::connect(_udpCommSocket, &QUdpSocket::readyRead, this, &QGCFlightGearLink::readBytes);


    // Connect to the various HIL signals that we use to then send information across the UDP protocol to FlightGear.
    connect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCFlightGearLink::updateControls);

    connect(this, &QGCFlightGearLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState);
    connect(this, &QGCFlightGearLink::sensorHilGpsChanged, _vehicle->uas(), &UAS::sendHilGps);
    connect(this, &QGCFlightGearLink::sensorHilRawImuChanged, _vehicle->uas(), &UAS::sendHilSensors);
    connect(this, &QGCFlightGearLink::sensorHilOpticalFlowChanged, _vehicle->uas(), &UAS::sendHilOpticalFlow);

    // Start a new QProcess to run FlightGear in
    _fgProcess = new QProcess(this);
    Q_CHECK_PTR(_fgProcess);
    _fgProcess->moveToThread(this);

    connect(_fgProcess, static_cast<void (QProcess::*)(QProcess::ProcessError)>(&QProcess::error),
            this, &QGCFlightGearLink::processError);

//#ifdef DEBUG_FLIGHTGEAR_CONNECT
    connect(_fgProcess, &QProcess::readyReadStandardOutput, this, &QGCFlightGearLink::_printFgfsOutput);
    connect(_fgProcess, &QProcess::readyReadStandardError, this, &QGCFlightGearLink::_printFgfsError);
//#endif

    if (!_fgProcessWorkingDirPath.isEmpty()) {
        _fgProcess->setWorkingDirectory(_fgProcessWorkingDirPath);
        qDebug() << "Working directory" << _fgProcess->workingDirectory();
    }

#ifdef Q_OS_WIN32
    // On Windows we need to full qualify the location of the excecutable. The call to setWorkingDirectory only
    // sets the QProcess context, not the QProcess::start context. For some strange reason this is not the case on
    // OSX.
    QDir fgProcessFullyQualified(_fgProcessWorkingDirPath);
    _fgProcessName = fgProcessFullyQualified.absoluteFilePath(_fgProcessName);
#endif

#ifdef DEBUG_FLIGHTGEAR_CONNECT
    qDebug() << "\nStarting FlightGear" << _fgProcessWorkingDirPath << _fgProcessName << _fgArgList << "\n";
#endif

    _fgProcess->start(_fgProcessName, _fgArgList);
    connectState = true;

    emit simulationConnected(connectState);
    emit simulationConnected();

    exec();
}

void QGCFlightGearLink::setPort(int port)
{
    this->port = port;
    disconnectSimulation();
    connectSimulation();
}

void QGCFlightGearLink::processError(QProcess::ProcessError err)
{
    switch(err)
    {
    case QProcess::FailedToStart:
        emit showCriticalMessageFromThread(tr("FlightGear Failed to Start"), _fgProcess->errorString());
        break;
    case QProcess::Crashed:
        emit showCriticalMessageFromThread(tr("FlightGear Crashed"), tr("This is a FlightGear-related problem. Please upgrade FlightGear"));
        break;
    case QProcess::Timedout:
        emit showCriticalMessageFromThread(tr("FlightGear Start Timed Out"), tr("Please check if the path and command is correct"));
        break;
    case QProcess::WriteError:
        emit showCriticalMessageFromThread(tr("Could not Communicate with FlightGear"), tr("Please check if the path and command is correct"));
        break;
    case QProcess::ReadError:
        emit showCriticalMessageFromThread(tr("Could not Communicate with FlightGear"), tr("Please check if the path and command is correct"));
        break;
    case QProcess::UnknownError:
    default:
        emit showCriticalMessageFromThread(tr("FlightGear Error"), tr("Please check if the path and command is correct."));
        break;
    }
}

/**
 * @param host Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551
 */
void QGCFlightGearLink::setRemoteHost(const QString& host)
{
    if (host.contains(":"))
    {
        //qDebug() << "HOST: " << host.split(":").first();
        QHostInfo info = QHostInfo::fromName(host.split(":").first());
        if (info.error() == QHostInfo::NoError)
        {
            // Add host
            QList<QHostAddress> hostAddresses = info.addresses();
            QHostAddress address;
            for (int i = 0; i < hostAddresses.size(); i++)
            {
                // Exclude loopback IPv4 and all IPv6 addresses
                if (!hostAddresses.at(i).toString().contains(":"))
                {
                    address = hostAddresses.at(i);
                }
            }
            currentHost = address;
            //qDebug() << "Address:" << address.toString();
            // Set port according to user input
            currentPort = host.split(":").last().toInt();
        }
    }
    else
    {