DoinLakeFlyer
5 years ago
12 changed files with 4 additions and 2659 deletions
@ -1,149 +0,0 @@
@@ -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 @@
@@ -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 @@
@@ -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 @@
@@ -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