Skip to content

Q
qgroundcontrol
Commit 1396001f authored by DoinLakeFlyer's avatar DoinLakeFlyer
Browse files
Options

parent e28a6c9c
Show whitespace changes
Inline Side-by-side
Showing with 4 additions and 2659 deletions
qgroundcontrol.pro
View file @ 1396001f
......@@ -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 @ 1396001f
......@@ -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));
}
_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 @ 1396001f
......@@ -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 @ 1396001f
......@@ -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 @ e28a6c9c
#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 @ e28a6c9c
/****************************************************************************
*
* (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 @ e28a6c9c
/****************************************************************************
*
* (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 @ e28a6c9c
/****************************************************************************
*
* (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.cc
* Implementation of X-Plane interface
* @author Lorenz Meier <lm@qgroundcontrol.org>
*
*/
#include <QTimer>
#include <QList>
#include <QDebug>
#include <QMutexLocker>
#include <QNetworkInterface>
#include <QHostInfo>
#include <iostream>
#include <Eigen/Eigen>
#include "QGCXPlaneLink.h"
#include "QGC.h"
#include "UAS.h"
#include "UASInterface.h"
//-- TODO: #include "QGCMessageBox.h"
QGCXPlaneLink::QGCXPlaneLink(Vehicle* vehicle, QString remoteHost, QHostAddress localHost, quint16 localPort) :
_vehicle(vehicle),
remoteHost(QHostAddress("127.0.0.1")),
remotePort(49000),
socket(nullptr),
process(nullptr),
terraSync(nullptr),
barometerOffsetkPa(-8.0f),
airframeID(QGCXPlaneLink::AIRFRAME_UNKNOWN),
xPlaneConnected(false),
xPlaneVersion(0),
simUpdateLast(QGC::groundTimeMilliseconds()),
simUpdateFirst(0),
simUpdateLastText(QGC::groundTimeMilliseconds()),
simUpdateLastGroundTruth(QGC::groundTimeMilliseconds()),
simUpdateHz(0),
_sensorHilEnabled(true),
_useHilActuatorControls(true),
_should_exit(false)
{
// 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);
setTerminationEnabled(false);
this->localHost = localHost;
this->localPort = localPort/*+mav->getUASID()*/;
connectState = false;
this->name = tr("X-Plane Link (localPort:%1)").arg(localPort);
setRemoteHost(remoteHost);
loadSettings();
}
QGCXPlaneLink::~QGCXPlaneLink()
{
storeSettings();
// Tell the thread to exit
_should_exit = true;
if (socket) {
socket->close();
socket->deleteLater();
socket = nullptr;
}
}
void QGCXPlaneLink::loadSettings()
{
// Load defaults from settings
QSettings settings;
settings.beginGroup("QGC_XPLANE_LINK");
setRemoteHost(settings.value("REMOTE_HOST", QString("%1:%2").arg(remoteHost.toString()).arg(remotePort)).toString());
setVersion(settings.value("XPLANE_VERSION", 10).toInt());
selectAirframe(settings.value("AIRFRAME", "default").toString());
_sensorHilEnabled = settings.value("SENSOR_HIL", _sensorHilEnabled).toBool();
_useHilActuatorControls = settings.value("ACTUATOR_HIL", _useHilActuatorControls).toBool();
settings.endGroup();
}
void QGCXPlaneLink::storeSettings()
{
// Store settings
QSettings settings;
settings.beginGroup("QGC_XPLANE_LINK");
settings.setValue("REMOTE_HOST", QString("%1:%2").arg(remoteHost.toString()).arg(remotePort));
settings.setValue("XPLANE_VERSION", xPlaneVersion);
settings.setValue("AIRFRAME", airframeName);
settings.setValue("SENSOR_HIL", _sensorHilEnabled);
settings.setValue("ACTUATOR_HIL", _useHilActuatorControls);
settings.endGroup();
}
void QGCXPlaneLink::setVersion(const QString& version)
{
unsigned int oldVersion = xPlaneVersion;
if (version.contains("9"))
{
xPlaneVersion = 9;
}
else if (version.contains("10"))
{
xPlaneVersion = 10;
}
else if (version.contains("11"))
{
xPlaneVersion = 11;
}
else if (version.contains("12"))
{
xPlaneVersion = 12;
}
if (oldVersion != xPlaneVersion)
{
emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion));
}
}
void QGCXPlaneLink::setVersion(unsigned int version)
{
bool changed = (xPlaneVersion != version);
xPlaneVersion = version;
if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion));
}
void QGCXPlaneLink::enableHilActuatorControls(bool enable)
{
if (enable != _useHilActuatorControls) {
_useHilActuatorControls = enable;
}
/* Only use override for new message and specific airframes */
MAV_TYPE type = _vehicle->vehicleType();
float value = 0.0f;
if (type == MAV_TYPE_VTOL_RESERVED2) {
value = (enable ? 1.0f : 0.0f);
}
sendDataRef("sim/operation/override/override_control_surfaces", value);
emit useHilActuatorControlsChanged(enable);
}
/**
* @brief Runs the thread
*
**/
void QGCXPlaneLink::run()
{
if (!_vehicle) {
emit statusMessage("No MAV present");
return;
}
if (connectState) {
emit statusMessage("Already connected");
return;
}
socket = new QUdpSocket(this);
socket->moveToThread(this);
connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint);
if (!connectState) {
emit statusMessage("Binding socket failed!");
socket->deleteLater();
socket = nullptr;
return;
}
emit statusMessage(tr("Waiting for XPlane.."));
QObject::connect(socket, &QUdpSocket::readyRead, this, &QGCXPlaneLink::readBytes);
connect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCXPlaneLink::updateControls, Qt::QueuedConnection);
connect(_vehicle, &Vehicle::hilActuatorControlsChanged, this, &QGCXPlaneLink::updateActuatorControls, Qt::QueuedConnection);
connect(this, &QGCXPlaneLink::hilGroundTruthChanged, _vehicle->uas(), &UAS::sendHilGroundTruth, Qt::QueuedConnection);
connect(this, &QGCXPlaneLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState, Qt::QueuedConnection);
connect(this, &QGCXPlaneLink::sensorHilGpsChanged, _vehicle->uas(), &UAS::sendHilGps, Qt::QueuedConnection);
connect(this, &QGCXPlaneLink::sensorHilRawImuChanged, _vehicle->uas(), &UAS::sendHilSensors, Qt::QueuedConnection);
_vehicle->uas()->startHil();
#pragma pack(push, 1)
struct iset_struct
{
char b[5];
int index; // (0->20 in the list below)
char str_ipad_them[16];
char str_port_them[6];
char padding[2];
int use_ip;
} ip; // to use this option, 0 not to.
#pragma pack(pop)
ip.b[0] = 'I';
ip.b[1] = 'S';
ip.b[2] = 'E';
ip.b[3] = 'T';
ip.b[4] = '0';
QList<QHostAddress> hostAddresses = QNetworkInterface::allAddresses();
QString localAddrStr;
QString localPortStr = QString("%1").arg(localPort);
for (int i = 0; i < hostAddresses.size(); i++)
{
// Exclude loopback IPv4 and all IPv6 addresses
if (hostAddresses.at(i) != QHostAddress("127.0.0.1") && !hostAddresses.at(i).toString().contains(":"))
{
localAddrStr = hostAddresses.at(i).toString();
break;
}
}
ip.index = 0;
strncpy(ip.str_ipad_them, localAddrStr.toLatin1(), qMin((int)sizeof(ip.str_ipad_them), 16));
strncpy(ip.str_port_them, localPortStr.toLatin1(), qMin((int)sizeof(ip.str_port_them), 6));
ip.use_ip = 1;
writeBytesSafe((const char*)&ip, sizeof(ip));
/* Call function which makes sure individual control override is enabled/disabled */
enableHilActuatorControls(_useHilActuatorControls);
_should_exit = false;
while(!_should_exit) {
QCoreApplication::processEvents();
QGC::SLEEP::msleep(5);
}
disconnect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCXPlaneLink::updateControls);
disconnect(this, &QGCXPlaneLink::hilGroundTruthChanged, _vehicle->uas(), &UAS::sendHilGroundTruth);
disconnect(this, &QGCXPlaneLink::hilStateChanged, _vehicle->uas(), &UAS::sendHilState);
disconnect(this, &QGCXPlaneLink::sensorHilGpsChanged, _vehicle->uas(), &UAS::sendHilGps);
disconnect(this, &QGCXPlaneLink::sensorHilRawImuChanged, _vehicle->uas(), &UAS::sendHilSensors);
connectState = false;
disconnect(socket, &QUdpSocket::readyRead, this, &QGCXPlaneLink::readBytes);
socket->close();
socket->deleteLater();
socket = nullptr;
emit simulationDisconnected();
emit simulationConnected(false);
}
void QGCXPlaneLink::setPort(int localPort)
{
this->localPort = localPort;
disconnectSimulation();
connectSimulation();
}
void QGCXPlaneLink::processError(QProcess::ProcessError err)
{
QString msg;
switch(err) {
case QProcess::FailedToStart:
msg = tr("X-Plane Failed to start. Please check if the path and command is correct");
break;
case QProcess::Crashed:
msg = tr("X-Plane crashed. This is an X-Plane-related problem, check for X-Plane upgrade.");
break;
case QProcess::Timedout:
msg = tr("X-Plane 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 X-Plane. Please check if the path and command are correct");
break;
case QProcess::UnknownError:
default:
msg = tr("X-Plane error occurred. Please check if the path and command is correct.");
break;
}
//-- TODO: QGCMessageBox::critical(tr("X-Plane HIL"), msg);
}
QString QGCXPlaneLink::getRemoteHost()
{
return QString("%1:%2").arg(remoteHost.toString()).arg(remotePort);
}
/**
* @param newHost Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551
*/
void QGCXPlaneLink::setRemoteHost(const QString& newHost)
{
if (newHost.length() == 0)
return;
if (newHost.contains(":"))
{
QHostInfo info = QHostInfo::fromName(newHost.split(":").first());
if (info.error() == QHostInfo::NoError)
{
// Add newHost
QList<QHostAddress> newHostAddresses = info.addresses();
QHostAddress address;
for (int i = 0; i < newHostAddresses.size(); i++)
{
// Exclude loopback IPv4 and all IPv6 addresses
if (!newHostAddresses.at(i).toString().contains(":"))
{
address = newHostAddresses.at(i);
}
}
remoteHost = address;
// Set localPort according to user input
remotePort = newHost.split(":").last().toInt();
}
}
else
{
QHostInfo info = QHostInfo::fromName(newHost);
if (info.error() == QHostInfo::NoError)
{
// Add newHost
remoteHost = info.addresses().first();
if (remotePort == 0) remotePort = 49000;
}
}
if (isConnected())
{
disconnectSimulation();
connectSimulation();
}
emit remoteChanged(QString("%1:%2").arg(remoteHost.toString()).arg(remotePort));
}
void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode)
{
/* Only use HIL_CONTROL when the checkbox is unchecked */
if (_useHilActuatorControls) {
//qDebug() << "received HIL_CONTROL but not using it";
return;
}
#pragma pack(push, 1)
struct payload {
char b[5];
int index;
float f[8];
} p;
#pragma pack(pop)
p.b[0] = 'D';
p.b[1] = 'A';
p.b[2] = 'T';
p.b[3] = 'A';
p.b[4] = '\0';
Q_UNUSED(time);
Q_UNUSED(systemMode);
Q_UNUSED(navMode);
if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR
|| _vehicle->vehicleType() == MAV_TYPE_HEXAROTOR
|| _vehicle->vehicleType() == MAV_TYPE_OCTOROTOR)
{
qDebug() << "MAV_TYPE_QUADROTOR";
// Individual effort will be provided directly to the actuators on Xplane quadrotor.
p.f[0] = yawRudder;
p.f[1] = rollAilerons;
p.f[2] = throttle;
p.f[3] = pitchElevator;
// Direct throttle control
p.index = 25;
writeBytesSafe((const char*)&p, sizeof(p));
}
else
{
// direct pass-through, normal fixed-wing.
p.f[0] = -pitchElevator;
p.f[1] = rollAilerons;
p.f[2] = yawRudder;
// Ail / Elevon / Rudder
// Send to group 12
p.index = 12;
writeBytesSafe((const char*)&p, sizeof(p));
// Send to group 8, which equals manual controls
p.index = 8;
writeBytesSafe((const char*)&p, sizeof(p));
// Send throttle to all four motors
p.index = 25;
memset(p.f, 0, sizeof(p.f));
p.f[0] = throttle;
p.f[1] = throttle;
p.f[2] = throttle;
p.f[3] = throttle;
writeBytesSafe((const char*)&p, sizeof(p));
}
}
void QGCXPlaneLink::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)
{
if (!_useHilActuatorControls) {
//qDebug() << "received HIL_ACTUATOR_CONTROLS but not using it";
return;
}
Q_UNUSED(time);
Q_UNUSED(flags);
Q_UNUSED(mode);
Q_UNUSED(ctl_12);
Q_UNUSED(ctl_13);
Q_UNUSED(ctl_14);
Q_UNUSED(ctl_15);
#pragma pack(push, 1)
struct payload {
char b[5];
int index;
float f[8];
} p;
#pragma pack(pop)
p.b[0] = 'D';
p.b[1] = 'A';
p.b[2] = 'T';
p.b[3] = 'A';
p.b[4] = '\0';
/* Initialize with zeroes */
memset(p.f, 0, sizeof(p.f));
switch (_vehicle->vehicleType()) {
case MAV_TYPE_QUADROTOR:
case MAV_TYPE_HEXAROTOR:
case MAV_TYPE_OCTOROTOR:
{
p.f[0] = ctl_0; ///< X-Plane Engine 1
p.f[1] = ctl_1; ///< X-Plane Engine 2
p.f[2] = ctl_2; ///< X-Plane Engine 3
p.f[3] = ctl_3; ///< X-Plane Engine 4
p.f[4] = ctl_4; ///< X-Plane Engine 5
p.f[5] = ctl_5; ///< X-Plane Engine 6
p.f[6] = ctl_6; ///< X-Plane Engine 7
p.f[7] = ctl_7; ///< X-Plane Engine 8
/* Direct throttle control */
p.index = 25;
writeBytesSafe((const char*)&p, sizeof(p));
break;
}
case MAV_TYPE_VTOL_RESERVED2:
{
/**
* Tailsitter with four control flaps and eight motors.
*/
/* Throttle channels */
p.f[0] = ctl_0;
p.f[1] = ctl_1;
p.f[2] = ctl_2;
p.f[3] = ctl_3;
p.f[4] = ctl_4;
p.f[5] = ctl_5;
p.f[6] = ctl_6;
p.f[7] = ctl_7;
p.index = 25;
writeBytesSafe((const char*)&p, sizeof(p));
/* Control individual actuators */
float max_surface_deflection = 30.0f; // Degrees
sendDataRef("sim/flightmodel/controls/wing1l_ail1def", ctl_8 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing1r_ail1def", ctl_9 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing2l_ail1def", ctl_10 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing2r_ail1def", ctl_11 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing1l_ail2def", ctl_12 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing1r_ail2def", ctl_13 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing2l_ail2def", ctl_14 * max_surface_deflection);
sendDataRef("sim/flightmodel/controls/wing2r_ail2def", ctl_15 * max_surface_deflection);
break;
}
default:
{
/* direct pass-through, normal fixed-wing. */
p.f[0] = -ctl_1; ///< X-Plane Elevator
p.f[1] = ctl_0; ///< X-Plane Aileron
p.f[2] = ctl_2; ///< X-Plane Rudder
/* Send to group 8, which equals manual controls */
p.index = 8;
writeBytesSafe((const char*)&p, sizeof(p));
/* Send throttle to all eight motors */
p.index = 25;
p.f[0] = ctl_3;
p.f[1] = ctl_3;
p.f[2] = ctl_3;
p.f[3] = ctl_3;
p.f[4] = ctl_3;
p.f[5] = ctl_3;
p.f[6] = ctl_3;
p.f[7] = ctl_3;
writeBytesSafe((const char*)&p, sizeof(p));
/* Send flap signals, assuming that they are mapped to channel 5 (ctl_4) */
sendDataRef("sim/flightmodel/controls/flaprqst", ctl_4);
sendDataRef("sim/flightmodel/controls/flap2rqst", ctl_4);
break;
}
}
}
Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) {
double c__ = cos(yaw);
double _c_ = cos(pitch);
double __c = cos(roll);
double s__ = sin(yaw);
double _s_ = sin(pitch);
double __s = sin(roll);
double cc_ = c__ * _c_;
double cs_ = c__ * _s_;
double sc_ = s__ * _c_;
double ss_ = s__ * _s_;
double c_c = c__ * __c;
double c_s = c__ * __s;
double s_c = s__ * __c;
double s_s = s__ * __s;
double _cc = _c_ * __c;
double _cs = _c_ * __s;
double csc = cs_ * __c;
double css = cs_ * __s;
double ssc = ss_ * __c;
double sss = ss_ * __s;
Eigen::Matrix3f wRo;
wRo <<
cc_ , css-s_c, csc+s_s,
sc_ , sss+c_c, ssc-c_s,
-_s_ , _cs, _cc;
return wRo;
}
void QGCXPlaneLink::_writeBytes(const QByteArray data)
{
if (data.isEmpty()) return;
// If socket exists and is connected, transmit the data
if (socket && connectState)
{
socket->writeDatagram(data, remoteHost, remotePort);
}
}
/**
* @brief Read all pending packets from the interface.
**/
void QGCXPlaneLink::readBytes()
{
// Only emit updates on attitude message
bool emitUpdate = false;
quint16 fields_changed = 0;
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: " << s << std::endl;
socket->readDatagram(data, maxLength, &sender, &senderPort);
if (s > maxLength) {
std::string headStr = std::string(data, data+5);
std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl;
}
// Calculate the number of data segments a 36 bytes
// XPlane always has 5 bytes header: 'DATA@'
unsigned nsegs = (s-5)/36;
//qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
#pragma pack(push, 1)
struct payload {
int index;
float f[8];
} p;
#pragma pack(pop)
bool oldConnectionState = xPlaneConnected;
if (data[0] == 'D' &&
data[1] == 'A' &&
data[2] == 'T' &&
data[3] == 'A')
{
xPlaneConnected = true;
if (oldConnectionState != xPlaneConnected) {
simUpdateFirst = QGC::groundTimeMilliseconds();
}
for (unsigned i = 0; i < nsegs; i++)
{
// Get index
unsigned ioff = (5+i*36);;
memcpy(&(p), data+ioff, sizeof(p));
if (p.index == 3)
{
float knotsToMetersPerSecond = 0.514444f;
ind_airspeed = p.f[5] * knotsToMetersPerSecond;
true_airspeed = p.f[6] * knotsToMetersPerSecond;
groundspeed = p.f[7] * knotsToMetersPerSecond;
//qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
}
if (p.index == 4)
{
// WORKAROUND: IF ground speed <<1m/s and altitude-above-ground <1m, do NOT use the X-Plane data, because X-Plane (tested
// with v10.3 and earlier) delivers yacc=0 and zacc=0 when the ground speed is very low, which gives e.g. wrong readings
// before launch when waiting on the runway. This might pose a problem for initial state estimation/calibration.
// Instead, we calculate our own accelerations.
if (fabsf(groundspeed)<0.1f && alt_agl<1.0)
{
// TODO: Add centrip. acceleration to the current static acceleration implementation.
Eigen::Vector3f g(0, 0, -9.80665f);
Eigen::Matrix3f R = euler_to_wRo(yaw, pitch, roll);
Eigen::Vector3f gr = R.transpose().eval() * g;
xacc = gr[0];
yacc = gr[1];
zacc = gr[2];
//qDebug() << "Calculated values" << gr[0] << gr[1] << gr[2];
}
else
{
// Accelerometer readings, directly from X-Plane and including centripetal forces.
const float one_g = 9.80665f;
xacc = p.f[5] * one_g;
yacc = p.f[6] * one_g;
zacc = -p.f[4] * one_g;
//qDebug() << "X-Plane values" << xacc << yacc << zacc;
}
fields_changed |= (1 << 0) | (1 << 1) | (1 << 2);
emitUpdate = true;
}
// atmospheric pressure aircraft for XPlane 9 and 10
else if (p.index == 6)
{
// inHg to hPa (hecto Pascal / millibar)
abs_pressure = p.f[0] * 33.863886666718317f;
temperature = p.f[1];
fields_changed |= (1 << 9) | (1 << 12);
}
// Forward controls from X-Plane to MAV, not very useful
// better: Connect Joystick to QGroundControl
// else if (p.index == 8)
// {
// //qDebug() << "MAN:" << p.f[0] << p.f[3] << p.f[7];
// man_roll = p.f[0];
// man_pitch = p.f[1];
// man_yaw = p.f[2];
// UAS* uas = dynamic_cast<UAS*>(mav);
// if (uas) uas->setManualControlCommands(man_roll, man_pitch, man_yaw, 0.6);
// }
else if ((xPlaneVersion == 10 && p.index == 16) || (xPlaneVersion == 9 && p.index == 17))
{
// Cross checked with XPlane flight
pitchspeed = p.f[0];
rollspeed = p.f[1];
yawspeed = p.f[2];
fields_changed |= (1 << 3) | (1 << 4) | (1 << 5);
emitUpdate = true;
}
else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18))
{
//qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
pitch = p.f[0] / 180.0f * M_PI;
roll = p.f[1] / 180.0f * M_PI;
yaw = p.f[2] / 180.0f * M_PI;
// X-Plane expresses yaw as 0..2 PI
if (yaw > M_PI) {
yaw -= 2.0f * static_cast<float>(M_PI);
}
if (yaw < -M_PI) {
yaw += 2.0f * static_cast<float>(M_PI);
}
float yawmag = p.f[3] / 180.0f * M_PI;
if (yawmag > M_PI) {
yawmag -= 2.0f * static_cast<float>(M_PI);
}
if (yawmag < -M_PI) {
yawmag += 2.0f * static_cast<float>(M_PI);
}
// Normal rotation matrix, but since we rotate the
// vector [0.25 0 0.45]', we end up with these relevant
// matrix parts.
xmag = cos(-yawmag) * 0.25f;
ymag = sin(-yawmag) * 0.25f;
zmag = 0.45f;
fields_changed |= (1 << 6) | (1 << 7) | (1 << 8);
double cosPhi = cos(roll);
double sinPhi = sin(roll);
double cosThe = cos(pitch);
double sinThe = sin(pitch);
double cosPsi = cos(0.0);
double sinPsi = sin(0.0);
float dcm[3][3];
dcm[0][0] = cosThe * cosPsi;
dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
dcm[1][0] = cosThe * sinPsi;
dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
dcm[2][0] = -sinThe;
dcm[2][1] = sinPhi * cosThe;
dcm[2][2] = cosPhi * cosThe;
Eigen::Matrix3f m = Eigen::Map<Eigen::Matrix3f>((float*)dcm).eval();
Eigen::Vector3f mag(xmag, ymag, zmag);
Eigen::Vector3f magbody = m * mag;
// qDebug() << "yaw mag:" << p.f[2] << "x" << xmag << "y" << ymag;
// qDebug() << "yaw mag in body:" << magbody(0) << magbody(1) << magbody(2);
xmag = magbody(0);
ymag = magbody(1);
zmag = magbody(2);
// Rotate the measurement vector into the body frame using roll and pitch
emitUpdate = true;
}
// else if (p.index == 19)
// {
// qDebug() << "ATT:" << p.f[0] << p.f[1] << p.f[2];
// }
else if (p.index == 20)
{
//qDebug() << "LAT/LON/ALT:" << p.f[0] << p.f[1] << p.f[2];
lat = p.f[0];
lon = p.f[1];
alt = p.f[2] * 0.3048f; // convert feet (MSL) to meters
alt_agl = p.f[3] * 0.3048f; //convert feet (AGL) to meters
}
else if (p.index == 21)
{
vy = p.f[3];
vx = -p.f[5];
// moving 'up' in XPlane is positive, but its negative in NED
// for us.
vz = -p.f[4];
}
else if (p.index == 12)
{
//qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2];
}
else if (p.index == 25)
{
//qDebug() << "THROTTLE" << p.f[0] << p.f[1] << p.f[2] << p.f[3];
}
else if (p.index == 0)
{
//qDebug() << "STATS" << "fgraphics/s" << p.f[0] << "fsim/s" << p.f[2] << "t frame" << p.f[3] << "cpu load" << p.f[4] << "grnd ratio" << p.f[5] << "filt ratio" << p.f[6];
}
else if (p.index == 11)
{
//qDebug() << "CONTROLS" << "ail" << p.f[0] << "elev" << p.f[1] << "rudder" << p.f[2] << "nwheel" << p.f[3];
}
else
{
//qDebug() << "UNKNOWN #" << p.index << p.f[0] << p.f[1] << p.f[2] << p.f[3];
}
}
}
else if (data[0] == 'S' &&
data[1] == 'N' &&
data[2] == 'A' &&
data[3] == 'P')
{
}
else if (data[0] == 'S' &&
data[1] == 'T' &&
data[2] == 'A' &&
data[3] == 'T')
{
}
else
{
qDebug() << "UNKNOWN PACKET:" << data;
}
// Wait for 0.5s before actually using the data, so that all fields are filled
if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) {
return;
}
// Send updated state
if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 2)
{
simUpdateHz = simUpdateHz * 0.9f + 0.1f * (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast));
if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000) {
emit statusMessage(tr("Receiving from XPlane at %1 Hz").arg(static_cast<int>(simUpdateHz)));
// Reset lowpass with current value
simUpdateHz = (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast));
// Set state
simUpdateLastText = QGC::groundTimeMilliseconds();
}
simUpdateLast = QGC::groundTimeMilliseconds();
if (_sensorHilEnabled)
{
diff_pressure = (ind_airspeed * ind_airspeed * 1.225f) / 2.0f;
/* tropospheric properties (0-11km) for standard atmosphere */
const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */
const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */
const double g = 9.80665; /* gravity constant in m/s/s */
const double R = 287.05; /* ideal gas constant in J/kg/K */
/* current pressure at MSL in kPa */
double p1 = 1013.25 / 10.0;
/* measured pressure in hPa, plus offset to simulate weather effects / offsets */
double p = abs_pressure / 10.0 + barometerOffsetkPa;
/*
* Solve:
*
* / -(aR / g) \
* | (p / p1) . T1 | - T1
* \ /
* h = ------------------------------- + h1
* a
*/
pressure_alt = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
// set pressure alt to changed
fields_changed |= (1 << 11);
emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed,
xmag, ymag, zmag, abs_pressure, diff_pressure / 100.0, pressure_alt, temperature, fields_changed);
// XXX make these GUI-configurable and add randomness
int gps_fix_type = 3;
float eph = 0.3f;
float epv = 0.6f;
float vel = sqrt(vx*vx + vy*vy + vz*vz);
float cog = atan2(vy, vx);
int satellites = 8;
emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites);
} else {
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
pitchspeed, yawspeed, lat, lon, alt,
vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
}
// Limit ground truth to 25 Hz
if (QGC::groundTimeMilliseconds() - simUpdateLastGroundTruth > 40) {
emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
pitchspeed, yawspeed, lat, lon, alt,
vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
simUpdateLastGroundTruth = QGC::groundTimeMilliseconds();
}
}
if (!oldConnectionState && xPlaneConnected)
{
emit statusMessage(tr("Receiving from XPlane."));
}
// // Echo data for debugging purposes
// std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl;
// int i;
// for (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 QGCXPlaneLink::bytesAvailable()
{
return socket->pendingDatagramSize();
}
/**
* @brief Disconnect the connection.
*
* @return True if connection has been disconnected, false if connection couldn't be disconnected.
**/
bool QGCXPlaneLink::disconnectSimulation()
{
if (connectState)
{
_should_exit = true;
} else {
emit simulationDisconnected();
emit simulationConnected(false);
}
return !connectState;
}
void QGCXPlaneLink::selectAirframe(const QString& plane)
{
airframeName = plane;
if (plane.contains("QRO"))
{
if (plane.contains("MK") && airframeID != AIRFRAME_QUAD_X_MK_10INCH_I2C)
{
airframeID = AIRFRAME_QUAD_X_MK_10INCH_I2C;
emit airframeChanged("QRO_X / MK");
}
else if (plane.contains("ARDRONE") && airframeID != AIRFRAME_QUAD_X_ARDRONE)
{
airframeID = AIRFRAME_QUAD_X_ARDRONE;
emit airframeChanged("QRO_X / ARDRONE");
}
else
{
bool changed = (airframeID != AIRFRAME_QUAD_DJI_F450_PWM);
airframeID = AIRFRAME_QUAD_DJI_F450_PWM;
if (changed) emit airframeChanged("QRO_X / DJI-F450 / PWM");
}
}
else
{
bool changed = (airframeID != AIRFRAME_UNKNOWN);
airframeID = AIRFRAME_UNKNOWN;
if (changed) emit airframeChanged("X Plane default");
}
}
void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw)
{
#pragma pack(push, 1)
struct VEH1_struct
{
char header[5];
quint32 p;
double lat_lon_ele[3];
float psi_the_phi[3];
float gear_flap_vect[3];
} pos;
#pragma pack(pop)
pos.header[0] = 'V';
pos.header[1] = 'E';
pos.header[2] = 'H';
pos.header[3] = '1';
pos.header[4] = '0';
pos.p = 0;
pos.lat_lon_ele[0] = lat;
pos.lat_lon_ele[1] = lon;
pos.lat_lon_ele[2] = alt;
pos.psi_the_phi[0] = roll;
pos.psi_the_phi[1] = pitch;
pos.psi_the_phi[2] = yaw;
pos.gear_flap_vect[0] = 0.0f;
pos.gear_flap_vect[1] = 0.0f;
pos.gear_flap_vect[2] = 0.0f;
writeBytesSafe((const char*)&pos, sizeof(pos));
// pos.header[0] = 'V';
// pos.header[1] = 'E';
// pos.header[2] = 'H';
// pos.header[3] = '1';
// pos.header[4] = '0';
// pos.p = 0;
// pos.lat_lon_ele[0] = -999;
// pos.lat_lon_ele[1] = -999;
// pos.lat_lon_ele[2] = -999;
// pos.psi_the_phi[0] = -999;
// pos.psi_the_phi[1] = -999;
// pos.psi_the_phi[2] = -999;
// pos.gear_flap_vect[0] = -999;
// pos.gear_flap_vect[1] = -999;
// pos.gear_flap_vect[2] = -999;
// writeBytesSafe((const char*)&pos, sizeof(pos));
}
/**
* Sets a random position with an offset of max 1/1000 degree
* and max 100 m altitude
*/
void QGCXPlaneLink::setRandomPosition()
{
// Initialize generator
srand(0);
double offLat = rand() / static_cast<double>(RAND_MAX) / 500.0 + 1.0/500.0;
double offLon = rand() / static_cast<double>(RAND_MAX) / 500.0 + 1.0/500.0;
double offAlt = rand() / static_cast<double>(RAND_MAX) * 200.0 + 100.0;
if (_vehicle->altitudeAMSL()->rawValue().toDouble() + offAlt < 0)
{
offAlt *= -1.0;
}
setPositionAttitude(_vehicle->latitude() + offLat,
_vehicle->longitude() + offLon,
_vehicle->altitudeAMSL()->rawValue().toDouble() + offAlt,
_vehicle->roll()->rawValue().toDouble(),
_vehicle->pitch()->rawValue().toDouble(),
_vehicle->heading()->rawValue().toDouble());
}
void QGCXPlaneLink::setRandomAttitude()
{
// Initialize generator
srand(0);
double roll = rand() / static_cast<double>(RAND_MAX) * 2.0 - 1.0;
double pitch = rand() / static_cast<double>(RAND_MAX) * 2.0 - 1.0;
double yaw = rand() / static_cast<double>(RAND_MAX) * 2.0 - 1.0;
setPositionAttitude(_vehicle->latitude(),
_vehicle->longitude(),
_vehicle->altitudeAMSL()->rawValue().toDouble(),
roll,
pitch,
yaw);
}
/**
* @brief Connect the connection.
*
* @return True if connection has been established, false if connection couldn't be established.
**/
bool QGCXPlaneLink::connectSimulation()
{
if (connectState) {
qDebug() << "Simulation already active";
} else {
qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
// XXX Hack
storeSettings();
start(HighPriority);
}
return true;
}
/**
* @brief Check if connection is active.
*
* @return True if link is connected, false otherwise.
**/
bool QGCXPlaneLink::isConnected()
{
return connectState;
}
QString QGCXPlaneLink::getName()
{
return name;
}
void QGCXPlaneLink::setName(QString name)
{
this->name = name;
// emit nameChanged(this->name);
}
void QGCXPlaneLink::sendDataRef(QString ref, float value)
{
#pragma pack(push, 1)
struct payload {
char b[5];
float value;
char name[500];
} dref;
#pragma pack(pop)
dref.b[0] = 'D';
dref.b[1] = 'R';
dref.b[2] = 'E';
dref.b[3] = 'F';
dref.b[4] = '0';
/* Set value */
dref.value = value;
/* Fill name with zeroes */
memset(dref.name, 0, sizeof(dref.name));
/* Set dref name */
/* Send command */
QByteArray ba = ref.toUtf8();
if (ba.length() > 500) {
return;
}
for (int i = 0; i < ba.length(); i++) {
dref.name[i] = ba.at(i);
}
writeBytesSafe((const char*)&dref, sizeof(dref));
}
src/comm/QGCXPlaneLink.h deleted 100644 → 0
View file @ e28a6c9c
/****************************************************************************
*
* (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 @ 1396001f
......@@ -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 @ 1396001f
......@@ -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 @ 1396001f
......@@ -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;
......
Markdown is supported
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