Newer
Older
/****************************************************************************
*
* (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.
*
****************************************************************************/
Lorenz Meier
committed
/**
* @file
* @brief Definition of UDP connection (server) for unmanned vehicles
* @author Lorenz Meier <lorenz@px4.io>
Lorenz Meier
committed
*
*/
#include <QTimer>
#include <QList>
#include <QDebug>
#include <QMutexLocker>
Lorenz Meier
committed
#include <iostream>
Lorenz Meier
committed
#include "QGCJSBSimLink.h"
#include "QGC.h"
Lorenz Meier
committed
QGCJSBSimLink::QGCJSBSimLink(Vehicle* vehicle, QString startupArguments, QString remoteHost, QHostAddress host, quint16 port)
: _vehicle(vehicle)
, startupArguments(startupArguments)
Lorenz Meier
committed
{
Lorenz Meier
committed
// 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);
Lorenz Meier
committed
this->host = host;
this->port = port + _vehicle->id();
Lorenz Meier
committed
this->connectState = false;
this->currentPort = 49000 + _vehicle->id();
Lorenz Meier
committed
this->name = tr("JSBSim Link (port:%1)").arg(port);
setRemoteHost(remoteHost);
}
QGCJSBSimLink::~QGCJSBSimLink()
{ //do not disconnect unless it is connected.
//disconnectSimulation will delete the memory that was allocated for process, terraSync and socket
Lorenz Meier
committed
if(connectState){
disconnectSimulation();
}
}
/**
* @brief Runs the thread
*
**/
void QGCJSBSimLink::run()
{
qDebug() << "STARTING FLIGHTGEAR LINK";
connectState = socket->bind(host, port, QAbstractSocket::ReuseAddressHint);
QObject::connect(socket, &QUdpSocket::readyRead, this, &QGCJSBSimLink::readBytes);
process = new QProcess(this);
connect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCJSBSimLink::updateControls);
connect(this, &QGCJSBSimLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState);
//connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate()));
// Catch process error
connect(process, static_cast<void (QProcess::*)(QProcess::ProcessError)>(&QProcess::error),
this, &QGCJSBSimLink::processError);
// Start Flightgear
QStringList arguments;
QString processJSB;
QString rootJSB;
#ifdef Q_OS_MACX
processJSB = "/usr/local/bin/JSBSim";
rootJSB = "/Applications/FlightGear.app/Contents/Resources/data";
#endif
#ifdef Q_OS_WIN32
processJSB = "C:\\Program Files (x86)\\FlightGear\\bin\\Win32\\fgfs";
rootJSB = "C:\\Program Files (x86)\\FlightGear\\data";
#endif
#ifdef Q_OS_LINUX
processJSB = "/usr/games/fgfs";
rootJSB = "/usr/share/games/flightgear";
#endif
// Sanity checks
bool sane = true;
QFileInfo executable(processJSB);
if (!executable.isExecutable())
{
//-- TODO: QGCMessageBox::critical("JSBSim", tr("JSBSim failed to start. JSBSim was not found at %1").arg(processJSB));
sane = false;
}
QFileInfo root(rootJSB);
if (!root.isDir())
{
//-- TODO: QGCMessageBox::critical("JSBSim", tr("JSBSim failed to start. JSBSim data directory was not found at %1").arg(rootJSB));
sane = false;
}
if (!sane) return;
/*Prepare JSBSim Arguments */
if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR)
arguments << QString("--realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=%s/flightgear.xml --script=%s/%s").arg(rootJSB, rootJSB, script);
arguments << QString("JSBSim --realtime --suspend --nice --simulation-rate=1000 --logdirectivefile=%s/flightgear.xml --script=%s/%s").arg(rootJSB, rootJSB, script);
}
process->start(processJSB, arguments);
emit simulationConnected(connectState);
if (connectState) {
emit simulationConnected();
connectionStartTime = QGC::groundTimeUsecs()/1000;
}
qDebug() << "STARTING SIM";
Lorenz Meier
committed
exec();
}
void QGCJSBSimLink::setPort(int port)
{
this->port = port;
disconnectSimulation();
connectSimulation();
}
void QGCJSBSimLink::processError(QProcess::ProcessError err)
{
QString msg;
switch(err) {
case QProcess::FailedToStart:
msg = tr("JSBSim Failed to start. Please check if the path and command is correct");
break;
case QProcess::Crashed:
msg = tr("JSBSim crashed. This is a JSBSim-related problem, check for JSBSim upgrade.");
break;
case QProcess::Timedout:
msg = tr("JSBSim start timed out. Please check if the path and command is correct");
break;
case QProcess::ReadError:
case QProcess::WriteError:
msg = tr("Could not communicate with JSBSim. Please check if the path and command are correct");
break;
case QProcess::UnknownError:
default:
msg = tr("JSBSim error occurred. Please check if the path and command is correct.");
break;
Lorenz Meier
committed
}
Lorenz Meier
committed
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
}
/**
* @param host Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551
*/
void QGCJSBSimLink::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
{
QHostInfo info = QHostInfo::fromName(host);
if (info.error() == QHostInfo::NoError)
{
// Add host
currentHost = info.addresses().first();
}
}
}
void QGCJSBSimLink::updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode)
Lorenz Meier
committed
{
// magnetos,aileron,elevator,rudder,throttle\n
//float magnetos = 3.0f;
Q_UNUSED(time);
Q_UNUSED(systemMode);
Q_UNUSED(navMode);
if(!qIsNaN(rollAilerons) && !qIsNaN(pitchElevator) && !qIsNaN(yawRudder) && !qIsNaN(throttle))
Lorenz Meier
committed
{
QString state("%1\t%2\t%3\t%4\t%5\n");
state = state.arg(rollAilerons).arg(pitchElevator).arg(yawRudder).arg(true).arg(throttle);
emit _invokeWriteBytes(state.toLatin1());
Lorenz Meier
committed
}
else
{
qDebug() << "HIL: Got NaN values from the hardware: isnan output: roll: " << qIsNaN(rollAilerons) << ", pitch: " << qIsNaN(pitchElevator) << ", yaw: " << qIsNaN(yawRudder) << ", throttle: " << qIsNaN(throttle);
Lorenz Meier
committed
}
//qDebug() << "Updated controls" << state;
}
void QGCJSBSimLink::_writeBytes(const QByteArray data)
Lorenz Meier
committed
{
//#define QGCJSBSimLink_DEBUG
#ifdef QGCJSBSimLink_DEBUG
QString bytes;
QString ascii;
for (int i=0, size = data.size(); i<size; i++)
Lorenz Meier
committed
{
unsigned char v = data[i];
bytes.append(QString().sprintf("%02x ", v));
if (data[i] > 31 && data[i] < 127)
{
ascii.append(data[i]);
}
else
{
ascii.append(219);
}
}
qDebug() << "Sent" << size << "bytes to" << currentHost.toString() << ":" << currentPort << "data:";
qDebug() << bytes;
qDebug() << "ASCII:" << ascii;
#endif
if (connectState && socket) socket->writeDatagram(data, currentHost, currentPort);
Lorenz Meier
committed
}
/**
* @brief Read a number of bytes from the interface.
*
* @param data Pointer to the data byte array to write the bytes to
* @param maxLength The maximum number of bytes to write
**/
void QGCJSBSimLink::readBytes()
{
const qint64 maxLength = 65536;
char data[maxLength];
QHostAddress sender;
quint16 senderPort;
unsigned int s = socket->pendingDatagramSize();
if (s > maxLength) std::cerr << __FILE__ << __LINE__ << " UDP datagram overflow, allowed to read less bytes than datagram size" << std::endl;
socket->readDatagram(data, maxLength, &sender, &senderPort);
Lorenz Meier
committed
// Print string
QByteArray b(data, s);
QString state(b);
Lorenz Meier
committed
// Parse string
float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed;
double lat, lon, alt;
double vx, vy, vz, xacc, yacc, zacc;
Lorenz Meier
committed
// Send updated state
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc);
*/
Lorenz Meier
committed
// Echo data for debugging purposes
std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl;
Lorenz Meier
committed
{
unsigned int v=data[i];
fprintf(stderr,"%02x ", v);
}
std::cerr << std::endl;
}
/**
* @brief Get the number of bytes to read.
*
* @return The number of bytes to read
**/
qint64 QGCJSBSimLink::bytesAvailable()
{
return socket->pendingDatagramSize();
}
/**
* @brief Disconnect the connection.
*
* @return True if connection has been disconnected, false if connection couldn't be disconnected.
**/
bool QGCJSBSimLink::disconnectSimulation()
{
disconnect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCJSBSimLink::updateControls);
disconnect(this, &QGCJSBSimLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState);
disconnect(process, static_cast<void (QProcess::*)(QProcess::ProcessError)>(&QProcess::error),
this, &QGCJSBSimLink::processError);
Lorenz Meier
committed
if (process)
{
process->close();
delete process;
Lorenz Meier
committed
}
if (socket)
{
socket->close();
delete socket;
Lorenz Meier
committed
}
connectState = false;
emit simulationDisconnected();
emit simulationConnected(false);
return !connectState;
}
/**
* @brief Connect the connection.
*
* @return True if connection has been established, false if connection couldn't be established.
**/
bool QGCJSBSimLink::connectSimulation()
{
start(HighPriority);
return true;
Lorenz Meier
committed
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
}
/**
* @brief Set the startup arguments used to start flightgear
*
**/
void QGCJSBSimLink::setStartupArguments(QString startupArguments)
{
this->startupArguments = startupArguments;
}
/**
* @brief Check if connection is active.
*
* @return True if link is connected, false otherwise.
**/
bool QGCJSBSimLink::isConnected()
{
return connectState;
}
QString QGCJSBSimLink::getName()
{
return name;
}
QString QGCJSBSimLink::getRemoteHost()
{
return QString("%1:%2").arg(currentHost.toString(), currentPort);
}
void QGCJSBSimLink::setName(QString name)
{
this->name = name;
}