LibrePilot/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp

/**
 ******************************************************************************
 *
 * @file       flightgearbridge.h
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @addtogroup GCSPlugins GCS Plugins
 * @{
 * @addtogroup HITLPlugin HITL Plugin
 * @{
 * @brief The Hardware In The Loop plugin
 *****************************************************************************/
/*
 * This program 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.
 *
 * This program 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 this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include "fgsimulator.h"
#include "extensionsystem/pluginmanager.h"
#include "coreplugin/icore.h"
#include "coreplugin/threadmanager.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

// FGSimulator::FGSimulator(QString hostAddr, int outPort, int inPort, bool manual, QString binPath, QString dataPath) :
// Simulator(hostAddr, outPort, inPort,  manual, binPath, dataPath),
// fgProcess(NULL)
// {
//// Note: Only tested on windows 7
// #if defined(Q_WS_WIN)
// cmdShell = QString("c:/windows/system32/cmd.exe");
// #else
// cmdShell = QString("bash");
// #endif
// }

FGSimulator::FGSimulator(const SimulatorSettings & params) :
    Simulator(params)
{
    udpCounterFGrecv  = 0;
    udpCounterGCSsend = 0;
}

FGSimulator::~FGSimulator()
{
    disconnect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead()));
}

void FGSimulator::setupUdpPorts(const QString & host, int inPort, int outPort)
{
    Q_UNUSED(outPort);

    if (inSocket->bind(QHostAddress(host), inPort)) {
        emit processOutput("Successfully bound to address " + host + " on port " + QString::number(inPort) + "\n");
    } else {
        emit processOutput("Cannot bind to address " + host + " on port " + QString::number(inPort) + "\n");
    }
}

bool FGSimulator::setupProcess()
{
    QMutexLocker locker(&lock);

    // Copy FlightGear generic protocol configuration file to the FG protocol directory
    // NOTE: Not working on Windows 7, if FG is installed in the "Program Files",
    // likelly due to permissions. The file should be manually copied to data/Protocol/opfgprotocol.xml
    // QFile xmlFile(":/flightgear/genericprotocol/opfgprotocol.xml");
    // xmlFile.open(QIODevice::ReadOnly | QIODevice::Text);
    // QString xml = xmlFile.readAll();
    // xmlFile.close();
    // QFile xmlFileOut(pathData + "/Protocol/opfgprotocol.xml");
    // xmlFileOut.open(QIODevice::WriteOnly | QIODevice::Text);
    // xmlFileOut.write(xml.toAscii());
    // xmlFileOut.close();

    Qt::HANDLE mainThread = QThread::currentThreadId();

    qDebug() << "setupProcess Thread: " << mainThread;

    simProcess = new QProcess();
    simProcess->setReadChannelMode(QProcess::MergedChannels);
    connect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead()));
    // Note: Only tested on windows 7
#if defined(Q_WS_WIN)
    QString cmdShell("c:/windows/system32/cmd.exe");
#else
    QString cmdShell("bash");
#endif

    // Start shell (Note: Could not start FG directly on Windows, only through terminal!)
    simProcess->start(cmdShell);
    if (simProcess->waitForStarted() == false) {
        emit processOutput("Error:" + simProcess->errorString());
        return false;
    }

    // Setup arguments
    // Note: The input generic protocol is set to update at a much higher rate than the actual updates are sent by the GCS.
    // If this is not done then a lag will be introduced by FlightGear, likelly because the receive socket buffer builds up during startup.
    QString args("--fg-root=\"" + settings.dataPath + "\" " +
                 "--timeofday=noon " +
                 "--httpd=5400 " +
                 "--enable-hud " +
                 "--in-air " +
                 "--altitude=3000 " +
                 "--vc=100 " +
                 "--log-level=alert " +
                 "--generic=socket,out,20," + settings.hostAddress + "," + QString::number(settings.inPort) + ",udp,opfgprotocol");
    if (settings.manualControlEnabled) { // <--[BCH] What does this do? Why does it depend on ManualControl?
        args.append(" --generic=socket,in,400," + settings.remoteAddress + "," + QString::number(settings.outPort) + ",udp,opfgprotocol");
    }

    // Start FlightGear - only if checkbox is selected in HITL options page
    if (settings.startSim) {
        QString cmd("\"" + settings.binPath + "\" " + args + "\n");
        simProcess->write(cmd.toAscii());
    } else {
        emit processOutput("Start Flightgear from the command line with the following arguments: \n\n" + args + "\n\n" +
                           "You can optionally run Flightgear from a networked computer.\n" +
                           "Make sure the computer running Flightgear can can ping your local interface adapter. ie." + settings.hostAddress + "\n"
                           "Remote computer must have the correct OpenPilot protocol installed.");
    }

    udpCounterGCSsend = 0;

    return true;
}

void FGSimulator::processReadyRead()
{
    QByteArray bytes = simProcess->readAllStandardOutput();
    QString str(bytes);

    if (!str.contains("Error reading data")) { // ignore error
        emit processOutput(str);
    }
}

void FGSimulator::transmitUpdate()
{
    ActuatorDesired::DataFields actData;
    FlightStatus::DataFields flightStatusData    = flightStatus->getData();
    ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData();
    float ailerons = -1;
    float elevator = -1;
    float rudder   = -1;
    float throttle = -1;

    if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) {
        // Read joystick input
        if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) {
            // Note: Pitch sign is reversed in FG ?
            ailerons = manCtrlData.Roll;
            elevator = -manCtrlData.Pitch;
            rudder   = manCtrlData.Yaw;
            throttle = manCtrlData.Throttle;
        }
    } else {
        // Read ActuatorDesired from autopilot
        actData  = actDesired->getData();

        ailerons = actData.Roll;
        elevator = -actData.Pitch;
        rudder   = actData.Yaw;
        throttle = actData.Throttle;
    }

    int allowableDifference = 10;

    // qDebug() << "UDP sent:" << udpCounterGCSsend << " - UDP Received:" << udpCounterFGrecv;

    if (udpCounterFGrecv == udpCounterGCSsend) {
        udpCounterGCSsend = 0;
    }

    if ((udpCounterGCSsend < allowableDifference) || (udpCounterFGrecv == 0)) { // FG udp queue is not delayed
        udpCounterGCSsend++;

        // Send update to FlightGear
        QString cmd;
        cmd = QString("%1,%2,%3,%4,%5\n")
              .arg(ailerons) // ailerons
              .arg(elevator) // elevator
              .arg(rudder) // rudder
              .arg(throttle) // throttle
              .arg(udpCounterGCSsend); // UDP packet counter delay

        QByteArray data = cmd.toAscii();

        if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) {
            emit processOutput("Error sending UDP packet to FG: " + outSocket->errorString() + "\n");
        }
    } else {
        // don't send new packet. Flightgear cannot process UDP fast enough.
        // V1.9.1 reads udp packets at set frequency and will get delayed if packets are sent too fast
        // V2.0 does not currently work with --generic-protocol
    }

    if (settings.manualControlEnabled) {
        actData.Roll     = ailerons;
        actData.Pitch    = -elevator;
        actData.Yaw      = rudder;
        actData.Throttle = throttle;
        // actData.NumLongUpdates = (float)udpCounterFGrecv;
        // actData.UpdateTime = (float)udpCounterGCSsend;
        actDesired->setData(actData);
    }
}


void FGSimulator::processUpdate(const QByteArray & inp)
{
    // TODO: this does not use the FLIGHT_PARAM structure, it should!
    // Split
    QString data(inp);
    QStringList fields = data.split(",");
    // Get xRate (deg/s)
    // float xRate = fields[0].toFloat() * 180.0/M_PI;
    // Get yRate (deg/s)
    // float yRate = fields[1].toFloat() * 180.0/M_PI;
    // Get zRate (deg/s)
    // float zRate = fields[2].toFloat() * 180.0/M_PI;
    // Get xAccel (m/s^2)
    float xAccel    = fields[3].toFloat() * FT2M;
    // Get yAccel (m/s^2)
    float yAccel    = fields[4].toFloat() * FT2M;
    // Get xAccel (m/s^2)
    float zAccel    = fields[5].toFloat() * FT2M;
    // Get pitch (deg)
    float pitch     = fields[6].toFloat();
    // Get pitchRate (deg/s)
    float pitchRate = fields[7].toFloat();
    // Get roll (deg)
    float roll     = fields[8].toFloat();
    // Get rollRate (deg/s)
    float rollRate = fields[9].toFloat();
    // Get yaw (deg)
    float yaw          = fields[10].toFloat();
    // Get yawRate (deg/s)
    float yawRate      = fields[11].toFloat();
    // Get latitude (deg)
    float latitude     = fields[12].toFloat();
    // Get longitude (deg)
    float longitude    = fields[13].toFloat();
    // Get heading (deg)
    float heading      = fields[14].toFloat();
    // Get altitude (m)
    float altitude_msl = fields[15].toFloat() * FT2M;
    // Get altitudeAGL (m)
    float altitude_agl = fields[16].toFloat() * FT2M;
    // Get groundspeed (m/s)
    float groundspeed  = fields[17].toFloat() * KT2MPS;
    // Get airspeed (m/s)
    float airspeed     = fields[18].toFloat() * KT2MPS;
    // Get temperature (degC)
    float temperature  = fields[19].toFloat();
    // Get pressure (kpa)
    float pressure     = fields[20].toFloat() * INHG2KPA;
    // Get VelocityState Down (cm/s)
    float velocityStateDown  = -fields[21].toFloat() * FPS2CMPS;
    // Get VelocityState East (cm/s)
    float velocityStateEast  = fields[22].toFloat() * FPS2CMPS;
    // Get VelocityState Down (cm/s)
    float velocityStateNorth = fields[23].toFloat() * FPS2CMPS;

    // Get UDP packets received by FG
    int n = fields[24].toInt();

    udpCounterFGrecv = n;

    ///////
    // Output formatting
    ///////
    Output2Hardware out;
    memset(&out, 0, sizeof(Output2Hardware));

    float NED[3];
    // convert from cm back to meters

    double LLA[3] = { latitude, longitude, altitude_msl };
    double ECEF[3];
    double RNE[9];
    Utils::CoordinateConversions().RneFromLLA(LLA, (double(*)[3])RNE);
    Utils::CoordinateConversions().LLA2ECEF(LLA, ECEF);
    Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float(*)[3])RNE, NED);


    // Update GPS Position objects
    out.latitude    = latitude * 1e7;
    out.longitude   = longitude * 1e7;
    out.altitude    = altitude_msl;
    out.agl = altitude_agl;
    out.groundspeed = groundspeed;

    out.calibratedAirspeed = airspeed;


    // Update BaroSensor object
    out.temperature = temperature;
    out.pressure    = pressure;

    // Update attState object
    out.roll      = roll;       // roll;
    out.pitch     = pitch;     // pitch
    out.heading   = yaw; // yaw

    out.dstN      = NED[0];
    out.dstE      = NED[1];
    out.dstD      = NED[2];

    // Update VelocityState.{North,East,Down}
    out.velNorth  = velocityStateNorth;
    out.velEast   = velocityStateEast;
    out.velDown   = velocityStateDown;

    // Update gyroscope sensor data
    out.rollRate  = rollRate;
    out.pitchRate = pitchRate;
    out.yawRate   = yawRate;

    // Update accelerometer sensor data
    out.accX      = xAccel;
    out.accY      = yAccel;
    out.accZ      = -zAccel;

    updateUAVOs(out);
}