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 "QGCXPlaneLink.h"
#include "QGC.h"
#include <QHostInfo>
#include "UAS.h"
Lorenz Meier
committed
#include "MainWindow.h"
QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress localHost, quint16 localPort) :
Lorenz Meier
committed
mav(mav),
Lorenz Meier
committed
socket(NULL),
Lorenz Meier
committed
process(NULL),
terraSync(NULL),
airframeID(QGCXPlaneLink::AIRFRAME_UNKNOWN),
xPlaneVersion(0),
simUpdateLast(QGC::groundTimeMilliseconds()),
simUpdateLastText(QGC::groundTimeMilliseconds()),
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);
this->localHost = localHost;
this->localPort = localPort/*+mav->getUASID()*/;
Lorenz Meier
committed
this->connectState = false;
this->name = tr("X-Plane Link (localPort:%1)").arg(localPort);
Lorenz Meier
committed
setRemoteHost(remoteHost);
Lorenz Meier
committed
}
QGCXPlaneLink::~QGCXPlaneLink()
// if(connectState) {
// disconnectSimulation();
// }
Lorenz Meier
committed
}
void QGCXPlaneLink::loadSettings()
{
// Load defaults from settings
QSettings settings;
settings.sync();
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);
settings.endGroup();
settings.sync();
}
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
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()
{
exec();
}
void QGCXPlaneLink::setPort(int localPort)
Lorenz Meier
committed
{
Lorenz Meier
committed
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
disconnectSimulation();
connectSimulation();
}
void QGCXPlaneLink::processError(QProcess::ProcessError err)
{
switch(err)
{
case QProcess::FailedToStart:
MainWindow::instance()->showCriticalMessage(tr("X-Plane Failed to Start"), tr("Please check if the path and command is correct"));
break;
case QProcess::Crashed:
MainWindow::instance()->showCriticalMessage(tr("X-Plane Crashed"), tr("This is a X-Plane-related problem. Please upgrade X-Plane"));
break;
case QProcess::Timedout:
MainWindow::instance()->showCriticalMessage(tr("X-Plane Start Timed Out"), tr("Please check if the path and command is correct"));
break;
case QProcess::WriteError:
MainWindow::instance()->showCriticalMessage(tr("Could not Communicate with X-Plane"), tr("Please check if the path and command is correct"));
break;
case QProcess::ReadError:
MainWindow::instance()->showCriticalMessage(tr("Could not Communicate with X-Plane"), tr("Please check if the path and command is correct"));
break;
case QProcess::UnknownError:
default:
MainWindow::instance()->showCriticalMessage(tr("X-Plane Error"), tr("Please check if the path and command is correct."));
break;
}
}
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::updateActuators(uint64_t time, float act1, float act2, float act3, float act4, float act5, float act6, float act7, float act8)
{
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
// Only update this for multirotors
{
Q_UNUSED(time);
Q_UNUSED(act5);
Q_UNUSED(act6);
Q_UNUSED(act7);
Q_UNUSED(act8);
#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';
p.index = 25;
memset(p.f, 0, sizeof(p.f));
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
p.f[0] = act1;
p.f[1] = act2;
p.f[2] = act3;
p.f[3] = act4;
// XXX the system corrects for the scale onboard, do not scale again
// if (airframeID == AIRFRAME_QUAD_X_MK_10INCH_I2C)
// {
// p.f[0] = act1 / 255.0f;
// p.f[1] = act2 / 255.0f;
// p.f[2] = act3 / 255.0f;
// p.f[3] = act4 / 255.0f;
// }
// else if (airframeID == AIRFRAME_QUAD_X_ARDRONE)
// {
// p.f[0] = act1 / 500.0f;
// p.f[1] = act2 / 500.0f;
// p.f[2] = act3 / 500.0f;
// p.f[3] = act4 / 500.0f;
// }
// else
// {
// p.f[0] = (act1 - 1000.0f) / 1000.0f;
// p.f[1] = (act2 - 1000.0f) / 1000.0f;
// p.f[2] = (act3 - 1000.0f) / 1000.0f;
// p.f[3] = (act4 - 1000.0f) / 1000.0f;
// }
// Throttle
writeBytes((const char*)&p, sizeof(p));
}
Lorenz Meier
committed
}
void QGCXPlaneLink::updateControls(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode)
{
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 (mav->getAirframe() == UASInterface::QGC_AIRFRAME_X8 ||
mav->getAirframe() == UASInterface::QGC_AIRFRAME_VIPER_2_0 ||
mav->getAirframe() == UASInterface::QGC_AIRFRAME_CAMFLYER_Q)
{
// de-mix delta-mixed inputs
// pitch input - mixed roll and pitch channels
p.f[0] = 0.5f * (rollAilerons - pitchElevator);
// roll input - mixed roll and pitch channels
p.f[1] = 0.5f * (rollAilerons + pitchElevator);
// yaw
p.f[2] = 0.0f;
}
Hyon Lim (Retina)
committed
else if (mav->getSystemType() == MAV_TYPE_QUADROTOR)
{
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
}
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
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;
const qint64 maxLength = 1000;
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" << std::endl;
socket->readDatagram(data, maxLength, &sender, &senderPort);
// 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";
}
// Do not actually use the XPlane value, but calculate our own
Lorenz Meier
committed
Eigen::Vector3f g(0, 0, -9.81f);
Eigen::Matrix3f R = euler_to_wRo(yaw, pitch, roll);
Eigen::Vector3f gr = R.transpose().eval() * g;
// TODO Add centrip. accel
xacc = gr[0];
yacc = gr[1];
zacc = gr[2];
fields_changed |= (1 << 0) | (1 << 1) | (1 << 2);
else if (p.index == 6 && xPlaneVersion == 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)
Lorenz Meier
committed
{
//qDebug() << "LAT/LON/ALT:" << p.f[0] << p.f[1] << p.f[2];
Lorenz Meier
committed
lat = p.f[0];
lon = p.f[1];
Lorenz Meier
committed
alt = p.f[2] * 0.3048f; // convert feet (MSL) to meters
}
else if (p.index == 21 && xPlaneVersion == 10)
{
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) > 3)
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();
}
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() - simUpdateLast > 40) {
emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
pitchspeed, yawspeed, lat, lon, alt,
vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
simUpdateLast = QGC::groundTimeMilliseconds();
Lorenz Meier
committed
if (!oldConnectionState && xPlaneConnected)
{
emit statusMessage(tr("Receiving from XPlane."));
Lorenz Meier
committed
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
// // 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) return true;
connectState = false;
Lorenz Meier
committed
if (process) disconnect(process, SIGNAL(error(QProcess::ProcessError)),
Lorenz Meier
committed
this, SLOT(processError(QProcess::ProcessError)));
Lorenz Meier
committed
if (mav)
{
disconnect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
disconnect(mav, SIGNAL(hilActuatorsChanged(uint64_t, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(uint64_t,float,float,float,float,float,float,float,float)));
disconnect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int)));
disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)));
UAS* uas = dynamic_cast<UAS*>(mav);
if (uas)
{
uas->stopHil();
}
Lorenz Meier
committed
}
Lorenz Meier
committed
if (process)
{
process->close();
delete process;
process = NULL;
}
if (terraSync)
{
terraSync->close();
delete terraSync;
terraSync = NULL;
}
if (socket)
{
socket->close();
delete socket;
socket = NULL;
}
emit simulationDisconnected();
emit simulationConnected(false);
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;
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
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;
Anton Babushkin
committed
if (mav->getAltitudeAMSL() + offAlt < 0)
Lorenz Meier
committed
{
offAlt *= -1.0;
}
setPositionAttitude(mav->getLatitude() + offLat,
mav->getLongitude() + offLon,
Anton Babushkin
committed
mav->getAltitudeAMSL() + offAlt,
Lorenz Meier
committed
mav->getRoll(),
mav->getPitch(),
mav->getYaw());
}
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(mav->getLatitude(),
mav->getLongitude(),
Anton Babushkin
committed
mav->getAltitudeAMSL(),
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()
{
qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
Lorenz Meier
committed
if (!mav) return false;
if (connectState) return false;
Lorenz Meier
committed
socket = new QUdpSocket(this);
connectState = socket->bind(localHost, localPort);
Lorenz Meier
committed
if (!connectState) return false;
Lorenz Meier
committed
QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes()));
connect(mav, SIGNAL(hilControlsChanged(uint64_t, float, float, float, float, uint8_t, uint8_t)), this, SLOT(updateControls(uint64_t,float,float,float,float,uint8_t,uint8_t)));
connect(mav, SIGNAL(hilActuatorsChanged(uint64_t, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(uint64_t,float,float,float,float,float,float,float,float)));
connect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), mav, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)));
connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), mav, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int)));
connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), mav, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)));
Lorenz Meier
committed
UAS* uas = dynamic_cast<UAS*>(mav);
if (uas)
{
uas->startHil();
}
Lorenz Meier
committed
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#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;