Browse Source

Revert "fix code style"

This reverts commit ae976049ca7b4818616e6b8be07a028fc7dfc26f.
QGC4.4
Bart Slinger 9 years ago
parent
commit
ae1531705a
  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

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

13
src/comm/QGCXPlaneLink.h

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

243
src/uas/UAS.cc

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

42
src/uas/UAS.h

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

5
src/ui/QGCHilXPlaneConfiguration.cc

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

Loading…
Cancel
Save