Skip to content
GitLab
Unverified Commit e9bafe97 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files

Merge pull request #8617 from DonLakeFlyer/RemoveHILCode 

Remove unused HIL code
parents 6d059984 1396001f
Expand all Hide whitespace changes
Inline Side-by-side
qgroundcontrol.pro
View file @ e9bafe97
......@@ -751,9 +751,6 @@ HEADERS += \
src/GPS/vehicle_gps_position.h \
src/Joystick/JoystickSDL.h \
src/RunGuard.h \
src/comm/QGCHilLink.h \
src/comm/QGCJSBSimLink.h \
src/comm/QGCXPlaneLink.h \
}
iOSBuild {
......@@ -940,8 +937,6 @@ SOURCES += \
src/GPS/RTCM/RTCMMavlink.cc \
src/Joystick/JoystickSDL.cc \
src/RunGuard.cc \
src/comm/QGCJSBSimLink.cc \
src/comm/QGCXPlaneLink.cc \
}
#
......
src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc
View file @ e9bafe97
......@@ -66,11 +66,9 @@ const QVariantList& PX4AutoPilotPlugin::vehicleComponents(void)
_airframeComponent->setupTriggerSignals();
_components.append(QVariant::fromValue((VehicleComponent*)_airframeComponent));
if (!_vehicle->hilMode()) {
_sensorsComponent = new SensorsComponent(_vehicle, this);
_sensorsComponent->setupTriggerSignals();
_components.append(QVariant::fromValue((VehicleComponent*)_sensorsComponent));
}
_sensorsComponent = new SensorsComponent(_vehicle, this);
_sensorsComponent->setupTriggerSignals();
_components.append(QVariant::fromValue((VehicleComponent*)_sensorsComponent));
_radioComponent = new PX4RadioComponent(_vehicle, this);
_radioComponent->setupTriggerSignals();
......@@ -151,7 +149,7 @@ QString PX4AutoPilotPlugin::prerequisiteSetup(VehicleComponent* component) const
return _airframeComponent->name();
} else if (_radioComponent && !_radioComponent->setupComplete()) {
return _radioComponent->name();
} else if (_sensorsComponent && !_vehicle->hilMode() && !_sensorsComponent->setupComplete()) {
} else if (_sensorsComponent && !_sensorsComponent->setupComplete()) {
return _sensorsComponent->name();
}
}
......
src/Vehicle/Vehicle.cc
View file @ e9bafe97
......@@ -788,9 +788,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_WIND_COV:
_handleWindCov(message);
break;
case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
_handleHilActuatorControls(message);
break;
case MAVLINK_MSG_ID_LOGGING_DATA:
_handleMavlinkLoggingData(message);
break;
......@@ -1451,30 +1448,6 @@ QString Vehicle::vehicleUIDStr()
return uid;
}
void Vehicle::_handleHilActuatorControls(mavlink_message_t &message)
{
mavlink_hil_actuator_controls_t hil;
mavlink_msg_hil_actuator_controls_decode(&message, &hil);
emit hilActuatorControlsChanged(hil.time_usec, hil.flags,
hil.controls[0],
hil.controls[1],
hil.controls[2],
hil.controls[3],
hil.controls[4],
hil.controls[5],
hil.controls[6],
hil.controls[7],
hil.controls[8],
hil.controls[9],
hil.controls[10],
hil.controls[11],
hil.controls[12],
hil.controls[13],
hil.controls[14],
hil.controls[15],
hil.mode);
}
void Vehicle::_handleCommandLong(mavlink_message_t& message)
{
#ifdef NO_SERIAL_LINK
......@@ -2526,30 +2499,6 @@ void Vehicle::setPriorityLinkByName(const QString& priorityLinkName)
}
}
bool Vehicle::hilMode()
{
return _base_mode & MAV_MODE_FLAG_HIL_ENABLED;
}
void Vehicle::setHilMode(bool hilMode)
{
mavlink_message_t msg;
uint8_t newBaseMode = _base_mode & ~MAV_MODE_FLAG_DECODE_POSITION_HIL;
if (hilMode) {
newBaseMode |= MAV_MODE_FLAG_HIL_ENABLED;
}
mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(),
_mavlink->getComponentId(),
priorityLink()->mavlinkChannel(),
&msg,
id(),
newBaseMode,
_custom_mode);
sendMessageOnLink(priorityLink(), msg);
}
void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple)
{
mavlink_message_t msg;
......
src/Vehicle/Vehicle.h
View file @ e9bafe97
......@@ -543,7 +543,6 @@ public:
Q_PROPERTY(QStringList flightModes READ flightModes NOTIFY flightModesChanged)
Q_PROPERTY(QStringList extraJoystickFlightModes READ extraJoystickFlightModes NOTIFY flightModesChanged)
Q_PROPERTY(QString flightMode READ flightMode WRITE setFlightMode NOTIFY flightModeChanged)
Q_PROPERTY(bool hilMode READ hilMode WRITE setHilMode NOTIFY hilModeChanged)
Q_PROPERTY(TrajectoryPoints* trajectoryPoints MEMBER _trajectoryPoints CONSTANT)
Q_PROPERTY(QmlObjectListModel* cameraTriggerPoints READ cameraTriggerPoints CONSTANT)
Q_PROPERTY(float latitude READ latitude NOTIFY coordinateChanged)
......@@ -866,9 +865,6 @@ public:
QVariantList links() const;
void setPriorityLinkByName(const QString& priorityLinkName);
bool hilMode();
void setHilMode(bool hilMode);
bool fixedWing() const;
bool multiRotor() const;
bool vtol() const;
......@@ -1135,9 +1131,6 @@ signals:
void armedPositionChanged();
void armedChanged (bool armed);
void flightModeChanged (const QString& flightMode);
void hilModeChanged (bool hilMode);
/** @brief HIL actuator controls (replaces HIL controls) */
void hilActuatorControlsChanged (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);
void connectionLostChanged (bool connectionLost);
void connectionLostEnabledChanged (bool connectionLostEnabled);
void autoDisconnectChanged (bool autoDisconnectChanged);
......@@ -1299,7 +1292,6 @@ private:
void _handleCommandLong (mavlink_message_t& message);
void _handleAutopilotVersion (LinkInterface* link, mavlink_message_t& message);
void _handleProtocolVersion (LinkInterface* link, mavlink_message_t& message);
void _handleHilActuatorControls (mavlink_message_t& message);
void _handleGpsRawInt (mavlink_message_t& message);
void _handleGlobalPositionInt (mavlink_message_t& message);
void _handleAltitude (mavlink_message_t& message);
......
src/comm/QGCHilLink.h deleted 100644 → 0
View file @ 6d059984
#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);
};
src/comm/QGCJSBSimLink.cc deleted 100644 → 0
View file @ 6d059984
/****************************************************************************
*
* (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;
}
src/comm/QGCJSBSimLink.h deleted 100644 → 0
View file @ 6d059984
/****************************************************************************
*
* (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);
};
src/comm/QGCXPlaneLink.cc deleted 100644 → 0
View file @ 6d059984
This diff is collapsed.
src/comm/QGCXPlaneLink.h deleted 100644 → 0
View file @ 6d059984
/****************************************************************************
*
* (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);
};
src/uas/UAS.cc
View file @ e9bafe97
......@@ -104,20 +104,11 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
temperature_var(0.0f),
*/
#ifndef __mobile__
simulation(0),
#endif
// The protected members.
connectionLost(false),
lastVoltageWarning(0),
lastNonNullTime(0),
onboardTimeOffsetInvalidCount(0),
hilEnabled(false),
sensorHil(false),
lastSendTimeGPS(0),
lastSendTimeSensors(0),
lastSendTimeOpticalFlow(0),
_vehicle(vehicle),
_firmwarePluginManager(firmwarePluginManager)
{
......@@ -229,13 +220,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time);
}
break;
case MAVLINK_MSG_ID_HIL_CONTROLS:
{
mavlink_hil_controls_t hil;
mavlink_msg_hil_controls_decode(&message, &hil);
emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode);
}
break;
case MAVLINK_MSG_ID_PARAM_VALUE:
{
......@@ -1002,384 +986,6 @@ void UAS::pairRX(int rxType, int rxSubType)
}
}
/**
* If enabled, connect the JSBSim link.
*/
#ifndef __mobile__
void UAS::enableHilJSBSim(bool enable, QString options)
{
auto* link = qobject_cast<QGCJSBSimLink*>(simulation);
if (!link) {
// Delete wrong sim
if (simulation) {
stopHil();
delete simulation;
}
simulation = new QGCJSBSimLink(_vehicle, options);
}
// Connect Flight Gear Link
link = qobject_cast<QGCJSBSimLink*>(simulation);
link->setStartupArguments(options);
if (enable)
{
startHil();
}
else
{
stopHil();
}
}
#endif
/**
* If enabled, connect the X-plane gear link.
*/
#ifndef __mobile__
void UAS::enableHilXPlane(bool enable)
{
auto* link = qobject_cast<QGCXPlaneLink*>(simulation);
if (!link) {
if (simulation) {
stopHil();
delete simulation;
}
simulation = new QGCXPlaneLink(_vehicle);
float noise_scaler = 0.0001f;
xacc_var = noise_scaler * 0.2914f;
yacc_var = noise_scaler * 0.2914f;
zacc_var = noise_scaler * 0.9577f;
rollspeed_var = noise_scaler * 0.8126f;
pitchspeed_var = noise_scaler * 0.6145f;
yawspeed_var = noise_scaler * 0.5852f;
xmag_var = noise_scaler * 0.0786f;
ymag_var = noise_scaler * 0.0566f;
zmag_var = noise_scaler * 0.0333f;
abs_pressure_var = noise_scaler * 0.5604f;
diff_pressure_var = noise_scaler * 0.2604f;
pressure_alt_var = noise_scaler * 0.5604f;
temperature_var = noise_scaler * 0.7290f;
}
// Connect X-Plane Link
if (enable)
{
startHil();
}
else
{
stopHil();
}
}
#endif
/**
* @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
* @param roll Roll angle (rad)
* @param pitch Pitch angle (rad)
* @param yaw Yaw angle (rad)
* @param rollspeed Roll angular speed (rad/s)
* @param pitchspeed Pitch angular speed (rad/s)
* @param yawspeed Yaw angular speed (rad/s)
* @param lat Latitude, expressed as * 1E7
* @param lon Longitude, expressed as * 1E7
* @param alt Altitude in meters, expressed as * 1000 (millimeters)
* @param vx Ground X Speed (Latitude), expressed as m/s * 100
* @param vy Ground Y Speed (Longitude), expressed as m/s * 100
* @param vz Ground Z Speed (Altitude), expressed as m/s * 100
* @param xacc X acceleration (mg)
* @param yacc Y acceleration (mg)
* @param zacc Z acceleration (mg)
*/
#ifndef __mobile__
void UAS::sendHilGroundTruth(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)
{
Q_UNUSED(time_us);
Q_UNUSED(xacc);
Q_UNUSED(yacc);
Q_UNUSED(zacc);
// Emit attitude for cross-check
emit valueChanged(uasId, "roll sim", "rad", roll, getUnixTime());
emit valueChanged(uasId, "pitch sim", "rad", pitch, getUnixTime());
emit valueChanged(uasId, "yaw sim", "rad", yaw, getUnixTime());
emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, getUnixTime());
emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, getUnixTime());
emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime());
emit valueChanged(uasId, "lat sim", "deg", lat*1e7, getUnixTime());
emit valueChanged(uasId, "lon sim", "deg", lon*1e7, getUnixTime());
emit valueChanged(uasId, "alt sim", "deg", alt*1e3, getUnixTime());
emit valueChanged(uasId, "vx sim", "m/s", vx*1e2, getUnixTime());
emit valueChanged(uasId, "vy sim", "m/s", vy*1e2, getUnixTime());
emit valueChanged(uasId, "vz sim", "m/s", vz*1e2, getUnixTime());
emit valueChanged(uasId, "IAS sim", "m/s", ind_airspeed, getUnixTime());
emit valueChanged(uasId, "TAS sim", "m/s", true_airspeed, getUnixTime());
}
#endif
/**
* @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
* @param roll Roll angle (rad)
* @param pitch Pitch angle (rad)
* @param yaw Yaw angle (rad)
* @param rollspeed Roll angular speed (rad/s)
* @param pitchspeed Pitch angular speed (rad/s)
* @param yawspeed Yaw angular speed (rad/s)
* @param lat Latitude, expressed as * 1E7
* @param lon Longitude, expressed as * 1E7
* @param alt Altitude in meters, expressed as * 1000 (millimeters)
* @param vx Ground X Speed (Latitude), expressed as m/s * 100
* @param vy Ground Y Speed (Longitude), expressed as m/s * 100
* @param vz Ground Z Speed (Altitude), expressed as m/s * 100
* @param xacc X acceleration (mg)
* @param yacc Y acceleration (mg)
* @param zacc Z acceleration (mg)
*/
#ifndef __mobile__
void UAS::sendHilState(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)
{
if (!_vehicle) {
return;
}
if (_vehicle->hilMode())
{
float q[4];
double cosPhi_2 = cos(double(roll) / 2.0);
double sinPhi_2 = sin(double(roll) / 2.0);
double cosTheta_2 = cos(double(pitch) / 2.0);
double sinTheta_2 = sin(double(pitch) / 2.0);
double cosPsi_2 = cos(double(yaw) / 2.0);
double sinPsi_2 = sin(double(yaw) / 2.0);
q[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +
sinPhi_2 * sinTheta_2 * sinPsi_2);
q[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -
cosPhi_2 * sinTheta_2 * sinPsi_2);
q[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 +
sinPhi_2 * cosTheta_2 * sinPsi_2);
q[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 -
sinPhi_2 * sinTheta_2 * cosPsi_2);
mavlink_message_t msg;
mavlink_msg_hil_state_quaternion_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
time_us, q, rollspeed, pitchspeed, yawspeed,
lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, ind_airspeed*100, true_airspeed*100, xacc*1000/9.81, yacc*1000/9.81, zacc*1000/9.81);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
}
else
{
// Attempt to set HIL mode
_vehicle->setHilMode(true);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
}
}
#endif
#ifndef __mobile__
float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
{
/* Calculate normally distributed variable noise with mean = 0 and variance = noise_var. Calculated according to
Box-Muller transform */
static const float epsilon = std::numeric_limits<float>::min(); //used to ensure non-zero uniform numbers
static float z0; //calculated normal distribution random variables with mu = 0, var = 1;
float u1, u2; //random variables generated from c++ rand();
/*Generate random variables in range (0 1] */
do
{
//TODO seed rand() with srand(time) but srand(time should be called once on startup)
//currently this will generate repeatable random noise
u1 = rand() * (1.0 / RAND_MAX);
u2 = rand() * (1.0 / RAND_MAX);
}
while ( u1 <= epsilon ); //Have a catch to ensure non-zero for log()
z0 = sqrt(-2.0 * log(u1)) * cos(2.0f * M_PI * u2); //calculate normally distributed variable with mu = 0, var = 1
//TODO add bias term that changes randomly to simulate accelerometer and gyro bias the exf should handle these
//as well
float noise = z0 * sqrt(noise_var); //calculate normally distributed variable with mu = 0, std = var^2
//Finally guard against any case where the noise is not real
if(std::isfinite(noise)) {
return truth_meas + noise;
} else {
return truth_meas;
}
}
#endif
/*
* @param abs_pressure Absolute Pressure (hPa)
* @param diff_pressure Differential Pressure (hPa)
*/
#ifndef __mobile__
void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed)
{
if (!_vehicle) {
return;
}
if (_vehicle->hilMode())
{
float xacc_corrupt = addZeroMeanNoise(xacc, xacc_var);
float yacc_corrupt = addZeroMeanNoise(yacc, yacc_var);
float zacc_corrupt = addZeroMeanNoise(zacc, zacc_var);
float rollspeed_corrupt = addZeroMeanNoise(rollspeed,rollspeed_var);
float pitchspeed_corrupt = addZeroMeanNoise(pitchspeed,pitchspeed_var);
float yawspeed_corrupt = addZeroMeanNoise(yawspeed,yawspeed_var);
float xmag_corrupt = addZeroMeanNoise(xmag, xmag_var);
float ymag_corrupt = addZeroMeanNoise(ymag, ymag_var);
float zmag_corrupt = addZeroMeanNoise(zmag, zmag_var);
float abs_pressure_corrupt = addZeroMeanNoise(abs_pressure,abs_pressure_var);
float diff_pressure_corrupt = addZeroMeanNoise(diff_pressure, diff_pressure_var);
float pressure_alt_corrupt = addZeroMeanNoise(pressure_alt, pressure_alt_var);
float temperature_corrupt = addZeroMeanNoise(temperature,temperature_var);
mavlink_message_t msg;
mavlink_msg_hil_sensor_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
time_us, xacc_corrupt, yacc_corrupt, zacc_corrupt, rollspeed_corrupt, pitchspeed_corrupt,
yawspeed_corrupt, xmag_corrupt, ymag_corrupt, zmag_corrupt, abs_pressure_corrupt,
diff_pressure_corrupt, pressure_alt_corrupt, temperature_corrupt, fields_changed);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
lastSendTimeSensors = QGC::groundTimeMilliseconds();
}
else
{
// Attempt to set HIL mode
_vehicle->setHilMode(true);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
}
}
#endif
#ifndef __mobile__
void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
float flow_comp_m_y, quint8 quality, float ground_distance)
{
if (!_vehicle) {
return;
}
// FIXME: This needs to be updated for new mavlink_msg_hil_optical_flow_pack api
Q_UNUSED(time_us);
Q_UNUSED(flow_x);
Q_UNUSED(flow_y);
Q_UNUSED(flow_comp_m_x);
Q_UNUSED(flow_comp_m_y);
Q_UNUSED(quality);
Q_UNUSED(ground_distance);
if (_vehicle->hilMode())
{
#if 0
mavlink_message_t msg;
mavlink_msg_hil_optical_flow_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
time_us, 0, 0 /* hack */, flow_x, flow_y, 0.0f /* hack */, 0.0f /* hack */, 0.0f /* hack */, 0 /* hack */, quality, ground_distance);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds();
#endif
}
else
{
// Attempt to set HIL mode
_vehicle->setHilMode(true);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
}
}
#endif
#ifndef __mobile__
void UAS::sendHilGps(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)
{
if (!_vehicle) {
return;
}
// Only send at 10 Hz max rate
if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100)
return;
if (_vehicle->hilMode())
{
float course = cog;
// map to 0..2pi
if (course < 0)
course += 2.0f * static_cast<float>(M_PI);
// scale from radians to degrees
course = (course / M_PI) * 180.0f;
mavlink_message_t msg;
mavlink_msg_hil_gps_pack_chan(mavlink->getSystemId(),
mavlink->getComponentId(),
_vehicle->priorityLink()->mavlinkChannel(),
&msg,
time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, vn*1e2, ve*1e2, vd*1e2, course*1e2, satellites);
lastSendTimeGPS = QGC::groundTimeMilliseconds();
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
}
else
{
// Attempt to set HIL mode
_vehicle->setHilMode(true);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
}
}
#endif
/**
* Connect flight gear link.
**/
#ifndef __mobile__
void UAS::startHil()
{
if (hilEnabled) return;
hilEnabled = true;
sensorHil = false;
_vehicle->setHilMode(true);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable.";
// Connect HIL simulation link
simulation->connectSimulation();
}
#endif
/**
* disable flight gear link.
*/
#ifndef __mobile__
void UAS::stopHil()
{
if (simulation && simulation->isConnected()) {
simulation->disconnectSimulation();
_vehicle->setHilMode(false);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to disable.";
}
hilEnabled = false;
sensorHil = false;
}
#endif
void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax)
{
if (!_vehicle) {
......@@ -1444,14 +1050,5 @@ void UAS::unsetRCToParameterMap()
void UAS::shutdownVehicle(void)
{
#ifndef __mobile__
stopHil();
if (simulation) {
// wait for the simulator to exit
simulation->wait();
simulation->disconnectSimulation();
simulation->deleteLater();
}
#endif
_vehicle = nullptr;
}
src/uas/UAS.h
View file @ e9bafe97
......@@ -22,9 +22,6 @@
#ifndef __mobile__
#include "FileManager.h"
#include "QGCHilLink.h"
#include "QGCJSBSimLink.h"
#include "QGCXPlaneLink.h"
#endif
Q_DECLARE_LOGGING_CATEGORY(UASLog)
......@@ -181,11 +178,6 @@ protected:
float pressure_alt_var; ///< variance of altitude pressure noise for HIL sim (hPa)
float temperature_var; ///< variance of temperature noise for HIL sim (C)
/// SIMULATION
#ifndef __mobile__
QGCHilLink* simulation; ///< Hardware in the loop simulation link
#endif
public:
/** @brief Get the human-readable status message for this code */
void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription);
......@@ -194,13 +186,6 @@ public:
virtual FileManager* getFileManager() { return &fileManager; }
#endif
/** @brief Get the HIL simulation */
#ifndef __mobile__
QGCHilLink* getHILSimulation() const {
return simulation;
}
#endif
QImage getImage();
void requestImage();
......@@ -208,52 +193,6 @@ public slots:
/** @brief Order the robot to pair its receiver **/
void pairRX(int rxType, int rxSubType);
/** @brief Enable / disable HIL */
#ifndef __mobile__
void enableHilJSBSim(bool enable, QString options);
void enableHilXPlane(bool enable);
/** @brief Send the full HIL state to the MAV */
void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate,
float pitchRotationRate, float yawRotationRate, 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 sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate,
float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt,
float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc);
/** @brief RAW sensors for sensor HIL */
void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed);
/** @brief Send Optical Flow sensor message for HIL, (arguments and units accoding to mavlink documentation*/
void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
float flow_comp_m_y, quint8 quality, float ground_distance);
float addZeroMeanNoise(float truth_meas, float noise_var);
/**
* @param time_us
* @param lat
* @param lon
* @param alt
* @param fix_type
* @param eph
* @param epv
* @param vel
* @param cog course over ground, in radians, -pi..pi
* @param satellites
*/
void sendHilGps(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);
/** @brief Places the UAV in Hardware-in-the-Loop simulation status **/
void startHil();
/** @brief Stops the UAV's Hardware-in-the-Loop simulation status **/
void stopHil();
#endif
/** @brief Set the values for the manual control of the vehicle */
void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode);
......@@ -280,8 +219,6 @@ signals:
void imageStarted(quint64 timestamp);
/** @brief A new camera image has arrived */
void imageReady(UASInterface* uas);
/** @brief HIL controls have changed */
void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode);
void rollChanged(double val,QString name);
void pitchChanged(double val,QString name);
......@@ -305,11 +242,6 @@ protected:
quint64 lastVoltageWarning; ///< Time at which the last voltage warning occurred
quint64 lastNonNullTime; ///< The last timestamp from the MAV that was not null
unsigned int onboardTimeOffsetInvalidCount; ///< Count when the offboard time offset estimation seemed wrong
bool hilEnabled;
bool sensorHil; ///< True if sensor HIL is enabled
quint64 lastSendTimeGPS; ///< Last HIL GPS message sent
quint64 lastSendTimeSensors; ///< Last HIL Sensors message sent
quint64 lastSendTimeOpticalFlow; ///< Last HIL Optical Flow message sent
private:
Vehicle* _vehicle;
......
src/uas/UASInterface.h
View file @ e9bafe97
......@@ -82,24 +82,6 @@ public slots:
/** @brief Order the robot to pair its receiver **/
virtual void pairRX(int rxType, int rxSubType) = 0;
/** @brief Send the full HIL state to the MAV */
#ifndef __mobile__
virtual void sendHilState(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) = 0;
/** @brief RAW sensors for sensor HIL */
virtual void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed) = 0;
/** @brief Send raw GPS for sensor HIL */
virtual void sendHilGps(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) = 0;
/** @brief Send Optical Flow sensor message for HIL, (arguments and units accoding to mavlink documentation*/
virtual void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
float flow_comp_m_y, quint8 quality, float ground_distance) = 0;
#endif
/** @brief Send command to map a RC channel to a parameter */
virtual void sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax) = 0;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment