Don Gagne
5 years ago
committed by
GitHub
12 changed files with 4 additions and 2659 deletions
@ -1,149 +0,0 @@ |
|||||||
#pragma once |
|
||||||
|
|
||||||
#include <QThread> |
|
||||||
#include <QProcess> |
|
||||||
#include "inttypes.h" |
|
||||||
|
|
||||||
class QGCHilLink : public QThread |
|
||||||
{ |
|
||||||
Q_OBJECT |
|
||||||
public: |
|
||||||
|
|
||||||
virtual bool isConnected() = 0; |
|
||||||
virtual qint64 bytesAvailable() = 0; |
|
||||||
virtual int getPort() const = 0; |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief The human readable port name |
|
||||||
*/ |
|
||||||
virtual QString getName() = 0; |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Get remote host and port |
|
||||||
* @return string in format <host>:<port> |
|
||||||
*/ |
|
||||||
virtual QString getRemoteHost() = 0; |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Get the application name and version |
|
||||||
* @return A string containing a unique application name and compatibility version |
|
||||||
*/ |
|
||||||
virtual QString getVersion() = 0; |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Get index of currently selected airframe |
|
||||||
* @return -1 if default is selected, index else |
|
||||||
*/ |
|
||||||
virtual int getAirFrameIndex() = 0; |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Check if sensor level HIL is enabled |
|
||||||
* @return true if sensor HIL is enabled |
|
||||||
*/ |
|
||||||
virtual bool sensorHilEnabled() = 0; |
|
||||||
|
|
||||||
public slots: |
|
||||||
virtual void setPort(int port) = 0; |
|
||||||
/** @brief Add a new host to broadcast messages to */ |
|
||||||
virtual void setRemoteHost(const QString& host) = 0; |
|
||||||
/** @brief Send new control states to the simulation */ |
|
||||||
virtual void updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode) = 0; |
|
||||||
virtual void processError(QProcess::ProcessError err) = 0; |
|
||||||
/** @brief Set the simulator version as text string */ |
|
||||||
virtual void setVersion(const QString& version) = 0; |
|
||||||
/** @brief Enable sensor-level HIL (instead of state-level HIL) */ |
|
||||||
virtual void enableSensorHIL(bool enable) = 0; |
|
||||||
|
|
||||||
virtual void selectAirframe(const QString& airframe) = 0; |
|
||||||
|
|
||||||
virtual void readBytes() = 0; |
|
||||||
/**
|
|
||||||
* @brief Write a number of bytes to the interface. |
|
||||||
* |
|
||||||
* @param data Pointer to the data byte array |
|
||||||
* @param size The size of the bytes array |
|
||||||
**/ |
|
||||||
void writeBytesSafe(const char* data, int length) |
|
||||||
{ |
|
||||||
emit _invokeWriteBytes(QByteArray(data, length)); |
|
||||||
} |
|
||||||
|
|
||||||
virtual bool connectSimulation() = 0; |
|
||||||
virtual bool disconnectSimulation() = 0; |
|
||||||
|
|
||||||
private slots: |
|
||||||
virtual void _writeBytes(const QByteArray) = 0; |
|
||||||
|
|
||||||
protected: |
|
||||||
virtual void setName(QString name) = 0; |
|
||||||
|
|
||||||
QGCHilLink() : |
|
||||||
QThread() |
|
||||||
{ |
|
||||||
connect(this, &QGCHilLink::_invokeWriteBytes, this, &QGCHilLink::_writeBytes); |
|
||||||
} |
|
||||||
|
|
||||||
signals: |
|
||||||
/**
|
|
||||||
* @brief This signal is emitted instantly when the link is connected |
|
||||||
**/ |
|
||||||
void simulationConnected(); |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief This signal is emitted instantly when the link is disconnected |
|
||||||
**/ |
|
||||||
void simulationDisconnected(); |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Thread safe signal to disconnect simulator from other threads |
|
||||||
**/ |
|
||||||
void disconnectSim(); |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief This signal is emitted instantly when the link status changes |
|
||||||
**/ |
|
||||||
void simulationConnected(bool connected); |
|
||||||
|
|
||||||
/** @brief State update from simulation */ |
|
||||||
void hilStateChanged(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, |
|
||||||
float pitchspeed, float yawspeed, double lat, double lon, double alt, |
|
||||||
float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc); |
|
||||||
|
|
||||||
void hilGroundTruthChanged(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, |
|
||||||
float pitchspeed, float yawspeed, double lat, double lon, double alt, |
|
||||||
float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc); |
|
||||||
|
|
||||||
void sensorHilGpsChanged(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites); |
|
||||||
|
|
||||||
void sensorHilRawImuChanged(quint64 time_us, float xacc, float yacc, float zacc, |
|
||||||
float xgyro, float ygyro, float zgyro, |
|
||||||
float xmag, float ymag, float zmag, |
|
||||||
float abs_pressure, float diff_pressure, |
|
||||||
float pressure_alt, float temperature, |
|
||||||
quint32 fields_updated); |
|
||||||
|
|
||||||
void sensorHilOpticalFlowChanged(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, |
|
||||||
float flow_comp_m_y, quint8 quality, float ground_distance); |
|
||||||
|
|
||||||
/** @brief Remote host and port changed */ |
|
||||||
void remoteChanged(const QString& hostPort); |
|
||||||
|
|
||||||
/** @brief Status text message from link */ |
|
||||||
void statusMessage(const QString& message); |
|
||||||
|
|
||||||
/** @brief Airframe changed */ |
|
||||||
void airframeChanged(const QString& airframe); |
|
||||||
|
|
||||||
/** @brief Selected sim version changed */ |
|
||||||
void versionChanged(const QString& version); |
|
||||||
|
|
||||||
/** @brief Selected sim version changed */ |
|
||||||
void versionChanged(const int version); |
|
||||||
|
|
||||||
/** @brief Sensor leve HIL state changed */ |
|
||||||
void sensorHilChanged(bool enabled); |
|
||||||
|
|
||||||
/** @brief Helper signal to force execution on the correct thread */ |
|
||||||
void _invokeWriteBytes(QByteArray); |
|
||||||
}; |
|
||||||
|
|
||||||
@ -1,399 +0,0 @@ |
|||||||
/****************************************************************************
|
|
||||||
* |
|
||||||
* (c) 2009-2020 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. |
|
||||||
* |
|
||||||
****************************************************************************/ |
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file |
|
||||||
* @brief Definition of UDP connection (server) for unmanned vehicles |
|
||||||
* @author Lorenz Meier <lorenz@px4.io> |
|
||||||
* |
|
||||||
*/ |
|
||||||
|
|
||||||
#include <QTimer> |
|
||||||
#include <QList> |
|
||||||
#include <QDebug> |
|
||||||
#include <QMutexLocker> |
|
||||||
#include <QHostInfo> |
|
||||||
|
|
||||||
#include <iostream> |
|
||||||
|
|
||||||
#include "UAS.h" |
|
||||||
#include "QGCJSBSimLink.h" |
|
||||||
#include "QGC.h" |
|
||||||
//-- TODO: #include "QGCMessageBox.h"
|
|
||||||
|
|
||||||
QGCJSBSimLink::QGCJSBSimLink(Vehicle* vehicle, QString startupArguments, QString remoteHost, QHostAddress host, quint16 port) |
|
||||||
: _vehicle(vehicle) |
|
||||||
, socket(nullptr) |
|
||||||
, process(nullptr) |
|
||||||
, startupArguments(startupArguments) |
|
||||||
{ |
|
||||||
// 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 + _vehicle->id(); |
|
||||||
this->connectState = false; |
|
||||||
this->currentPort = 49000 + _vehicle->id(); |
|
||||||
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
|
|
||||||
if(connectState){ |
|
||||||
disconnectSimulation(); |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Runs the thread |
|
||||||
* |
|
||||||
**/ |
|
||||||
void QGCJSBSimLink::run() |
|
||||||
{ |
|
||||||
qDebug() << "STARTING FLIGHTGEAR LINK"; |
|
||||||
|
|
||||||
if (!_vehicle) return; |
|
||||||
socket = new QUdpSocket(this); |
|
||||||
socket->moveToThread(this); |
|
||||||
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); |
|
||||||
|
|
||||||
_vehicle->uas()->startHil(); |
|
||||||
|
|
||||||
//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); |
|
||||||
} |
|
||||||
else |
|
||||||
{ |
|
||||||
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"; |
|
||||||
|
|
||||||
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; |
|
||||||
} |
|
||||||
|
|
||||||
//-- TODO: QGCMessageBox::critical("JSBSim HIL", msg);
|
|
||||||
} |
|
||||||
|
|
||||||
/**
|
|
||||||
* @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) |
|
||||||
{ |
|
||||||
// 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)) |
|
||||||
{ |
|
||||||
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()); |
|
||||||
} |
|
||||||
else |
|
||||||
{ |
|
||||||
qDebug() << "HIL: Got NaN values from the hardware: isnan output: roll: " << qIsNaN(rollAilerons) << ", pitch: " << qIsNaN(pitchElevator) << ", yaw: " << qIsNaN(yawRudder) << ", throttle: " << qIsNaN(throttle); |
|
||||||
} |
|
||||||
//qDebug() << "Updated controls" << state;
|
|
||||||
} |
|
||||||
|
|
||||||
void QGCJSBSimLink::_writeBytes(const QByteArray data) |
|
||||||
{ |
|
||||||
//#define QGCJSBSimLink_DEBUG
|
|
||||||
#ifdef QGCJSBSimLink_DEBUG |
|
||||||
QString bytes; |
|
||||||
QString ascii; |
|
||||||
for (int i=0, size = data.size(); i<size; i++) |
|
||||||
{ |
|
||||||
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); |
|
||||||
} |
|
||||||
|
|
||||||
/**
|
|
||||||
* @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); |
|
||||||
|
|
||||||
/*
|
|
||||||
// Print string
|
|
||||||
QByteArray b(data, s); |
|
||||||
QString state(b); |
|
||||||
|
|
||||||
// Parse string
|
|
||||||
float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed; |
|
||||||
double lat, lon, alt; |
|
||||||
double vx, vy, vz, xacc, yacc, zacc; |
|
||||||
|
|
||||||
// Send updated state
|
|
||||||
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, |
|
||||||
pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc); |
|
||||||
*/ |
|
||||||
|
|
||||||
// Echo data for debugging purposes
|
|
||||||
std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl; |
|
||||||
for (unsigned int i=0; i<s; i++) |
|
||||||
{ |
|
||||||
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); |
|
||||||
|
|
||||||
if (process) |
|
||||||
{ |
|
||||||
process->close(); |
|
||||||
delete process; |
|
||||||
process = nullptr; |
|
||||||
} |
|
||||||
if (socket) |
|
||||||
{ |
|
||||||
socket->close(); |
|
||||||
delete socket; |
|
||||||
socket = nullptr; |
|
||||||
} |
|
||||||
|
|
||||||
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; |
|
||||||
} |
|
||||||
|
|
||||||
/**
|
|
||||||
* @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; |
|
||||||
} |
|
||||||
@ -1,149 +0,0 @@ |
|||||||
/****************************************************************************
|
|
||||||
* |
|
||||||
* (c) 2009-2020 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. |
|
||||||
* |
|
||||||
****************************************************************************/ |
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file |
|
||||||
* @brief UDP connection (server) for unmanned vehicles |
|
||||||
* @author Lorenz Meier <mavteam@student.ethz.ch> |
|
||||||
* |
|
||||||
*/ |
|
||||||
|
|
||||||
#pragma once |
|
||||||
|
|
||||||
#include <QString> |
|
||||||
#include <QList> |
|
||||||
#include <QMap> |
|
||||||
#include <QMutex> |
|
||||||
#include <QUdpSocket> |
|
||||||
#include <QTimer> |
|
||||||
#include <QProcess> |
|
||||||
#include <LinkInterface.h> |
|
||||||
#include "QGCConfig.h" |
|
||||||
#include "QGCHilLink.h" |
|
||||||
#include "Vehicle.h" |
|
||||||
|
|
||||||
class QGCJSBSimLink : public QGCHilLink |
|
||||||
{ |
|
||||||
Q_OBJECT |
|
||||||
//Q_INTERFACES(QGCJSBSimLinkInterface:LinkInterface)
|
|
||||||
|
|
||||||
public: |
|
||||||
QGCJSBSimLink(Vehicle* vehicle, QString startupArguments, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress host = QHostAddress::Any, quint16 port = 49005); |
|
||||||
~QGCJSBSimLink(); |
|
||||||
|
|
||||||
bool isConnected(); |
|
||||||
qint64 bytesAvailable(); |
|
||||||
int getPort() const { |
|
||||||
return port; |
|
||||||
} |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief The human readable port name |
|
||||||
*/ |
|
||||||
QString getName(); |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Get remote host and port |
|
||||||
* @return string in format <host>:<port> |
|
||||||
*/ |
|
||||||
QString getRemoteHost(); |
|
||||||
|
|
||||||
QString getVersion() |
|
||||||
{ |
|
||||||
return QString("FlightGear %1").arg(flightGearVersion); |
|
||||||
} |
|
||||||
|
|
||||||
int getAirFrameIndex() |
|
||||||
{ |
|
||||||
return -1; |
|
||||||
} |
|
||||||
|
|
||||||
void run(); |
|
||||||
|
|
||||||
bool sensorHilEnabled() { |
|
||||||
return _sensorHilEnabled; |
|
||||||
} |
|
||||||
|
|
||||||
public slots: |
|
||||||
// void setAddress(QString address);
|
|
||||||
void setPort(int port); |
|
||||||
/** @brief Add a new host to broadcast messages to */ |
|
||||||
void setRemoteHost(const QString& host); |
|
||||||
/** @brief Send new control states to the simulation */ |
|
||||||
void updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode); |
|
||||||
// /** @brief Remove a host from broadcasting messages to */
|
|
||||||
// void removeHost(const QString& host);
|
|
||||||
// void readPendingDatagrams();
|
|
||||||
void processError(QProcess::ProcessError err); |
|
||||||
/** @brief Set the simulator version as text string */ |
|
||||||
void setVersion(const QString& version) |
|
||||||
{ |
|
||||||
Q_UNUSED(version); |
|
||||||
} |
|
||||||
|
|
||||||
void selectAirframe(const QString& airframe) |
|
||||||
{ |
|
||||||
script = airframe; |
|
||||||
} |
|
||||||
|
|
||||||
void enableSensorHIL(bool enable) { |
|
||||||
if (enable != _sensorHilEnabled) { |
|
||||||
_sensorHilEnabled = enable; |
|
||||||
emit sensorHilChanged(enable); |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
void readBytes(); |
|
||||||
|
|
||||||
private slots: |
|
||||||
/**
|
|
||||||
* @brief Write a number of bytes to the interface. |
|
||||||
* |
|
||||||
* @param data Pointer to the data byte array |
|
||||||
* @param size The size of the bytes array |
|
||||||
**/ |
|
||||||
void _writeBytes(const QByteArray data); |
|
||||||
|
|
||||||
public slots: |
|
||||||
bool connectSimulation(); |
|
||||||
bool disconnectSimulation(); |
|
||||||
|
|
||||||
void setStartupArguments(QString startupArguments); |
|
||||||
|
|
||||||
private: |
|
||||||
Vehicle* _vehicle; |
|
||||||
QString name; |
|
||||||
QHostAddress host; |
|
||||||
QHostAddress currentHost; |
|
||||||
quint16 currentPort; |
|
||||||
quint16 port; |
|
||||||
int id; |
|
||||||
QUdpSocket* socket; |
|
||||||
bool connectState; |
|
||||||
|
|
||||||
quint64 bitsSentTotal; |
|
||||||
quint64 bitsSentCurrent; |
|
||||||
quint64 bitsSentMax; |
|
||||||
quint64 bitsReceivedTotal; |
|
||||||
quint64 bitsReceivedCurrent; |
|
||||||
quint64 bitsReceivedMax; |
|
||||||
quint64 connectionStartTime; |
|
||||||
QMutex statisticsMutex; |
|
||||||
QMutex dataMutex; |
|
||||||
QTimer refreshTimer; |
|
||||||
QProcess* process; |
|
||||||
unsigned int flightGearVersion; |
|
||||||
QString startupArguments; |
|
||||||
QString script; |
|
||||||
bool _sensorHilEnabled; |
|
||||||
|
|
||||||
void setName(QString name); |
|
||||||
}; |
|
||||||
|
|
||||||
@ -1,237 +0,0 @@ |
|||||||
/****************************************************************************
|
|
||||||
* |
|
||||||
* (c) 2009-2020 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. |
|
||||||
* |
|
||||||
****************************************************************************/ |
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @file QGCXPlaneLink.h |
|
||||||
* @brief X-Plane simulation link |
|
||||||
* @author Lorenz Meier <mavteam@student.ethz.ch> |
|
||||||
* |
|
||||||
*/ |
|
||||||
|
|
||||||
#pragma once |
|
||||||
|
|
||||||
#include <QString> |
|
||||||
#include <QList> |
|
||||||
#include <QMap> |
|
||||||
#include <QMutex> |
|
||||||
#include <QUdpSocket> |
|
||||||
#include <QTimer> |
|
||||||
#include <QProcess> |
|
||||||
#include <LinkInterface.h> |
|
||||||
#include "QGCConfig.h" |
|
||||||
#include "QGCHilLink.h" |
|
||||||
#include "Vehicle.h" |
|
||||||
|
|
||||||
class QGCXPlaneLink : public QGCHilLink |
|
||||||
{ |
|
||||||
Q_OBJECT |
|
||||||
//Q_INTERFACES(QGCXPlaneLinkInterface:LinkInterface)
|
|
||||||
|
|
||||||
public: |
|
||||||
QGCXPlaneLink(Vehicle* vehicle, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress localHost = QHostAddress::Any, quint16 localPort = 49005); |
|
||||||
~QGCXPlaneLink(); |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Load X-Plane HIL settings |
|
||||||
*/ |
|
||||||
void loadSettings(); |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Store X-Plane HIL settings |
|
||||||
*/ |
|
||||||
void storeSettings(); |
|
||||||
|
|
||||||
bool isConnected(); |
|
||||||
qint64 bytesAvailable(); |
|
||||||
int getPort() const { |
|
||||||
return localPort; |
|
||||||
} |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief The human readable port name |
|
||||||
*/ |
|
||||||
QString getName(); |
|
||||||
|
|
||||||
void run(); |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Get remote host and port |
|
||||||
* @return string in format <host>:<port> |
|
||||||
*/ |
|
||||||
QString getRemoteHost(); |
|
||||||
|
|
||||||
enum AIRFRAME |
|
||||||
{ |
|
||||||
AIRFRAME_UNKNOWN = 0, |
|
||||||
AIRFRAME_QUAD_DJI_F450_PWM, |
|
||||||
AIRFRAME_QUAD_X_MK_10INCH_I2C, |
|
||||||
AIRFRAME_QUAD_X_ARDRONE, |
|
||||||
AIRFRAME_FIXED_WING_BIXLER_II, |
|
||||||
AIRFRAME_FIXED_WING_BIXLER_II_AILERONS |
|
||||||
}; |
|
||||||
|
|
||||||
QString getVersion() |
|
||||||
{ |
|
||||||
return QString("X-Plane %1").arg(xPlaneVersion); |
|
||||||
} |
|
||||||
|
|
||||||
int getAirFrameIndex() |
|
||||||
{ |
|
||||||
return (int)airframeID; |
|
||||||
} |
|
||||||
|
|
||||||
bool sensorHilEnabled() { |
|
||||||
return _sensorHilEnabled; |
|
||||||
} |
|
||||||
|
|
||||||
bool useHilActuatorControls() { |
|
||||||
return _useHilActuatorControls; |
|
||||||
} |
|
||||||
|
|
||||||
signals: |
|
||||||
/** @brief Sensor leve HIL state changed */ |
|
||||||
void useHilActuatorControlsChanged(bool enabled); |
|
||||||
|
|
||||||
public slots: |
|
||||||
// void setAddress(QString address);
|
|
||||||
void setPort(int port); |
|
||||||
/** @brief Add a new host to broadcast messages to */ |
|
||||||
void setRemoteHost(const QString& host); |
|
||||||
/** @brief Send new control states to the simulation */ |
|
||||||
void updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode); |
|
||||||
/** @brief Send new control commands to the simulation */ |
|
||||||
void updateActuatorControls(quint64 time, quint64 flags, |
|
||||||
float ctl_0, |
|
||||||
float ctl_1, |
|
||||||
float ctl_2, |
|
||||||
float ctl_3, |
|
||||||
float ctl_4, |
|
||||||
float ctl_5, |
|
||||||
float ctl_6, |
|
||||||
float ctl_7, |
|
||||||
float ctl_8, |
|
||||||
float ctl_9, |
|
||||||
float ctl_10, |
|
||||||
float ctl_11, |
|
||||||
float ctl_12, |
|
||||||
float ctl_13, |
|
||||||
float ctl_14, |
|
||||||
float ctl_15, |
|
||||||
quint8 mode); |
|
||||||
/** @brief Set the simulator version as text string */ |
|
||||||
void setVersion(const QString& version); |
|
||||||
/** @brief Set the simulator version as integer */ |
|
||||||
void setVersion(unsigned int version); |
|
||||||
|
|
||||||
void enableSensorHIL(bool enable) { |
|
||||||
if (enable != _sensorHilEnabled) { |
|
||||||
_sensorHilEnabled = enable; |
|
||||||
emit sensorHilChanged(enable); |
|
||||||
} |
|
||||||
} |
|
||||||
|
|
||||||
void enableHilActuatorControls(bool enable); |
|
||||||
|
|
||||||
void processError(QProcess::ProcessError err); |
|
||||||
|
|
||||||
void readBytes(); |
|
||||||
|
|
||||||
private slots: |
|
||||||
/**
|
|
||||||
* @brief Write a number of bytes to the interface. |
|
||||||
* |
|
||||||
* @param data Pointer to the data byte array |
|
||||||
* @param size The size of the bytes array |
|
||||||
**/ |
|
||||||
void _writeBytes(const QByteArray data); |
|
||||||
|
|
||||||
public slots: |
|
||||||
bool connectSimulation(); |
|
||||||
bool disconnectSimulation(); |
|
||||||
/**
|
|
||||||
* @brief Select airplane model |
|
||||||
* @param plane the name of the airplane |
|
||||||
*/ |
|
||||||
void selectAirframe(const QString& airframe); |
|
||||||
/**
|
|
||||||
* @brief Set the airplane position and attitude |
|
||||||
* @param lat |
|
||||||
* @param lon |
|
||||||
* @param alt |
|
||||||
* @param roll |
|
||||||
* @param pitch |
|
||||||
* @param yaw |
|
||||||
*/ |
|
||||||
void setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw); |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Set a random position |
|
||||||
*/ |
|
||||||
void setRandomPosition(); |
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Set a random attitude |
|
||||||
*/ |
|
||||||
void setRandomAttitude(); |
|
||||||
|
|
||||||
protected: |
|
||||||
Vehicle* _vehicle; |
|
||||||
QString name; |
|
||||||
QHostAddress localHost; |
|
||||||
quint16 localPort; |
|
||||||
QHostAddress remoteHost; |
|
||||||
quint16 remotePort; |
|
||||||
int id; |
|
||||||
QUdpSocket* socket; |
|
||||||
bool connectState; |
|
||||||
|
|
||||||
quint64 bitsSentTotal; |
|
||||||
quint64 bitsSentCurrent; |
|
||||||
quint64 bitsSentMax; |
|
||||||
quint64 bitsReceivedTotal; |
|
||||||
quint64 bitsReceivedCurrent; |
|
||||||
quint64 bitsReceivedMax; |
|
||||||
quint64 connectionStartTime; |
|
||||||
QMutex statisticsMutex; |
|
||||||
QMutex dataMutex; |
|
||||||
QTimer refreshTimer; |
|
||||||
QProcess* process; |
|
||||||
QProcess* terraSync; |
|
||||||
|
|
||||||
bool gpsReceived; |
|
||||||
bool attitudeReceived; |
|
||||||
|
|
||||||
float roll, pitch, yaw, rollspeed, pitchspeed, yawspeed; |
|
||||||
double lat, lon, alt, alt_agl; |
|
||||||
float vx, vy, vz, xacc, yacc, zacc; |
|
||||||
float ind_airspeed; |
|
||||||
float true_airspeed; |
|
||||||
float groundspeed; |
|
||||||
float xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature; |
|
||||||
float barometerOffsetkPa; |
|
||||||
|
|
||||||
float man_roll, man_pitch, man_yaw; |
|
||||||
QString airframeName; |
|
||||||
enum AIRFRAME airframeID; |
|
||||||
bool xPlaneConnected; |
|
||||||
unsigned int xPlaneVersion; |
|
||||||
quint64 simUpdateLast; |
|
||||||
quint64 simUpdateFirst; |
|
||||||
quint64 simUpdateLastText; |
|
||||||
quint64 simUpdateLastGroundTruth; |
|
||||||
float simUpdateHz; |
|
||||||
bool _sensorHilEnabled; |
|
||||||
bool _useHilActuatorControls; |
|
||||||
bool _should_exit; |
|
||||||
|
|
||||||
void setName(QString name); |
|
||||||
void sendDataRef(QString ref, float value); |
|
||||||
}; |
|
||||||
|
|
||||||
Loading…
Reference in new issue