zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Revert "fix code style" 

This reverts commit ae976049ca7b4818616e6b8be07a028fc7dfc26f.
QGC4.4
Bart Slinger 9 years ago
parent
0f035389f9
commit
ae1531705a
5 changed files with 2808 additions and 3057 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 136
      src/comm/QGCXPlaneLink.cc
  2. 13
      src/comm/QGCXPlaneLink.h
  3. 243
      src/uas/UAS.cc
  4. 42
      src/uas/UAS.h
  5. 5
      src/ui/QGCHilXPlaneConfiguration.cc

136
src/comm/QGCXPlaneLink.cc
Unescape View File

@ -73,8 +73,7 @@ QGCXPlaneLink::~QGCXPlaneLink()
// Tell the thread to exit // Tell the thread to exit
_should_exit = true; _should_exit = true;
if (socket) if (socket) {
{
socket->close(); socket->close();
socket->deleteLater(); socket->deleteLater();
socket = NULL; socket = NULL;
@ -110,22 +109,18 @@ void QGCXPlaneLink::storeSettings()
void QGCXPlaneLink::setVersion(const QString& version) void QGCXPlaneLink::setVersion(const QString& version)
{ {
unsigned int oldVersion = xPlaneVersion; unsigned int oldVersion = xPlaneVersion;
if (version.contains("9")) if (version.contains("9"))
{ {
xPlaneVersion = 9; xPlaneVersion = 9;
} }
else if (version.contains("10")) else if (version.contains("10"))
{ {
xPlaneVersion = 10; xPlaneVersion = 10;
} }
else if (version.contains("11")) else if (version.contains("11"))
{ {
xPlaneVersion = 11; xPlaneVersion = 11;
} }
else if (version.contains("12")) else if (version.contains("12"))
{ {
xPlaneVersion = 12; xPlaneVersion = 12;
@ -141,23 +136,19 @@ void QGCXPlaneLink::setVersion(unsigned int version)
{ {
bool changed = (xPlaneVersion != version); bool changed = (xPlaneVersion != version);
xPlaneVersion = version; xPlaneVersion = version;
if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion)); if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion));
} }
void QGCXPlaneLink::enableHilActuatorControls(bool enable) void QGCXPlaneLink::enableHilActuatorControls(bool enable)
{ {
if (enable != _useHilActuatorControls) if (enable != _useHilActuatorControls) {
{
_useHilActuatorControls = enable; _useHilActuatorControls = enable;
} }
/* Only use override for new message and specific airframes */ /* Only use override for new message and specific airframes */
MAV_TYPE type = _vehicle->vehicleType(); MAV_TYPE type = _vehicle->vehicleType();
float value = 0.0f; float value = 0.0f;
if (type == MAV_TYPE_VTOL_RESERVED2) {
if (type == MAV_TYPE_VTOL_RESERVED2)
{
value = (enable ? 1.0f : 0.0f); value = (enable ? 1.0f : 0.0f);
} }
@ -172,14 +163,12 @@ void QGCXPlaneLink::enableHilActuatorControls(bool enable)
**/ **/
void QGCXPlaneLink::run() void QGCXPlaneLink::run()
{ {
if (!_vehicle) if (!_vehicle) {
{
emit statusMessage("No MAV present"); emit statusMessage("No MAV present");
return; return;
} }
if (connectState) if (connectState) {
{
emit statusMessage("Already connected"); emit statusMessage("Already connected");
return; return;
} }
@ -187,9 +176,7 @@ void QGCXPlaneLink::run()
socket = new QUdpSocket(this); socket = new QUdpSocket(this);
socket->moveToThread(this); socket->moveToThread(this);
connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint); connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint);
if (!connectState) {
if (!connectState)
{
emit statusMessage("Binding socket failed!"); emit statusMessage("Binding socket failed!");
@ -257,8 +244,7 @@ void QGCXPlaneLink::run()
_should_exit = false; _should_exit = false;
while (!_should_exit) while(!_should_exit) {
{
QCoreApplication::processEvents(); QCoreApplication::processEvents();
QGC::SLEEP::msleep(5); QGC::SLEEP::msleep(5);
} }
@ -292,8 +278,7 @@ void QGCXPlaneLink::processError(QProcess::ProcessError err)
{ {
QString msg; QString msg;
switch (err) switch(err) {
{
case QProcess::FailedToStart: case QProcess::FailedToStart:
msg = tr("X-Plane Failed to start. Please check if the path and command is correct"); msg = tr("X-Plane Failed to start. Please check if the path and command is correct");
break; break;
@ -337,13 +322,11 @@ void QGCXPlaneLink::setRemoteHost(const QString &newHost)
if (newHost.contains(":")) if (newHost.contains(":"))
{ {
QHostInfo info = QHostInfo::fromName(newHost.split(":").first()); QHostInfo info = QHostInfo::fromName(newHost.split(":").first());
if (info.error() == QHostInfo::NoError) if (info.error() == QHostInfo::NoError)
{ {
// Add newHost // Add newHost
QList<QHostAddress> newHostAddresses = info.addresses(); QList<QHostAddress> newHostAddresses = info.addresses();
QHostAddress address; QHostAddress address;
for (int i = 0; i < newHostAddresses.size(); i++) for (int i = 0; i < newHostAddresses.size(); i++)
{ {
// Exclude loopback IPv4 and all IPv6 addresses // Exclude loopback IPv4 and all IPv6 addresses
@ -352,22 +335,18 @@ void QGCXPlaneLink::setRemoteHost(const QString &newHost)
address = newHostAddresses.at(i); address = newHostAddresses.at(i);
} }
} }
remoteHost = address; remoteHost = address;
// Set localPort according to user input // Set localPort according to user input
remotePort = newHost.split(":").last().toInt(); remotePort = newHost.split(":").last().toInt();
} }
} }
else else
{ {
QHostInfo info = QHostInfo::fromName(newHost); QHostInfo info = QHostInfo::fromName(newHost);
if (info.error() == QHostInfo::NoError) if (info.error() == QHostInfo::NoError)
{ {
// Add newHost // Add newHost
remoteHost = info.addresses().first(); remoteHost = info.addresses().first();
if (remotePort == 0) remotePort = 49000; if (remotePort == 0) remotePort = 49000;
} }
} }
@ -384,15 +363,12 @@ void QGCXPlaneLink::setRemoteHost(const QString &newHost)
void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode) 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 */ /* Only use HIL_CONTROL when the checkbox is unchecked */
if (_useHilActuatorControls) if (_useHilActuatorControls) {
{
//qDebug() << "received HIL_CONTROL but not using it"; //qDebug() << "received HIL_CONTROL but not using it";
return; return;
} }
#pragma pack(push, 1) #pragma pack(push, 1)
struct payload struct payload {
{
char b[5]; char b[5];
int index; int index;
float f[8]; float f[8];
@ -425,7 +401,6 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch
p.index = 25; p.index = 25;
writeBytesSafe((const char*)&p, sizeof(p)); writeBytesSafe((const char*)&p, sizeof(p));
} }
else else
{ {
// direct pass-through, normal fixed-wing. // direct pass-through, normal fixed-wing.
@ -456,8 +431,7 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch
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) 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) if (!_useHilActuatorControls) {
{
//qDebug() << "received HIL_ACTUATOR_CONTROLS but not using it"; //qDebug() << "received HIL_ACTUATOR_CONTROLS but not using it";
return; return;
} }
@ -471,8 +445,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
Q_UNUSED(ctl_15); Q_UNUSED(ctl_15);
#pragma pack(push, 1) #pragma pack(push, 1)
struct payload struct payload {
{
char b[5]; char b[5];
int index; int index;
float f[8]; float f[8];
@ -488,8 +461,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
/* Initialize with zeroes */ /* Initialize with zeroes */
memset(p.f, 0, sizeof(p.f)); memset(p.f, 0, sizeof(p.f));
switch (_vehicle->vehicleType()) switch (_vehicle->vehicleType()) {
{
case MAV_TYPE_QUADROTOR: case MAV_TYPE_QUADROTOR:
case MAV_TYPE_HEXAROTOR: case MAV_TYPE_HEXAROTOR:
case MAV_TYPE_OCTOROTOR: case MAV_TYPE_OCTOROTOR:
@ -508,7 +480,6 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
writeBytesSafe((const char*)&p, sizeof(p)); writeBytesSafe((const char*)&p, sizeof(p));
break; break;
} }
case MAV_TYPE_VTOL_RESERVED2: case MAV_TYPE_VTOL_RESERVED2:
{ {
/** /**
@ -536,7 +507,6 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
break; break;
} }
default: default:
{ {
/* direct pass-through, normal fixed-wing. */ /* direct pass-through, normal fixed-wing. */
@ -566,8 +536,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
} }
Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) {
{
double c__ = cos(yaw); double c__ = cos(yaw);
double _c_ = cos(pitch); double _c_ = cos(pitch);
double __c = cos(roll); double __c = cos(roll);
@ -622,13 +591,9 @@ void QGCXPlaneLink::readBytes()
quint16 senderPort; quint16 senderPort;
unsigned int s = socket->pendingDatagramSize(); 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; 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); socket->readDatagram(data, maxLength, &sender, &senderPort);
if (s > maxLength) {
if (s > maxLength)
{
std::string headStr = std::string(data, data+5); std::string headStr = std::string(data, data+5);
std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl; std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl;
} }
@ -640,8 +605,7 @@ void QGCXPlaneLink::readBytes()
//qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs; //qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
#pragma pack(push, 1) #pragma pack(push, 1)
struct payload struct payload {
{
int index; int index;
float f[8]; float f[8];
} p; } p;
@ -656,8 +620,7 @@ void QGCXPlaneLink::readBytes()
{ {
xPlaneConnected = true; xPlaneConnected = true;
if (oldConnectionState != xPlaneConnected) if (oldConnectionState != xPlaneConnected) {
{
simUpdateFirst = QGC::groundTimeMilliseconds(); simUpdateFirst = QGC::groundTimeMilliseconds();
} }
@ -675,7 +638,6 @@ void QGCXPlaneLink::readBytes()
//qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s"; //qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
} }
if (p.index == 4) 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 // WORKAROUND: IF ground speed <<1m/s and altitude-above-ground <1m, do NOT use the X-Plane data, because X-Plane (tested
@ -695,7 +657,6 @@ void QGCXPlaneLink::readBytes()
//qDebug() << "Calculated values" << gr[0] << gr[1] << gr[2]; //qDebug() << "Calculated values" << gr[0] << gr[1] << gr[2];
} }
else else
{ {
// Accelerometer readings, directly from X-Plane and including centripetal forces. // Accelerometer readings, directly from X-Plane and including centripetal forces.
@ -710,7 +671,6 @@ void QGCXPlaneLink::readBytes()
fields_changed |= (1 << 0) | (1 << 1) | (1 << 2); fields_changed |= (1 << 0) | (1 << 1) | (1 << 2);
emitUpdate = true; emitUpdate = true;
} }
// atmospheric pressure aircraft for XPlane 9 and 10 // atmospheric pressure aircraft for XPlane 9 and 10
else if (p.index == 6) else if (p.index == 6)
{ {
@ -719,7 +679,6 @@ void QGCXPlaneLink::readBytes()
temperature = p.f[1]; temperature = p.f[1];
fields_changed |= (1 << 9) | (1 << 12); fields_changed |= (1 << 9) | (1 << 12);
} }
// Forward controls from X-Plane to MAV, not very useful // Forward controls from X-Plane to MAV, not very useful
// better: Connect Joystick to QGroundControl // better: Connect Joystick to QGroundControl
// else if (p.index == 8) // else if (p.index == 8)
@ -741,7 +700,6 @@ void QGCXPlaneLink::readBytes()
emitUpdate = true; emitUpdate = true;
} }
else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18)) 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]; //qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
@ -750,25 +708,19 @@ void QGCXPlaneLink::readBytes()
yaw = p.f[2] / 180.0f * M_PI; yaw = p.f[2] / 180.0f * M_PI;
// X-Plane expresses yaw as 0..2 PI // X-Plane expresses yaw as 0..2 PI
if (yaw > M_PI) if (yaw > M_PI) {
{
yaw -= 2.0f * static_cast<float>(M_PI); yaw -= 2.0f * static_cast<float>(M_PI);
} }
if (yaw < -M_PI) {
if (yaw < -M_PI)
{
yaw += 2.0f * static_cast<float>(M_PI); yaw += 2.0f * static_cast<float>(M_PI);
} }
float yawmag = p.f[3] / 180.0f * M_PI; float yawmag = p.f[3] / 180.0f * M_PI;
if (yawmag > M_PI) if (yawmag > M_PI) {
{
yawmag -= 2.0f * static_cast<float>(M_PI); yawmag -= 2.0f * static_cast<float>(M_PI);
} }
if (yawmag < -M_PI) {
if (yawmag < -M_PI)
{
yawmag += 2.0f * static_cast<float>(M_PI); yawmag += 2.0f * static_cast<float>(M_PI);
} }
@ -833,7 +785,6 @@ void QGCXPlaneLink::readBytes()
alt = p.f[2] * 0.3048f; // convert feet (MSL) to meters alt = p.f[2] * 0.3048f; // convert feet (MSL) to meters
alt_agl = p.f[3] * 0.3048f; //convert feet (AGL) to meters alt_agl = p.f[3] * 0.3048f; //convert feet (AGL) to meters
} }
else if (p.index == 21) else if (p.index == 21)
{ {
vy = p.f[3]; vy = p.f[3];
@ -842,7 +793,6 @@ void QGCXPlaneLink::readBytes()
// for us. // for us.
vz = -p.f[4]; vz = -p.f[4];
} }
else if (p.index == 12) else if (p.index == 12)
{ {
//qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2]; //qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2];
@ -885,8 +835,7 @@ void QGCXPlaneLink::readBytes()
} }
// Wait for 0.5s before actually using the data, so that all fields are filled // Wait for 0.5s before actually using the data, so that all fields are filled
if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) {
{
return; return;
} }
@ -894,9 +843,7 @@ void QGCXPlaneLink::readBytes()
if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 2) if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 2)
{ {
simUpdateHz = simUpdateHz * 0.9f + 0.1f * (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); simUpdateHz = simUpdateHz * 0.9f + 0.1f * (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast));
if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000) {
if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000)
{
emit statusMessage(tr("Receiving from XPlane at %1 Hz").arg(static_cast<int>(simUpdateHz))); emit statusMessage(tr("Receiving from XPlane at %1 Hz").arg(static_cast<int>(simUpdateHz)));
// Reset lowpass with current value // Reset lowpass with current value
simUpdateHz = (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); simUpdateHz = (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast));
@ -948,18 +895,14 @@ void QGCXPlaneLink::readBytes()
int satellites = 8; int satellites = 8;
emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites); emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites);
} } else {
else
{
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
pitchspeed, yawspeed, lat, lon, alt, pitchspeed, yawspeed, lat, lon, alt,
vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc); vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
} }
// Limit ground truth to 25 Hz // Limit ground truth to 25 Hz
if (QGC::groundTimeMilliseconds() - simUpdateLastGroundTruth > 40) if (QGC::groundTimeMilliseconds() - simUpdateLastGroundTruth > 40) {
{
emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
pitchspeed, yawspeed, lat, lon, alt, pitchspeed, yawspeed, lat, lon, alt,
vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc); vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
@ -1005,10 +948,7 @@ bool QGCXPlaneLink::disconnectSimulation()
if (connectState) if (connectState)
{ {
_should_exit = true; _should_exit = true;
} } else {
else
{
emit simulationDisconnected(); emit simulationDisconnected();
emit simulationConnected(false); emit simulationConnected(false);
} }
@ -1027,27 +967,22 @@ void QGCXPlaneLink::selectAirframe(const QString &plane)
airframeID = AIRFRAME_QUAD_X_MK_10INCH_I2C; airframeID = AIRFRAME_QUAD_X_MK_10INCH_I2C;
emit airframeChanged("QRO_X / MK"); emit airframeChanged("QRO_X / MK");
} }
else if (plane.contains("ARDRONE") && airframeID != AIRFRAME_QUAD_X_ARDRONE) else if (plane.contains("ARDRONE") && airframeID != AIRFRAME_QUAD_X_ARDRONE)
{ {
airframeID = AIRFRAME_QUAD_X_ARDRONE; airframeID = AIRFRAME_QUAD_X_ARDRONE;
emit airframeChanged("QRO_X / ARDRONE"); emit airframeChanged("QRO_X / ARDRONE");
} }
else else
{ {
bool changed = (airframeID != AIRFRAME_QUAD_DJI_F450_PWM); bool changed = (airframeID != AIRFRAME_QUAD_DJI_F450_PWM);
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("QRO_X / DJI-F450 / PWM");
} }
} }
else else
{ {
bool changed = (airframeID != AIRFRAME_UNKNOWN); bool changed = (airframeID != AIRFRAME_UNKNOWN);
airframeID = AIRFRAME_UNKNOWN; airframeID = AIRFRAME_UNKNOWN;
if (changed) emit airframeChanged("X Plane default"); if (changed) emit airframeChanged("X Plane default");
} }
} }
@ -1152,13 +1087,9 @@ void QGCXPlaneLink::setRandomAttitude()
**/ **/
bool QGCXPlaneLink::connectSimulation() bool QGCXPlaneLink::connectSimulation()
{ {
if (connectState) if (connectState) {
{
qDebug() << "Simulation already active"; qDebug() << "Simulation already active";
} } else {
else
{
qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort; qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
// XXX Hack // XXX Hack
storeSettings(); storeSettings();
@ -1193,8 +1124,7 @@ void QGCXPlaneLink::setName(QString name)
void QGCXPlaneLink::sendDataRef(QString ref, float value) void QGCXPlaneLink::sendDataRef(QString ref, float value)
{ {
#pragma pack(push, 1) #pragma pack(push, 1)
struct payload struct payload {
{
char b[5]; char b[5];
float value; float value;
char name[500]; char name[500];
@ -1217,16 +1147,12 @@ void QGCXPlaneLink::sendDataRef(QString ref, float value)
/* Send command */ /* Send command */
QByteArray ba = ref.toUtf8(); QByteArray ba = ref.toUtf8();
if (ba.length() > 500) {
if (ba.length() > 500)
{
return; return;
} }
for (int i = 0; i < ba.length(); i++) for (int i = 0; i < ba.length(); i++) {
{
dref.name[i] = ba.at(i); dref.name[i] = ba.at(i);
} }
writeBytesSafe((const char*)&dref, sizeof(dref)); writeBytesSafe((const char*)&dref, sizeof(dref));
} }

13
src/comm/QGCXPlaneLink.h
Unescape View File

@ -51,8 +51,7 @@ public:
bool isConnected(); bool isConnected();
qint64 bytesAvailable(); qint64 bytesAvailable();
int getPort() const int getPort() const {
{
return localPort; return localPort;
} }
@ -89,13 +88,11 @@ public:
return (int)airframeID; return (int)airframeID;
} }
bool sensorHilEnabled() bool sensorHilEnabled() {
{
return _sensorHilEnabled; return _sensorHilEnabled;
} }
bool useHilActuatorControls() bool useHilActuatorControls() {
{
return _useHilActuatorControls; return _useHilActuatorControls;
} }
@ -134,11 +131,9 @@ public slots:
/** @brief Set the simulator version as integer */ /** @brief Set the simulator version as integer */
void setVersion(unsigned int version); void setVersion(unsigned int version);
void enableSensorHIL(bool enable) void enableSensorHIL(bool enable) {
{
if (enable != _sensorHilEnabled) if (enable != _sensorHilEnabled)
_sensorHilEnabled = enable; _sensorHilEnabled = enable;
emit sensorHilChanged(enable); emit sensorHilChanged(enable);
} }

243
src/uas/UAS.cc
Unescape View File

@ -201,19 +201,16 @@ void UAS::receiveMessage(mavlink_message_t message)
componentName = "ANONYMOUS"; componentName = "ANONYMOUS";
break; break;
} }
case MAV_COMP_ID_IMU: case MAV_COMP_ID_IMU:
{ {
componentName = "IMU #1"; componentName = "IMU #1";
break; break;
} }
case MAV_COMP_ID_CAMERA: case MAV_COMP_ID_CAMERA:
{ {
componentName = "CAMERA"; componentName = "CAMERA";
break; break;
} }
case MAV_COMP_ID_MISSIONPLANNER: case MAV_COMP_ID_MISSIONPLANNER:
{ {
componentName = "MISSIONPLANNER"; componentName = "MISSIONPLANNER";
@ -243,7 +240,6 @@ void UAS::receiveMessage(mavlink_message_t message)
// Prefer IMU 2 over IMU 1 (FIXME) // Prefer IMU 2 over IMU 1 (FIXME)
componentID[message.msgid] = MAV_COMP_ID_IMU_2; componentID[message.msgid] = MAV_COMP_ID_IMU_2;
break; break;
default: default:
// Do nothing // Do nothing
break; break;
@ -255,7 +251,6 @@ void UAS::receiveMessage(mavlink_message_t message)
// Prefer the first component // Prefer the first component
componentID[message.msgid] = message.compid; componentID[message.msgid] = message.compid;
} }
else else
{ {
// Got this message already // Got this message already
@ -277,7 +272,6 @@ void UAS::receiveMessage(mavlink_message_t message)
{ {
break; break;
} }
mavlink_heartbeat_t state; mavlink_heartbeat_t state;
mavlink_msg_heartbeat_decode(&message, &state); mavlink_msg_heartbeat_decode(&message, &state);
@ -309,7 +303,6 @@ void UAS::receiveMessage(mavlink_message_t message)
{ {
break; break;
} }
mavlink_sys_status_t state; mavlink_sys_status_t state;
mavlink_msg_sys_status_decode(&message, &state); mavlink_msg_sys_status_decode(&message, &state);
@ -343,12 +336,10 @@ void UAS::receiveMessage(mavlink_message_t message)
{ {
state.drop_rate_comm = 10000; state.drop_rate_comm = 10000;
} }
emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f); emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f);
emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time); emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time);
} }
break; break;
case MAVLINK_MSG_ID_ATTITUDE: case MAVLINK_MSG_ID_ATTITUDE:
{ {
mavlink_attitude_t attitude; mavlink_attitude_t attitude;
@ -370,7 +361,6 @@ void UAS::receiveMessage(mavlink_message_t message)
} }
} }
break; break;
case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
{ {
mavlink_attitude_quaternion_t attitude; mavlink_attitude_quaternion_t attitude;
@ -400,24 +390,17 @@ void UAS::receiveMessage(mavlink_message_t message)
float phi, theta, psi; float phi, theta, psi;
theta = asin(-dcm[2][0]); theta = asin(-dcm[2][0]);
if (fabs(theta - M_PI_2) < 1.0e-3f) if (fabs(theta - M_PI_2) < 1.0e-3f) {
{
phi = 0.0f; phi = 0.0f;
psi = (atan2(dcm[1][2] - dcm[0][1], psi = (atan2(dcm[1][2] - dcm[0][1],
dcm[0][2] + dcm[1][1]) + phi); dcm[0][2] + dcm[1][1]) + phi);
} } else if (fabs(theta + M_PI_2) < 1.0e-3f) {
else if (fabs(theta + M_PI_2) < 1.0e-3f)
{
phi = 0.0f; phi = 0.0f;
psi = atan2f(dcm[1][2] - dcm[0][1], psi = atan2f(dcm[1][2] - dcm[0][1],
dcm[0][2] + dcm[1][1] - phi); dcm[0][2] + dcm[1][1] - phi);
} } else {
else
{
phi = atan2f(dcm[2][1], dcm[2][2]); phi = atan2f(dcm[2][1], dcm[2][2]);
psi = atan2f(dcm[1][0], dcm[0][0]); psi = atan2f(dcm[1][0], dcm[0][0]);
} }
@ -439,7 +422,6 @@ void UAS::receiveMessage(mavlink_message_t message)
} }
} }
break; break;
case MAVLINK_MSG_ID_HIL_CONTROLS: case MAVLINK_MSG_ID_HIL_CONTROLS:
{ {
mavlink_hil_controls_t hil; mavlink_hil_controls_t hil;
@ -447,7 +429,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode); emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode);
} }
break; break;
case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS: case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
{ {
mavlink_hil_actuator_controls_t hil; mavlink_hil_actuator_controls_t hil;
@ -472,7 +453,6 @@ void UAS::receiveMessage(mavlink_message_t message)
hil.mode); hil.mode);
} }
break; break;
case MAVLINK_MSG_ID_VFR_HUD: case MAVLINK_MSG_ID_VFR_HUD:
{ {
mavlink_vfr_hud_t hud; mavlink_vfr_hud_t hud;
@ -489,16 +469,13 @@ void UAS::receiveMessage(mavlink_message_t message)
setAltitudeAMSL(hud.alt); setAltitudeAMSL(hud.alt);
setGroundSpeed(hud.groundspeed); setGroundSpeed(hud.groundspeed);
if (!qIsNaN(hud.airspeed)) if (!qIsNaN(hud.airspeed))
setAirSpeed(hud.airspeed); setAirSpeed(hud.airspeed);
speedZ = -hud.climb; speedZ = -hud.climb;
emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time);
emit speedChanged(this, groundSpeed, airSpeed, time); emit speedChanged(this, groundSpeed, airSpeed, time);
} }
break; break;
case MAVLINK_MSG_ID_LOCAL_POSITION_NED: case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
//std::cerr << std::endl; //std::cerr << std::endl;
//std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
@ -518,7 +495,6 @@ void UAS::receiveMessage(mavlink_message_t message)
} }
} }
break; break;
case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE: case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
{ {
mavlink_global_vision_position_estimate_t pos; mavlink_global_vision_position_estimate_t pos;
@ -527,7 +503,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time); emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time);
} }
break; break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
//std::cerr << std::endl; //std::cerr << std::endl;
//std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
@ -558,7 +533,6 @@ void UAS::receiveMessage(mavlink_message_t message)
isGlobalPositionKnown = true; isGlobalPositionKnown = true;
} }
break; break;
case MAVLINK_MSG_ID_GPS_RAW_INT: case MAVLINK_MSG_ID_GPS_RAW_INT:
{ {
mavlink_gps_raw_int_t pos; mavlink_gps_raw_int_t pos;
@ -568,12 +542,10 @@ void UAS::receiveMessage(mavlink_message_t message)
// TODO: track localization state not only for gps but also for other loc. sources // TODO: track localization state not only for gps but also for other loc. sources
int loc_type = pos.fix_type; int loc_type = pos.fix_type;
if (loc_type == 1) if (loc_type == 1)
{ {
loc_type = 0; loc_type = 0;
} }
setSatelliteCount(pos.satellites_visible); setSatelliteCount(pos.satellites_visible);
if (pos.fix_type > 2) if (pos.fix_type > 2)
@ -585,24 +557,18 @@ void UAS::receiveMessage(mavlink_message_t message)
altitude_gps = pos.alt/1000.0; altitude_gps = pos.alt/1000.0;
// If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead. // If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead.
if (!globalEstimatorActive) if (!globalEstimatorActive) {
{
setLatitude(latitude_gps); setLatitude(latitude_gps);
setLongitude(longitude_gps); setLongitude(longitude_gps);
emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time); emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time);
emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time);
float vel = pos.vel/100.0f; float vel = pos.vel/100.0f;
// Smaller than threshold and not NaN // Smaller than threshold and not NaN
if ((vel < 1000000) && !qIsNaN(vel) && !qIsInf(vel)) if ((vel < 1000000) && !qIsNaN(vel) && !qIsInf(vel)) {
{
setGroundSpeed(vel); setGroundSpeed(vel);
emit speedChanged(this, groundSpeed, airSpeed, time); emit speedChanged(this, groundSpeed, airSpeed, time);
} } else {
else
{
emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_NOTICE, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel)); emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_NOTICE, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel));
} }
} }
@ -611,23 +577,18 @@ void UAS::receiveMessage(mavlink_message_t message)
double dtmp; double dtmp;
//-- Raw GPS data //-- Raw GPS data
dtmp = pos.eph == 0xFFFF ? 1e10f : pos.eph / 100.0; dtmp = pos.eph == 0xFFFF ? 1e10f : pos.eph / 100.0;
if(dtmp != satRawHDOP) if(dtmp != satRawHDOP)
{ {
satRawHDOP = dtmp; satRawHDOP = dtmp;
emit satRawHDOPChanged(satRawHDOP); emit satRawHDOPChanged(satRawHDOP);
} }
dtmp = pos.epv == 0xFFFF ? 1e10f : pos.epv / 100.0; dtmp = pos.epv == 0xFFFF ? 1e10f : pos.epv / 100.0;
if(dtmp != satRawVDOP) if(dtmp != satRawVDOP)
{ {
satRawVDOP = dtmp; satRawVDOP = dtmp;
emit satRawVDOPChanged(satRawVDOP); emit satRawVDOPChanged(satRawVDOP);
} }
dtmp = pos.cog == 0xFFFF ? 0.0 : pos.cog / 100.0; dtmp = pos.cog == 0xFFFF ? 0.0 : pos.cog / 100.0;
if(dtmp != satRawCOG) if(dtmp != satRawCOG)
{ {
satRawCOG = dtmp; satRawCOG = dtmp;
@ -639,17 +600,14 @@ void UAS::receiveMessage(mavlink_message_t message)
emit localizationChanged(this, loc_type); emit localizationChanged(this, loc_type);
} }
break; break;
case MAVLINK_MSG_ID_GPS_STATUS: case MAVLINK_MSG_ID_GPS_STATUS:
{ {
mavlink_gps_status_t pos; mavlink_gps_status_t pos;
mavlink_msg_gps_status_decode(&message, &pos); mavlink_msg_gps_status_decode(&message, &pos);
for(int i = 0; i < (int)pos.satellites_visible; i++) for(int i = 0; i < (int)pos.satellites_visible; i++)
{ {
emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i])); emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i]));
} }
setSatelliteCount(pos.satellites_visible); setSatelliteCount(pos.satellites_visible);
} }
break; break;
@ -669,7 +627,6 @@ void UAS::receiveMessage(mavlink_message_t message)
processParamValueMsg(message, parameterName,rawValue,paramVal); processParamValueMsg(message, parameterName,rawValue,paramVal);
} }
break; break;
case MAVLINK_MSG_ID_ATTITUDE_TARGET: case MAVLINK_MSG_ID_ATTITUDE_TARGET:
{ {
mavlink_attitude_target_t out; mavlink_attitude_target_t out;
@ -692,14 +649,12 @@ void UAS::receiveMessage(mavlink_message_t message)
{ {
break; break;
} }
mavlink_position_target_local_ned_t p; mavlink_position_target_local_ned_t p;
mavlink_msg_position_target_local_ned_decode(&message, &p); mavlink_msg_position_target_local_ned_decode(&message, &p);
quint64 time = getUnixTimeFromMs(p.time_boot_ms); quint64 time = getUnixTimeFromMs(p.time_boot_ms);
emit positionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */, time); emit positionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */, time);
} }
break; break;
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
{ {
mavlink_set_position_target_local_ned_t p; mavlink_set_position_target_local_ned_t p;
@ -707,7 +662,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */); emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */);
} }
break; break;
case MAVLINK_MSG_ID_STATUSTEXT: case MAVLINK_MSG_ID_STATUSTEXT:
{ {
QByteArray b; QByteArray b;
@ -727,7 +681,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit textMessageReceived(uasId, message.compid, severity, text); emit textMessageReceived(uasId, message.compid, severity, text);
_say(text.toLower(), severity); _say(text.toLower(), severity);
} }
else else
{ {
emit textMessageReceived(uasId, message.compid, severity, text); emit textMessageReceived(uasId, message.compid, severity, text);
@ -770,11 +723,9 @@ void UAS::receiveMessage(mavlink_message_t message)
for (int i = 0; i < imagePayload; ++i) for (int i = 0; i < imagePayload; ++i)
{ {
if (pos <= imageSize) if (pos <= imageSize) {
{
imageRecBuffer[pos] = img.data[i]; imageRecBuffer[pos] = img.data[i];
} }
++pos; ++pos;
} }
@ -826,8 +777,7 @@ void UAS::receiveMessage(mavlink_message_t message)
void UAS::startCalibration(UASInterface::StartCalibrationType calType) void UAS::startCalibration(UASInterface::StartCalibrationType calType)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
@ -838,44 +788,34 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
int accelCal = 0; int accelCal = 0;
int escCal = 0; int escCal = 0;
switch (calType) switch (calType) {
{
case StartCalibrationGyro: case StartCalibrationGyro:
gyroCal = 1; gyroCal = 1;
break; break;
case StartCalibrationMag: case StartCalibrationMag:
magCal = 1; magCal = 1;
break; break;
case StartCalibrationAirspeed: case StartCalibrationAirspeed:
airspeedCal = 1; airspeedCal = 1;
break; break;
case StartCalibrationRadio: case StartCalibrationRadio:
radioCal = 1; radioCal = 1;
break; break;
case StartCalibrationCopyTrims: case StartCalibrationCopyTrims:
radioCal = 2; radioCal = 2;
break; break;
case StartCalibrationAccel: case StartCalibrationAccel:
accelCal = 1; accelCal = 1;
break; break;
case StartCalibrationLevel: case StartCalibrationLevel:
accelCal = 2; accelCal = 2;
break; break;
case StartCalibrationEsc: case StartCalibrationEsc:
escCal = 1; escCal = 1;
break; break;
case StartCalibrationUavcanEsc: case StartCalibrationUavcanEsc:
escCal = 2; escCal = 2;
break; break;
case StartCalibrationCompassMot: case StartCalibrationCompassMot:
airspeedCal = 1; // ArduPilot, bit of a hack airspeedCal = 1; // ArduPilot, bit of a hack
break; break;
@ -901,8 +841,7 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
void UAS::stopCalibration(void) void UAS::stopCalibration(void)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
@ -926,19 +865,16 @@ void UAS::stopCalibration(void)
void UAS::startBusConfig(UASInterface::StartBusConfigType calType) void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
int actuatorCal = 0; int actuatorCal = 0;
switch (calType) switch (calType) {
{
case StartBusConfigActuators: case StartBusConfigActuators:
actuatorCal = 1; actuatorCal = 1;
break; break;
case EndBusConfigActuators: case EndBusConfigActuators:
actuatorCal = 0; actuatorCal = 0;
break; break;
@ -964,8 +900,7 @@ void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
void UAS::stopBusConfig(void) void UAS::stopBusConfig(void)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
@ -1001,7 +936,6 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
// qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds(); // qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();
return QGC::groundTimeMilliseconds(); return QGC::groundTimeMilliseconds();
} }
// Check if time is smaller than 40 years, // Check if time is smaller than 40 years,
// assuming no system without Unix timestamp // assuming no system without Unix timestamp
// runs longer than 40 years continuously without // runs longer than 40 years continuously without
@ -1019,7 +953,6 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
// 1000 milliseconds // 1000 milliseconds
// 1000 microseconds // 1000 microseconds
#ifndef _MSC_VER #ifndef _MSC_VER
else if (time < 1261440000000000LLU) else if (time < 1261440000000000LLU)
#else #else
else if (time < 1261440000000000) else if (time < 1261440000000000)
@ -1030,10 +963,8 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
{ {
onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000; onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
} }
return time/1000 + onboardTimeOffset; return time/1000 + onboardTimeOffset;
} }
else else
{ {
// Time is not zero and larger than 40 years -> has to be // Time is not zero and larger than 40 years -> has to be
@ -1068,7 +999,6 @@ quint64 UAS::getUnixTimeFromMs(quint64 time)
quint64 UAS::getUnixTime(quint64 time) quint64 UAS::getUnixTime(quint64 time)
{ {
quint64 ret = 0; quint64 ret = 0;
if (attitudeStamped) if (attitudeStamped)
{ {
ret = lastAttitude; ret = lastAttitude;
@ -1078,7 +1008,6 @@ quint64 UAS::getUnixTime(quint64 time)
{ {
ret = QGC::groundTimeMilliseconds(); ret = QGC::groundTimeMilliseconds();
} }
// Check if time is smaller than 40 years, // Check if time is smaller than 40 years,
// assuming no system without Unix timestamp // assuming no system without Unix timestamp
// runs longer than 40 years continuously without // runs longer than 40 years continuously without
@ -1096,7 +1025,6 @@ quint64 UAS::getUnixTime(quint64 time)
// 1000 milliseconds // 1000 milliseconds
// 1000 microseconds // 1000 microseconds
#ifndef _MSC_VER #ifndef _MSC_VER
else if (time < 1261440000000000LLU) else if (time < 1261440000000000LLU)
#else #else
else if (time < 1261440000000000) else if (time < 1261440000000000)
@ -1108,12 +1036,10 @@ quint64 UAS::getUnixTime(quint64 time)
lastNonNullTime = time; lastNonNullTime = time;
onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000; onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
} }
if (time > lastNonNullTime) lastNonNullTime = time; if (time > lastNonNullTime) lastNonNullTime = time;
ret = time/1000 + onboardTimeOffset; ret = time/1000 + onboardTimeOffset;
} }
else else
{ {
// Time is not zero and larger than 40 years -> has to be // Time is not zero and larger than 40 years -> has to be
@ -1137,37 +1063,30 @@ void UAS::getStatusForCode(int statusCode, QString &uasState, QString &stateDesc
uasState = tr("UNINIT"); uasState = tr("UNINIT");
stateDescription = tr("Unitialized, booting up."); stateDescription = tr("Unitialized, booting up.");
break; break;
case MAV_STATE_BOOT: case MAV_STATE_BOOT:
uasState = tr("BOOT"); uasState = tr("BOOT");
stateDescription = tr("Booting system, please wait."); stateDescription = tr("Booting system, please wait.");
break; break;
case MAV_STATE_CALIBRATING: case MAV_STATE_CALIBRATING:
uasState = tr("CALIBRATING"); uasState = tr("CALIBRATING");
stateDescription = tr("Calibrating sensors, please wait."); stateDescription = tr("Calibrating sensors, please wait.");
break; break;
case MAV_STATE_ACTIVE: case MAV_STATE_ACTIVE:
uasState = tr("ACTIVE"); uasState = tr("ACTIVE");
stateDescription = tr("Active, normal operation."); stateDescription = tr("Active, normal operation.");
break; break;
case MAV_STATE_STANDBY: case MAV_STATE_STANDBY:
uasState = tr("STANDBY"); uasState = tr("STANDBY");
stateDescription = tr("Standby mode, ready for launch."); stateDescription = tr("Standby mode, ready for launch.");
break; break;
case MAV_STATE_CRITICAL: case MAV_STATE_CRITICAL:
uasState = tr("CRITICAL"); uasState = tr("CRITICAL");
stateDescription = tr("FAILURE: Continuing operation."); stateDescription = tr("FAILURE: Continuing operation.");
break; break;
case MAV_STATE_EMERGENCY: case MAV_STATE_EMERGENCY:
uasState = tr("EMERGENCY"); uasState = tr("EMERGENCY");
stateDescription = tr("EMERGENCY: Land Immediately!"); stateDescription = tr("EMERGENCY: Land Immediately!");
break; break;
//case MAV_STATE_HILSIM: //case MAV_STATE_HILSIM:
//uasState = tr("HIL SIM"); //uasState = tr("HIL SIM");
//stateDescription = tr("HIL Simulation, Sensors read from SIM"); //stateDescription = tr("HIL Simulation, Sensors read from SIM");
@ -1217,7 +1136,6 @@ QImage UAS::getImage()
} }
} }
// BMP with header // BMP with header
else if (imageType == MAVLINK_DATA_STREAM_IMG_BMP || else if (imageType == MAVLINK_DATA_STREAM_IMG_BMP ||
imageType == MAVLINK_DATA_STREAM_IMG_JPEG || imageType == MAVLINK_DATA_STREAM_IMG_JPEG ||
@ -1240,8 +1158,7 @@ QImage UAS::getImage()
void UAS::requestImage() void UAS::requestImage()
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
@ -1270,7 +1187,6 @@ quint64 UAS::getUptime() const
{ {
return 0; return 0;
} }
else else
{ {
return QGC::groundTimeMilliseconds() - startTime; return QGC::groundTimeMilliseconds() - startTime;
@ -1286,8 +1202,7 @@ void UAS::processParamValueMsg(mavlink_message_t &msg, const QString &paramName,
// Insert with correct type // Insert with correct type
switch (rawValue.param_type) switch (rawValue.param_type) {
{
case MAV_PARAM_TYPE_REAL32: case MAV_PARAM_TYPE_REAL32:
paramValue = QVariant(paramUnion.param_float); paramValue = QVariant(paramUnion.param_float);
break; break;
@ -1335,8 +1250,7 @@ void UAS::processParamValueMsg(mavlink_message_t &msg, const QString &paramName,
void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
@ -1363,8 +1277,7 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float
*/ */
void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode) void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
@ -1382,17 +1295,12 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
// The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz. // The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz.
bool sendCommand = false; bool sendCommand = false;
if (countSinceLastTransmission++ >= 5) {
if (countSinceLastTransmission++ >= 5)
{
sendCommand = true; sendCommand = true;
countSinceLastTransmission = 0; countSinceLastTransmission = 0;
} } else if ((!qIsNaN(roll) && roll != manualRollAngle) || (!qIsNaN(pitch) && pitch != manualPitchAngle) ||
else if ((!qIsNaN(roll) && roll != manualRollAngle) || (!qIsNaN(pitch) && pitch != manualPitchAngle) ||
(!qIsNaN(yaw) && yaw != manualYawAngle) || (!qIsNaN(thrust) && thrust != manualThrust) || (!qIsNaN(yaw) && yaw != manualYawAngle) || (!qIsNaN(thrust) && thrust != manualThrust) ||
buttons != manualButtons) buttons != manualButtons) {
{
sendCommand = true; sendCommand = true;
// Ensure that another message will be sent the next time this function is called // Ensure that another message will be sent the next time this function is called
@ -1400,8 +1308,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
} }
// Now if we should trigger an update, let's do that // Now if we should trigger an update, let's do that
if (sendCommand) if (sendCommand) {
{
// Save the new manual control inputs // Save the new manual control inputs
manualRollAngle = roll; manualRollAngle = roll;
manualPitchAngle = pitch; manualPitchAngle = pitch;
@ -1411,8 +1318,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
mavlink_message_t message; mavlink_message_t message;
if (joystickMode == Vehicle::JoystickModeAttitude) if (joystickMode == Vehicle::JoystickModeAttitude) {
{
// send an external attitude setpoint command (rate control disabled) // send an external attitude setpoint command (rate control disabled)
float attitudeQuaternion[4]; float attitudeQuaternion[4];
mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion); mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion);
@ -1430,10 +1336,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
0, 0,
thrust thrust
); );
} } else if (joystickMode == Vehicle::JoystickModePosition) {
else if (joystickMode == Vehicle::JoystickModePosition)
{
// Send the the local position setpoint (local pos sp external message) // Send the the local position setpoint (local pos sp external message)
static float px = 0; static float px = 0;
static float py = 0; static float py = 0;
@ -1463,10 +1366,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
yaw, yaw,
0 0
); );
} } else if (joystickMode == Vehicle::JoystickModeForce) {
else if (joystickMode == Vehicle::JoystickModeForce)
{
// Send the the force setpoint (local pos sp external message) // Send the the force setpoint (local pos sp external message)
float dcm[3][3]; float dcm[3][3];
mavlink_euler_to_dcm(roll, pitch, yaw, dcm); mavlink_euler_to_dcm(roll, pitch, yaw, dcm);
@ -1494,10 +1394,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
0, 0,
0 0
); );
} } else if (joystickMode == Vehicle::JoystickModeVelocity) {
else if (joystickMode == Vehicle::JoystickModeVelocity)
{
// Send the the local velocity setpoint (local pos sp external message) // Send the the local velocity setpoint (local pos sp external message)
static float vx = 0; static float vx = 0;
static float vy = 0; static float vy = 0;
@ -1529,10 +1426,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
0, 0,
yawrate yawrate
); );
} } else if (joystickMode == Vehicle::JoystickModeRC) {
else if (joystickMode == Vehicle::JoystickModeRC)
{
// Save the new manual control inputs // Save the new manual control inputs
manualRollAngle = roll; manualRollAngle = roll;
@ -1566,11 +1460,9 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
#ifndef __mobile__ #ifndef __mobile__
void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw) void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
const uint8_t base_mode = _vehicle->baseMode(); const uint8_t base_mode = _vehicle->baseMode();
// If system has manual inputs enabled and is armed // If system has manual inputs enabled and is armed
@ -1599,7 +1491,6 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
//emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); //emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
} }
else else
{ {
qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first"; qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first";
@ -1612,8 +1503,7 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
*/ */
void UAS::pairRX(int rxType, int rxSubType) void UAS::pairRX(int rxType, int rxSubType)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
@ -1632,16 +1522,12 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
Q_UNUSED(configuration); Q_UNUSED(configuration);
QGCFlightGearLink* link = dynamic_cast<QGCFlightGearLink*>(simulation); QGCFlightGearLink* link = dynamic_cast<QGCFlightGearLink*>(simulation);
if (!link) {
if (!link)
{
// Delete wrong sim // Delete wrong sim
if (simulation) if (simulation) {
{
stopHil(); stopHil();
delete simulation; delete simulation;
} }
simulation = new QGCFlightGearLink(_vehicle, options); simulation = new QGCFlightGearLink(_vehicle, options);
} }
@ -1664,14 +1550,12 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
link = dynamic_cast<QGCFlightGearLink*>(simulation); link = dynamic_cast<QGCFlightGearLink*>(simulation);
link->setStartupArguments(options); link->setStartupArguments(options);
link->sensorHilEnabled(sensorHil); link->sensorHilEnabled(sensorHil);
// FIXME: this signal is not on the base hil configuration widget, only on the FG widget // FIXME: this signal is not on the base hil configuration widget, only on the FG widget
//QObject::connect(configuration, SIGNAL(barometerOffsetChanged(float)), link, SLOT(setBarometerOffset(float))); //QObject::connect(configuration, SIGNAL(barometerOffsetChanged(float)), link, SLOT(setBarometerOffset(float)));
if (enable) if (enable)
{ {
startHil(); startHil();
} }
else else
{ {
stopHil(); stopHil();
@ -1686,28 +1570,21 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
void UAS::enableHilJSBSim(bool enable, QString options) void UAS::enableHilJSBSim(bool enable, QString options)
{ {
QGCJSBSimLink* link = dynamic_cast<QGCJSBSimLink*>(simulation); QGCJSBSimLink* link = dynamic_cast<QGCJSBSimLink*>(simulation);
if (!link) {
if (!link)
{
// Delete wrong sim // Delete wrong sim
if (simulation) if (simulation) {
{
stopHil(); stopHil();
delete simulation; delete simulation;
} }
simulation = new QGCJSBSimLink(_vehicle, options); simulation = new QGCJSBSimLink(_vehicle, options);
} }
// Connect Flight Gear Link // Connect Flight Gear Link
link = dynamic_cast<QGCJSBSimLink*>(simulation); link = dynamic_cast<QGCJSBSimLink*>(simulation);
link->setStartupArguments(options); link->setStartupArguments(options);
if (enable) if (enable)
{ {
startHil(); startHil();
} }
else else
{ {
stopHil(); stopHil();
@ -1722,15 +1599,11 @@ void UAS::enableHilJSBSim(bool enable, QString options)
void UAS::enableHilXPlane(bool enable) void UAS::enableHilXPlane(bool enable)
{ {
QGCXPlaneLink* link = dynamic_cast<QGCXPlaneLink*>(simulation); QGCXPlaneLink* link = dynamic_cast<QGCXPlaneLink*>(simulation);
if (!link) {
if (!link) if (simulation) {
{
if (simulation)
{
stopHil(); stopHil();
delete simulation; delete simulation;
} }
simulation = new QGCXPlaneLink(_vehicle); simulation = new QGCXPlaneLink(_vehicle);
float noise_scaler = 0.0001f; float noise_scaler = 0.0001f;
@ -1748,13 +1621,11 @@ void UAS::enableHilXPlane(bool enable)
pressure_alt_var = noise_scaler * 0.5604f; pressure_alt_var = noise_scaler * 0.5604f;
temperature_var = noise_scaler * 0.7290f; temperature_var = noise_scaler * 0.7290f;
} }
// Connect X-Plane Link // Connect X-Plane Link
if (enable) if (enable)
{ {
startHil(); startHil();
} }
else else
{ {
stopHil(); stopHil();
@ -1835,8 +1706,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
float pitchspeed, float yawspeed, double lat, double lon, double alt, 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) float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
@ -1865,7 +1735,6 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
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); 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->sendMessageOnPriorityLink(msg); _vehicle->sendMessageOnPriorityLink(msg);
} }
else else
{ {
// Attempt to set HIL mode // Attempt to set HIL mode
@ -1901,13 +1770,9 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
float noise = z0 * sqrt(noise_var); //calculate normally distributed variable with mu = 0, std = var^2 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 //Finally guard against any case where the noise is not real
if (std::isfinite(noise)) if(std::isfinite(noise)) {
{
return truth_meas + noise; return truth_meas + noise;
} } else {
else
{
return truth_meas; return truth_meas;
} }
} }
@ -1921,8 +1786,7 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, 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) float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
@ -1950,7 +1814,6 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
_vehicle->sendMessageOnPriorityLink(msg); _vehicle->sendMessageOnPriorityLink(msg);
lastSendTimeSensors = QGC::groundTimeMilliseconds(); lastSendTimeSensors = QGC::groundTimeMilliseconds();
} }
else else
{ {
// Attempt to set HIL mode // Attempt to set HIL mode
@ -1964,8 +1827,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, 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) float flow_comp_m_y, quint8 quality, float ground_distance)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
@ -1990,7 +1852,6 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds(); lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds();
#endif #endif
} }
else else
{ {
// Attempt to set HIL mode // Attempt to set HIL mode
@ -2004,8 +1865,7 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
#ifndef __mobile__ #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) 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) if (!_vehicle) {
{
return; return;
} }
@ -2016,11 +1876,9 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
if (_vehicle->hilMode()) if (_vehicle->hilMode())
{ {
float course = cog; float course = cog;
// map to 0..2pi // map to 0..2pi
if (course < 0) if (course < 0)
course += 2.0f * static_cast<float>(M_PI); course += 2.0f * static_cast<float>(M_PI);
// scale from radians to degrees // scale from radians to degrees
course = (course / M_PI) * 180.0f; course = (course / M_PI) * 180.0f;
@ -2030,7 +1888,6 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
lastSendTimeGPS = QGC::groundTimeMilliseconds(); lastSendTimeGPS = QGC::groundTimeMilliseconds();
_vehicle->sendMessageOnPriorityLink(msg); _vehicle->sendMessageOnPriorityLink(msg);
} }
else else
{ {
// Attempt to set HIL mode // Attempt to set HIL mode
@ -2047,7 +1904,6 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
void UAS::startHil() void UAS::startHil()
{ {
if (hilEnabled) return; if (hilEnabled) return;
hilEnabled = true; hilEnabled = true;
sensorHil = false; sensorHil = false;
_vehicle->setHilMode(true); _vehicle->setHilMode(true);
@ -2063,13 +1919,11 @@ void UAS::startHil()
#ifndef __mobile__ #ifndef __mobile__
void UAS::stopHil() void UAS::stopHil()
{ {
if (simulation && simulation->isConnected()) if (simulation && simulation->isConnected()) {
{
simulation->disconnectSimulation(); simulation->disconnectSimulation();
_vehicle->setHilMode(false); _vehicle->setHilMode(false);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to disable."; qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to disable.";
} }
hilEnabled = false; hilEnabled = false;
sensorHil = false; sensorHil = false;
} }
@ -2085,15 +1939,13 @@ QMap<int, QString> UAS::getComponents()
void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax) void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
mavlink_message_t message; mavlink_message_t message;
char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {}; char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};
// Copy string into buffer, ensuring not to exceed the buffer size // Copy string into buffer, ensuring not to exceed the buffer size
for (unsigned int i = 0; i < sizeof(param_id_cstr); i++) for (unsigned int i = 0; i < sizeof(param_id_cstr); i++)
{ {
@ -2121,15 +1973,13 @@ void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 p
void UAS::unsetRCToParameterMap() void UAS::unsetRCToParameterMap()
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {}; char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};
for (int i = 0; i < 3; i++) for (int i = 0; i < 3; i++) {
{
mavlink_message_t message; mavlink_message_t message;
mavlink_msg_param_map_rc_pack(mavlink->getSystemId(), mavlink_msg_param_map_rc_pack(mavlink->getSystemId(),
mavlink->getComponentId(), mavlink->getComponentId(),
@ -2157,15 +2007,12 @@ void UAS::shutdownVehicle(void)
{ {
#ifndef __mobile__ #ifndef __mobile__
stopHil(); stopHil();
if (simulation) {
if (simulation)
{
// wait for the simulator to exit // wait for the simulator to exit
simulation->wait(); simulation->wait();
simulation->disconnectSimulation(); simulation->disconnectSimulation();
simulation->deleteLater(); simulation->deleteLater();
} }
#endif #endif
_vehicle = NULL; _vehicle = NULL;
} }

42
src/uas/UAS.h
Unescape View File

@ -290,68 +290,55 @@ public:
} }
// Setters for HIL noise variance // Setters for HIL noise variance
void setXaccVar(float var) void setXaccVar(float var){
{
xacc_var = var; xacc_var = var;
} }
void setYaccVar(float var) void setYaccVar(float var){
{
yacc_var = var; yacc_var = var;
} }
void setZaccVar(float var) void setZaccVar(float var){
{
zacc_var = var; zacc_var = var;
} }
void setRollSpeedVar(float var) void setRollSpeedVar(float var){
{
rollspeed_var = var; rollspeed_var = var;
} }
void setPitchSpeedVar(float var) void setPitchSpeedVar(float var){
{
pitchspeed_var = var; pitchspeed_var = var;
} }
void setYawSpeedVar(float var) void setYawSpeedVar(float var){
{
pitchspeed_var = var; pitchspeed_var = var;
} }
void setXmagVar(float var) void setXmagVar(float var){
{
xmag_var = var; xmag_var = var;
} }
void setYmagVar(float var) void setYmagVar(float var){
{
ymag_var = var; ymag_var = var;
} }
void setZmagVar(float var) void setZmagVar(float var){
{
zmag_var = var; zmag_var = var;
} }
void setAbsPressureVar(float var) void setAbsPressureVar(float var){
{
abs_pressure_var = var; abs_pressure_var = var;
} }
void setDiffPressureVar(float var) void setDiffPressureVar(float var){
{
diff_pressure_var = var; diff_pressure_var = var;
} }
void setPressureAltVar(float var) void setPressureAltVar(float var){
{
pressure_alt_var = var; pressure_alt_var = var;
} }
void setTemperatureVar(float var) void setTemperatureVar(float var){
{
temperature_var = var; temperature_var = var;
} }
@ -476,8 +463,7 @@ public:
/** @brief Get the HIL simulation */ /** @brief Get the HIL simulation */
#ifndef __mobile__ #ifndef __mobile__
QGCHilLink *getHILSimulation() const QGCHilLink* getHILSimulation() const {
{
return simulation; return simulation;
} }
#endif #endif

5
src/ui/QGCHilXPlaneConfiguration.cc
Unescape View File

@ -58,20 +58,17 @@ void QGCHilXPlaneConfiguration::setVersion(int version)
void QGCHilXPlaneConfiguration::toggleSimulation(bool connect) void QGCHilXPlaneConfiguration::toggleSimulation(bool connect)
{ {
if (!link) if (!link) {
{
return; return;
} }
Q_UNUSED(connect); Q_UNUSED(connect);
if (!link->isConnected()) if (!link->isConnected())
{ {
link->setRemoteHost(ui->hostComboBox->currentText()); link->setRemoteHost(ui->hostComboBox->currentText());
link->connectSimulation(); link->connectSimulation();
ui->startButton->setText(tr("Disconnect")); ui->startButton->setText(tr("Disconnect"));
} }
else else
{ {
link->disconnectSimulation(); link->disconnectSimulation();

Write Preview
Loading…
Cancel
Save