LibrePilot/ground/src/plugins/hitlnew/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

const float FGSimulator::FT2M = 0.3048;
const float FGSimulator::KT2MPS = 0.514444444;
const float FGSimulator::INHG2KPA = 3.386;
const float FGSimulator::FPS2CMPS = 30.48;


//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)
{
}

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

void FGSimulator::setupUdpPorts(const QString& host, int inPort, int outPort)
{
	inSocket->bind(QHostAddress(host), inPort);
}

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 " +
                     "--generic=socket,out,50,localhost," + QString::number(settings.inPort) + ",udp,opfgprotocol");
	if(!settings.manual)
	{
            args.append(" --generic=socket,in,400,localhost," + QString::number(settings.outPort) + ",udp,opfgprotocol");
	}

	// Start FlightGear
	QString cmd("\"" + settings.binPath + "\" " + args + "\n");
	simProcess->write(cmd.toAscii());

	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()
{
	// Read ActuatorDesired from autopilot
	ActuatorDesired::DataFields actData = actDesired->getData();
	float ailerons = actData.Roll;
	float elevator = -actData.Pitch;
	float rudder = actData.Yaw;
	float throttle = actData.Throttle;
	// Send update to FlightGear
	QString cmd;
	cmd = QString("%1,%2,%3,%4\n")
		  .arg(ailerons)
		  .arg(elevator)
		  .arg(rudder)
		  .arg(throttle);
	QByteArray data = cmd.toAscii();
	outSocket->writeDatagram(data, QHostAddress(settings.hostAddress), settings.outPort);
}


void FGSimulator::processUpdate(const QByteArray& inp)
{
        static char once=0;
	// 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 = fields[15].toFloat() * FT2M;
	// Get altitudeAGL (m)
	float altitudeAGL = 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 VelocityActual Down (cm/s)
        float velocityActualDown = fields[21].toFloat() * FPS2CMPS;
	// Get VelocityActual East (cm/s)
	float velocityActualEast = fields[22].toFloat() * FPS2CMPS;	
	// Get VelocityActual Down (cm/s)
	float velocityActualNorth = fields[23].toFloat() * FPS2CMPS;

        //run once
        HomeLocation::DataFields homeData;
        if(!once)
        {
            memset(&homeData, 0, sizeof(HomeLocation::DataFields));
            // Update homelocation
            homeData.Latitude = latitude * 10e6;
            homeData.Longitude = longitude * 10e6;
            homeData.Altitude = 0;
            double LLA[3];
            LLA[0]=latitude;
            LLA[1]=longitude;
            LLA[2]=0;
            double ECEF[3];
            Utils::CoordinateConversions().RneFromLLA(LLA,(double(*)[3])homeData.RNE);
            Utils::CoordinateConversions().LLA2ECEF(LLA,ECEF);
            homeData.ECEF[0]=ECEF[0]*100;
            homeData.ECEF[1]=ECEF[1]*100;
            homeData.ECEF[2]=ECEF[2]*100;
            homeData.Be[0]=0;
            homeData.Be[1]=0;
            homeData.Be[2]=0;
            posHome->setData(homeData);
            once=1;
        }
	
	// Update VelocityActual.{Nort,East,Down}
	VelocityActual::DataFields velocityActualData;
	memset(&velocityActualData, 0, sizeof(VelocityActual::DataFields));
	velocityActualData.North = velocityActualNorth;
	velocityActualData.East = velocityActualEast;
	velocityActualData.Down = velocityActualDown;
	velActual->setData(velocityActualData);
	
	// Update PositionActual.{Nort,East,Down}
	PositionActual::DataFields positionActualData;
	memset(&positionActualData, 0, sizeof(PositionActual::DataFields));
        positionActualData.North = 0; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
	positionActualData.East = 0; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
	positionActualData.Down = (altitude * 100); //Multiply by 100 because positionActual expects input in Centimeters.
        posActual->setData(positionActualData);

	// Update AltitudeActual object
        BaroAltitude::DataFields altActualData;
        memset(&altActualData, 0, sizeof(BaroAltitude::DataFields));
        altActualData.Altitude = altitudeAGL;
	altActualData.Temperature = temperature;
	altActualData.Pressure = pressure;
	altActual->setData(altActualData);

	// Update attActual object
	AttitudeActual::DataFields attActualData;
        memset(&attActualData, 0, sizeof(AttitudeActual::DataFields));
	attActualData.Roll = roll;
	attActualData.Pitch = pitch;
	attActualData.Yaw = yaw;
	attActualData.q1 = 0;
	attActualData.q2 = 0;
	attActualData.q3 = 0;
	attActualData.q4 = 0;
	attActual->setData(attActualData);

	// Update gps objects
        GPSPosition::DataFields gpsData;
        memset(&gpsData, 0, sizeof(GPSPosition::DataFields));
        gpsData.Altitude = altitude;
        gpsData.Heading = heading;
	gpsData.Groundspeed = groundspeed;
        gpsData.Latitude = latitude*1e7;
        gpsData.Longitude = longitude*1e7;
	gpsData.Satellites = 10;
        gpsData.Status = GPSPosition::STATUS_FIX3D;
        gpsPos->setData(gpsData);

        /*float NED[3];
        double LLA[3] = {(double) gpsData.Latitude / 1e7, (double) gpsData.Longitude / 1e7, (double) (gpsData.GeoidSeparation + gpsData.Altitude)};
        // convert from cm back to meters
        double ECEF[3] = {(double) (homeData.ECEF[0] / 100), (double) (homeData.ECEF[1] / 100), (double) (homeData.ECEF[2] / 100)};
                Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float (*)[3]) homeData.RNE, NED);

        positionActualData.North = NED[0]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
        positionActualData.East = NED[1]; //Currently hardcoded as there is no way of setting up a reference point to calculate distance
        positionActualData.Down = NED[2]; //Multiply by 100 because positionActual expects input in Centimeters.
        posActual->setData(positionActualData);*/
}