Newer
Older
Lorenz Meier
committed
/*=====================================================================
QGroundControl Open Source Ground Control Station
(c) 2009 - 2011 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
This file is part of the QGROUNDCONTROL project
QGROUNDCONTROL is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
QGROUNDCONTROL is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file QGCXPlaneLink.cc
* Implementation of X-Plane interface
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#include <QTimer>
#include <QList>
#include <QDebug>
#include <QMutexLocker>
#include <QNetworkInterface>
Lorenz Meier
committed
#include <iostream>
#include <Eigen/Eigen>
Lorenz Meier
committed
#include "QGCXPlaneLink.h"
#include "QGC.h"
#include "UAS.h"
Lorenz Meier
committed
QGCXPlaneLink::QGCXPlaneLink(Vehicle* vehicle, QString remoteHost, QHostAddress localHost, quint16 localPort) :
_vehicle(vehicle),
Lorenz Meier
committed
socket(NULL),
Lorenz Meier
committed
process(NULL),
terraSync(NULL),
airframeID(QGCXPlaneLink::AIRFRAME_UNKNOWN),
xPlaneVersion(0),
simUpdateLast(QGC::groundTimeMilliseconds()),
simUpdateLastText(QGC::groundTimeMilliseconds()),
simUpdateLastGroundTruth(QGC::groundTimeMilliseconds()),
_sensorHilEnabled(true),
_should_exit(false)
Lorenz Meier
committed
{
Lorenz Meier
committed
// We're doing it wrong - because the Qt folks got the API wrong:
// http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/
moveToThread(this);
Lorenz Meier
committed
setTerminationEnabled(false);
this->localHost = localHost;
this->localPort = localPort/*+mav->getUASID()*/;
Lorenz Meier
committed
connectState = false;
this->name = tr("X-Plane Link (localPort:%1)").arg(localPort);
Lorenz Meier
committed
setRemoteHost(remoteHost);
Lorenz Meier
committed
}
QGCXPlaneLink::~QGCXPlaneLink()
Lorenz Meier
committed
if (socket) {
socket->close();
socket->deleteLater();
socket = NULL;
}
Lorenz Meier
committed
}
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());
selectAirframe(settings.value("AIRFRAME", "default").toString());
_sensorHilEnabled = settings.value("SENSOR_HIL", _sensorHilEnabled).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);
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
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));
}
Lorenz Meier
committed
/**
* @brief Runs the thread
*
**/
void QGCXPlaneLink::run()
{
Lorenz Meier
committed
emit statusMessage("No MAV present");
return;
}
if (connectState) {
emit statusMessage("Already connected");
return;
}
socket = new QUdpSocket(this);
connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint);
Lorenz Meier
committed
if (!connectState) {
emit statusMessage("Binding socket failed!");
Lorenz Meier
committed
socket->deleteLater();
Lorenz Meier
committed
socket = NULL;
return;
}
Lorenz Meier
committed
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(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();
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
#pragma pack(push, 1)
struct iset_struct
{
char b[5];
int index; // (0->20 in the lsit 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;
}
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;
writeBytes((const char*)&ip, sizeof(ip));
Lorenz Meier
committed
_should_exit = false;
while(!_should_exit) {
QCoreApplication::processEvents();
QGC::SLEEP::msleep(5);
}
Lorenz Meier
committed
disconnect(_vehicle->uas(), &UAS::hilControlsChanged, this, &QGCXPlaneLink::updateControls);
Lorenz Meier
committed
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);
Lorenz Meier
committed
connectState = false;
disconnect(socket, &QUdpSocket::readyRead, this, &QGCXPlaneLink::readBytes);
Lorenz Meier
committed
Lorenz Meier
committed
socket->close();
Lorenz Meier
committed
socket = NULL;
emit simulationDisconnected();
emit simulationConnected(false);
Lorenz Meier
committed
}
void QGCXPlaneLink::setPort(int localPort)
Lorenz Meier
committed
{
Lorenz Meier
committed
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;
Lorenz Meier
committed
}
QGCMessageBox::critical(tr("X-Plane HIL"), msg);
Lorenz Meier
committed
}
return QString("%1:%2").arg(remoteHost.toString()).arg(remotePort);
Lorenz Meier
committed
/**
* @param newHost Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551
Lorenz Meier
committed
*/
void QGCXPlaneLink::setRemoteHost(const QString& newHost)
Lorenz Meier
committed
{
Lorenz Meier
committed
{
QHostInfo info = QHostInfo::fromName(newHost.split(":").first());
Lorenz Meier
committed
if (info.error() == QHostInfo::NoError)
{
// Add newHost
QList<QHostAddress> newHostAddresses = info.addresses();
Lorenz Meier
committed
QHostAddress address;
Lorenz Meier
committed
{
// Exclude loopback IPv4 and all IPv6 addresses
if (!newHostAddresses.at(i).toString().contains(":"))
Lorenz Meier
committed
{
Lorenz Meier
committed
}
}
remoteHost = address;
// Set localPort according to user input
Lorenz Meier
committed
}
}
else
{
Lorenz Meier
committed
if (info.error() == QHostInfo::NoError)
{
Lorenz Meier
committed
}
}
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)
Lorenz Meier
committed
{
Lorenz Meier
committed
#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';
Hyon Lim (Retina)
committed
Q_UNUSED(time);
Q_UNUSED(systemMode);
Q_UNUSED(navMode);
bool isFixedWing = true;
if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR)
Hyon Lim (Retina)
committed
{
qDebug() << "MAV_TYPE_QUADROTOR";
// Individual effort will be provided directly to the actuators on Xplane quadrotor.
Hyon Lim (Retina)
committed
p.f[1] = rollAilerons;
p.f[2] = throttle;
p.f[3] = pitchElevator;
Hyon Lim (Retina)
committed
isFixedWing = false;
}
Hyon Lim (Retina)
committed
// direct pass-through, normal fixed-wing.
p.f[0] = -pitchElevator;
p.f[1] = rollAilerons;
p.f[2] = yawRudder;
}
Lorenz Meier
committed
Hyon Lim (Retina)
committed
if(isFixedWing)
{
// Ail / Elevon / Rudder
p.index = 12; // XPlane, wing sweep
writeBytes((const char*)&p, sizeof(p));
p.index = 8; // XPlane, joystick? why?
writeBytes((const char*)&p, sizeof(p));
p.index = 25; // Thrust
memset(p.f, 0, sizeof(p.f));
p.f[0] = throttle;
p.f[1] = throttle;
p.f[2] = throttle;
p.f[3] = throttle;
// Throttle
writeBytes((const char*)&p, sizeof(p));
}
else
{
qDebug() << "Transmitting p.index = 25";
p.index = 25; // XPlane, throttle command.
writeBytes((const char*)&p, sizeof(p));
}
Lorenz Meier
committed
}
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
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;
}
Lorenz Meier
committed
void QGCXPlaneLink::writeBytes(const char* data, qint64 size)
{
if (!data) return;
// If socket exists and is connected, transmit the data
if (socket && connectState)
Lorenz Meier
committed
{
socket->writeDatagram(data, size, remoteHost, remotePort);
Lorenz Meier
committed
}
}
/**
* @brief Read all pending packets from the interface.
Lorenz Meier
committed
**/
void QGCXPlaneLink::readBytes()
{
// Only emit updates on attitude message
bool emitUpdate = false;
Lorenz Meier
committed
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;
Lorenz Meier
committed
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;
}
Lorenz Meier
committed
// Calculate the number of data segments a 36 bytes
// XPlane always has 5 bytes header: 'DATA@'
unsigned nsegs = (s-5)/36;
Lorenz Meier
committed
//qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
Lorenz Meier
committed
#pragma pack(push, 1)
struct payload {
int index;
float f[8];
} p;
#pragma pack(pop)
bool oldConnectionState = xPlaneConnected;
Lorenz Meier
committed
if (data[0] == 'D' &&
data[1] == 'A' &&
data[2] == 'T' &&
data[3] == 'A')
{
xPlaneConnected = true;
Lorenz Meier
committed
if (oldConnectionState != xPlaneConnected) {
simUpdateFirst = QGC::groundTimeMilliseconds();
}
Lorenz Meier
committed
for (unsigned i = 0; i < nsegs; i++)
{
// Get index
unsigned ioff = (5+i*36);;
memcpy(&(p), data+ioff, sizeof(p));
if (p.index == 3)
{
ind_airspeed = p.f[5] * 0.44704f;
true_airspeed = p.f[6] * 0.44704f;
Lorenz Meier
committed
groundspeed = p.f[7] * 0.44704;
//qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
}
// 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);
// atmospheric pressure aircraft for XPlane 9 and 10
Lorenz Meier
committed
// inHg to hPa (hecto Pascal / millibar)
abs_pressure = p.f[0] * 33.863886666718317f;
// 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);
// }
Lorenz Meier
committed
else if ((xPlaneVersion == 10 && p.index == 16) || (xPlaneVersion == 9 && p.index == 17))
Lorenz Meier
committed
{
Lorenz Meier
committed
// 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);
Lorenz Meier
committed
}
else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18))
Lorenz Meier
committed
{
//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;
Lorenz Meier
committed
// X-Plane expresses yaw as 0..2 PI
if (yaw > M_PI) {
}
if (yaw < -M_PI) {
Lorenz Meier
committed
float yawmag = p.f[3] / 180.0f * M_PI;
Lorenz Meier
committed
// 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
Lorenz Meier
committed
}
Lorenz Meier
committed
Lorenz Meier
committed
// 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
Lorenz Meier
committed
}
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];
Lorenz Meier
committed
else if (p.index == 12)
{
//qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2];
Lorenz Meier
committed
}
else if (p.index == 25)
{
//qDebug() << "THROTTLE" << p.f[0] << p.f[1] << p.f[2] << p.f[3];
Lorenz Meier
committed
}
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];
Lorenz Meier
committed
}
else if (p.index == 11)
{
//qDebug() << "CONTROLS" << "ail" << p.f[0] << "elev" << p.f[1] << "rudder" << p.f[2] << "nwheel" << p.f[3];
Lorenz Meier
committed
}
else
{
//qDebug() << "UNKNOWN #" << p.index << p.f[0] << p.f[1] << p.f[2] << p.f[3];
Lorenz Meier
committed
}
}
}
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')
{
Lorenz Meier
committed
}
else
{
qDebug() << "UNKNOWN PACKET:" << data;
}
Lorenz Meier
committed
// Wait for 0.5s before actually using the data, so that all fields are filled
if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) {
return;
}
Lorenz Meier
committed
// 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;
Lorenz Meier
committed
/* 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);
Lorenz Meier
committed
// XXX make these GUI-configurable and add randomness
int gps_fix_type = 3;
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();
}
Lorenz Meier
committed
if (!oldConnectionState && xPlaneConnected)
{
emit statusMessage(tr("Receiving from XPlane."));
Lorenz Meier
committed
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
// // 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()
{
Lorenz Meier
committed
if (connectState)
Lorenz Meier
committed
{
Lorenz Meier
committed
_should_exit = true;
} else {
emit simulationDisconnected();
emit simulationConnected(false);
Lorenz Meier
committed
}
Lorenz Meier
committed
return !connectState;
}
Lorenz Meier
committed
{
airframeName = plane;
Lorenz Meier
committed
if (plane.contains("QRO"))
{
if (plane.contains("MK") && airframeID != AIRFRAME_QUAD_X_MK_10INCH_I2C)
{
airframeID = AIRFRAME_QUAD_X_MK_10INCH_I2C;
else if (plane.contains("ARDRONE") && airframeID != AIRFRAME_QUAD_X_ARDRONE)
{
airframeID = AIRFRAME_QUAD_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");
if (changed) emit airframeChanged("X Plane default");
Lorenz Meier
committed
}
void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw)
{
Lorenz Meier
committed
struct VEH1_struct
{
Lorenz Meier
committed
quint32 p;
double lat_lon_ele[3];
float psi_the_phi[3];
float gear_flap_vect[3];
} pos;
Lorenz Meier
committed
pos.header[0] = 'V';
pos.header[1] = 'E';
pos.header[2] = 'H';
pos.header[3] = '1';
pos.header[4] = '0';
Lorenz Meier
committed
pos.p = 0;
pos.lat_lon_ele[0] = lat;
pos.lat_lon_ele[1] = lon;
pos.lat_lon_ele[2] = alt;
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
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;
writeBytes((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;
// writeBytes((const char*)&pos, sizeof(pos));
Lorenz Meier
committed
}
/**
* 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() + offAlt < 0)
Lorenz Meier
committed
{
offAlt *= -1.0;
}
setPositionAttitude(_vehicle->latitude() + offLat,
_vehicle->longitude() + offLon,
_vehicle->altitudeAMSL() + offAlt,
_vehicle->roll(),
_vehicle->pitch(),
_vehicle->uas()->getYaw());
Lorenz Meier
committed
}
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(),
Lorenz Meier
committed
roll,
pitch,
yaw);
}
Lorenz Meier
committed
/**
* @brief Connect the connection.
*
* @return True if connection has been established, false if connection couldn't be established.
**/
bool QGCXPlaneLink::connectSimulation()
{
Lorenz Meier
committed
if (connectState) {
qDebug() << "Simulation already active";
} else {
qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
// XXX Hack
storeSettings();
start(HighPriority);
}
Lorenz Meier
committed
}
/**
* @brief Check if connection is active.
*
* @return True if link is connected, false otherwise.
**/
bool QGCXPlaneLink::isConnected()
{
return connectState;
}
QString QGCXPlaneLink::getName()
{
return name;
}