QGCFlightGearLink.cc

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

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
 *   @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 <iostream>

#include "QGCFlightGearLink.h"
#include "QGC.h"
#include "QGCFileDialog.h"
#include "QGCMessageBox.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(UASInterface* mav, QString startupArguments, QString remoteHost, QHostAddress host, quint16 port) :
    flightGearVersion(3),
    startupArguments(startupArguments),
    _sensorHilEnabled(true),
    barometerOffsetkPa(0.0f),
    _udpCommSocket(NULL),
    _fgProcess(NULL)
{
    // 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+mav->getUASID();
    this->connectState = false;
    this->currentPort = 49000+mav->getUASID();
    this->mav = mav;
    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);
}

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

/// @brief Runs the simulation thread. We do setup work here which needs to happen in the seperate thread.
void QGCFlightGearLink::run()
{
    Q_ASSERT(mav);
    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);
    QObject::connect(_udpCommSocket, SIGNAL(readyRead()), this, SLOT(readBytes()));
    
    
    // Connect to the various HIL signals that we use to then send information across the UDP protocol to FlightGear.
    connect(mav, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)),
            this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8)));
    connect(this, SIGNAL(hilStateChanged(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)),
            mav, SLOT(sendHilState(quint64, float, float, float, float,float, float, double, double, double, float, float, float, float, float, float, float, float)));
    connect(this, SIGNAL(sensorHilGpsChanged(quint64, double, double, double, int, float, float, float, float, float, float, float, int)),
            mav, SLOT(sendHilGps(quint64, double, double, double, int, float, float, float, float, float, float, float, int)));
    connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)),
            mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)));
    connect(this, SIGNAL(sensorHilOpticalFlowChanged(quint64, qint16, qint16, float,float, quint8, float)),
            mav, SLOT(sendHilOpticalFlow(quint64, qint16, qint16, float, float, quint8, float)));
    
    // Start a new QProcess to run FlightGear in
    _fgProcess = new QProcess(this);
    Q_CHECK_PTR(_fgProcess);
    _fgProcess->moveToThread(this);
    
    connect(_fgProcess, SIGNAL(error(QProcess::ProcessError)), this, SLOT(processError(QProcess::ProcessError)));
#ifdef DEBUG_FLIGHTGEAR_CONNECT
    connect(_fgProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(_printFgfsOutput()));
    connect(_fgProcess, SIGNAL(readyReadStandardError()), this, SLOT(_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);
                }
            }