1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-12 20:08:48 +01:00
LibrePilot/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp

340 lines
12 KiB
C++
Raw Normal View History

2013-04-05 22:46:56 +02:00
/**
******************************************************************************
*
* @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
2013-04-05 22:46:56 +02:00
#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) :
2013-04-05 22:46:56 +02:00
Simulator(params)
{
udpCounterFGrecv = 0;
2013-04-05 22:46:56 +02:00
udpCounterGCSsend = 0;
}
FGSimulator::~FGSimulator()
{
disconnect(simProcess, SIGNAL(readyReadStandardOutput()), this, SLOT(processReadyRead()));
}
void FGSimulator::setupUdpPorts(const QString & host, int inPort, int outPort)
2013-04-05 22:46:56 +02:00
{
Q_UNUSED(outPort);
if (inSocket->bind(QHostAddress(host), inPort)) {
2013-04-05 22:46:56 +02:00
emit processOutput("Successfully bound to address " + host + " on port " + QString::number(inPort) + "\n");
} else {
2013-04-05 22:46:56 +02:00
emit processOutput("Cannot bind to address " + host + " on port " + QString::number(inPort) + "\n");
}
2013-04-05 22:46:56 +02:00
}
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();
2013-04-05 22:46:56 +02:00
Qt::HANDLE mainThread = QThread::currentThreadId();
qDebug() << "setupProcess Thread: " << mainThread;
2013-04-05 22:46:56 +02:00
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) {
2013-04-05 22:46:56 +02:00
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?
2013-04-05 22:46:56 +02:00
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) {
2013-04-05 22:46:56 +02:00
QString cmd("\"" + settings.binPath + "\" " + args + "\n");
simProcess->write(cmd.toAscii());
} else {
2013-04-05 22:46:56 +02:00
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
2013-04-05 22:46:56 +02:00
emit processOutput(str);
}
}
void FGSimulator::transmitUpdate()
{
ActuatorDesired::DataFields actData;
FlightStatus::DataFields flightStatusData = flightStatus->getData();
2013-04-05 22:46:56 +02:00
ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData();
float ailerons = -1;
float elevator = -1;
float rudder = -1;
2013-04-05 22:46:56 +02:00
float throttle = -1;
if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) {
2013-04-05 22:46:56 +02:00
// Read joystick input
if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) {
2013-04-05 22:46:56 +02:00
// Note: Pitch sign is reversed in FG ?
ailerons = manCtrlData.Roll;
elevator = -manCtrlData.Pitch;
rudder = manCtrlData.Yaw;
2013-04-05 22:46:56 +02:00
throttle = manCtrlData.Throttle;
}
} else {
2013-04-05 22:46:56 +02:00
// Read ActuatorDesired from autopilot
actData = actDesired->getData();
2013-04-05 22:46:56 +02:00
ailerons = actData.Roll;
elevator = -actData.Pitch;
rudder = actData.Yaw;
2013-04-05 22:46:56 +02:00
throttle = actData.Throttle;
}
int allowableDifference = 10;
// qDebug() << "UDP sent:" << udpCounterGCSsend << " - UDP Received:" << udpCounterFGrecv;
2013-04-05 22:46:56 +02:00
if (udpCounterFGrecv == udpCounterGCSsend) {
2013-04-05 22:46:56 +02:00
udpCounterGCSsend = 0;
}
if ((udpCounterGCSsend < allowableDifference) || (udpCounterFGrecv == 0)) { // FG udp queue is not delayed
2013-04-05 22:46:56 +02:00
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
2013-04-05 22:46:56 +02:00
QByteArray data = cmd.toAscii();
if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) {
2013-04-05 22:46:56 +02:00
emit processOutput("Error sending UDP packet to FG: " + outSocket->errorString() + "\n");
}
} else {
2013-04-05 22:46:56 +02:00
// 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;
2013-04-05 22:46:56 +02:00
actData.Throttle = throttle;
// actData.NumLongUpdates = (float)udpCounterFGrecv;
// actData.UpdateTime = (float)udpCounterGCSsend;
2013-04-05 22:46:56 +02:00
actDesired->setData(actData);
}
}
void FGSimulator::processUpdate(const QByteArray & inp)
2013-04-05 22:46:56 +02:00
{
// TODO: this does not use the FLIGHT_PARAM structure, it should!
2013-04-05 22:46:56 +02:00
// Split
QString data(inp);
QStringList fields = data.split(",");
// Get xRate (deg/s)
// float xRate = fields[0].toFloat() * 180.0/M_PI;
2013-04-05 22:46:56 +02:00
// Get yRate (deg/s)
// float yRate = fields[1].toFloat() * 180.0/M_PI;
2013-04-05 22:46:56 +02:00
// Get zRate (deg/s)
// float zRate = fields[2].toFloat() * 180.0/M_PI;
2013-04-05 22:46:56 +02:00
// Get xAccel (m/s^2)
float xAccel = fields[3].toFloat() * FT2M;
2013-04-05 22:46:56 +02:00
// Get yAccel (m/s^2)
float yAccel = fields[4].toFloat() * FT2M;
2013-04-05 22:46:56 +02:00
// Get xAccel (m/s^2)
float zAccel = fields[5].toFloat() * FT2M;
2013-04-05 22:46:56 +02:00
// Get pitch (deg)
float pitch = fields[6].toFloat();
2013-04-05 22:46:56 +02:00
// Get pitchRate (deg/s)
float pitchRate = fields[7].toFloat();
// Get roll (deg)
float roll = fields[8].toFloat();
2013-04-05 22:46:56 +02:00
// Get rollRate (deg/s)
float rollRate = fields[9].toFloat();
// Get yaw (deg)
float yaw = fields[10].toFloat();
2013-04-05 22:46:56 +02:00
// Get yawRate (deg/s)
float yawRate = fields[11].toFloat();
2013-04-05 22:46:56 +02:00
// Get latitude (deg)
float latitude = fields[12].toFloat();
2013-04-05 22:46:56 +02:00
// Get longitude (deg)
float longitude = fields[13].toFloat();
2013-04-05 22:46:56 +02:00
// Get heading (deg)
float heading = fields[14].toFloat();
2013-04-05 22:46:56 +02:00
// 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;
2013-04-05 22:46:56 +02:00
// Get airspeed (m/s)
float airspeed = fields[18].toFloat() * KT2MPS;
2013-04-05 22:46:56 +02:00
// Get temperature (degC)
float temperature = fields[19].toFloat();
2013-04-05 22:46:56 +02:00
// 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;
2013-04-05 22:46:56 +02:00
// Get UDP packets received by FG
int n = fields[24].toInt();
2013-04-05 22:46:56 +02:00
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 };
2013-04-05 22:46:56 +02:00
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);
2013-04-05 22:46:56 +02:00
// Update GPS Position objects
out.latitude = latitude * 1e7;
out.longitude = longitude * 1e7;
out.altitude = altitude_msl;
2013-04-05 22:46:56 +02:00
out.agl = altitude_agl;
out.groundspeed = groundspeed;
out.calibratedAirspeed = airspeed;
// Update BaroSensor object
2013-04-05 22:46:56 +02:00
out.temperature = temperature;
out.pressure = pressure;
2013-04-05 22:46:56 +02:00
// Update attState object
out.roll = roll; // roll;
out.pitch = pitch; // pitch
out.heading = yaw; // yaw
2013-04-05 22:46:56 +02:00
out.dstN = NED[0];
out.dstE = NED[1];
out.dstD = NED[2];
2013-04-05 22:46:56 +02:00
// Update VelocityState.{North,East,Down}
out.velNorth = velocityStateNorth;
out.velEast = velocityStateEast;
out.velDown = velocityStateDown;
2013-04-05 22:46:56 +02:00
// Update gyroscope sensor data
out.rollRate = rollRate;
2013-04-05 22:46:56 +02:00
out.pitchRate = pitchRate;
out.yawRate = yawRate;
2013-04-05 22:46:56 +02:00
// Update accelerometer sensor data
out.accX = xAccel;
out.accY = yAccel;
out.accZ = -zAccel;
2013-04-05 22:46:56 +02:00
updateUAVOs(out);
}