Skip to content
GitLab
Commit 0f035389 authored by Bart Slinger's avatar Bart Slinger
Browse files

fix code style

parent e6a125ee
Show whitespace changes
Inline Side-by-side
src/comm/QGCXPlaneLink.cc
View file @ 0f035389
......@@ -32,7 +32,7 @@
#include "QGCMessageBox.h"
#include "HomePositionManager.h"
QGCXPlaneLink::QGCXPlaneLink(Vehicle* vehicle, QString remoteHost, QHostAddress localHost, quint16 localPort) :
QGCXPlaneLink::QGCXPlaneLink(Vehicle *vehicle, QString remoteHost, QHostAddress localHost, quint16 localPort) :
_vehicle(vehicle),
remoteHost(QHostAddress("127.0.0.1")),
remotePort(49000),
......@@ -73,7 +73,8 @@ QGCXPlaneLink::~QGCXPlaneLink()
// Tell the thread to exit
_should_exit = true;
if (socket) {
if (socket)
{
socket->close();
socket->deleteLater();
socket = NULL;
......@@ -106,21 +107,25 @@ void QGCXPlaneLink::storeSettings()
settings.endGroup();
}
void QGCXPlaneLink::setVersion(const QString& version)
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;
......@@ -136,19 +141,23 @@ 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);
}
......@@ -163,12 +172,14 @@ 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;
}
......@@ -176,7 +187,9 @@ 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!");
......@@ -237,14 +250,15 @@ void QGCXPlaneLink::run()
strncpy(ip.str_port_them, localPortStr.toLatin1(), qMin((int)sizeof(ip.str_port_them), 6));
ip.use_ip = 1;
writeBytesSafe((const char*)&ip, sizeof(ip));
writeBytesSafe((const char *)&ip, sizeof(ip));
/* Call function which makes sure individual control override is enabled/disabled */
enableHilActuatorControls(_useHilActuatorControls);
_should_exit = false;
while(!_should_exit) {
while (!_should_exit)
{
QCoreApplication::processEvents();
QGC::SLEEP::msleep(5);
}
......@@ -278,7 +292,8 @@ 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;
......@@ -314,7 +329,7 @@ QString QGCXPlaneLink::getRemoteHost()
/**
* @param newHost Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551
*/
void QGCXPlaneLink::setRemoteHost(const QString& newHost)
void QGCXPlaneLink::setRemoteHost(const QString &newHost)
{
if (newHost.length() == 0)
return;
......@@ -322,11 +337,13 @@ 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
......@@ -335,18 +352,22 @@ 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;
}
}
......@@ -363,17 +384,20 @@ 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 {
#pragma pack(push, 1)
struct payload
{
char b[5];
int index;
float f[8];
} p;
#pragma pack(pop)
#pragma pack(pop)
p.b[0] = 'D';
p.b[1] = 'A';
......@@ -399,8 +423,9 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch
// Direct throttle control
p.index = 25;
writeBytesSafe((const char*)&p, sizeof(p));
writeBytesSafe((const char *)&p, sizeof(p));
}
else
{
// direct pass-through, normal fixed-wing.
......@@ -412,11 +437,11 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch
// Send to group 12
p.index = 12;
writeBytesSafe((const char*)&p, sizeof(p));
writeBytesSafe((const char *)&p, sizeof(p));
// Send to group 8, which equals manual controls
p.index = 8;
writeBytesSafe((const char*)&p, sizeof(p));
writeBytesSafe((const char *)&p, sizeof(p));
// Send throttle to all four motors
p.index = 25;
......@@ -425,13 +450,14 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch
p.f[1] = throttle;
p.f[2] = throttle;
p.f[3] = throttle;
writeBytesSafe((const char*)&p, sizeof(p));
writeBytesSafe((const char *)&p, sizeof(p));
}
}
void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode)
{
if (!_useHilActuatorControls) {
if (!_useHilActuatorControls)
{
//qDebug() << "received HIL_ACTUATOR_CONTROLS but not using it";
return;
}
......@@ -444,13 +470,14 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
Q_UNUSED(ctl_14);
Q_UNUSED(ctl_15);
#pragma pack(push, 1)
struct payload {
#pragma pack(push, 1)
struct payload
{
char b[5];
int index;
float f[8];
} p;
#pragma pack(pop)
#pragma pack(pop)
p.b[0] = 'D';
p.b[1] = 'A';
......@@ -461,7 +488,8 @@ 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:
......@@ -477,9 +505,10 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
/* Direct throttle control */
p.index = 25;
writeBytesSafe((const char*)&p, sizeof(p));
writeBytesSafe((const char *)&p, sizeof(p));
break;
}
case MAV_TYPE_VTOL_RESERVED2:
{
/**
......@@ -496,7 +525,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
p.f[6] = ctl_6;
p.f[7] = ctl_7;
p.index = 25;
writeBytesSafe((const char*)&p, sizeof(p));
writeBytesSafe((const char *)&p, sizeof(p));
/* Control individual actuators */
float max_surface_deflection = 30.0f; // Degrees
......@@ -507,6 +536,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
break;
}
default:
{
/* direct pass-through, normal fixed-wing. */
......@@ -516,7 +546,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
/* Send to group 8, which equals manual controls */
p.index = 8;
writeBytesSafe((const char*)&p, sizeof(p));
writeBytesSafe((const char *)&p, sizeof(p));
/* Send throttle to all eight motors */
p.index = 25;
......@@ -528,7 +558,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
p.f[5] = ctl_3;
p.f[6] = ctl_3;
p.f[7] = ctl_3;
writeBytesSafe((const char*)&p, sizeof(p));
writeBytesSafe((const char *)&p, sizeof(p));
break;
}
......@@ -536,7 +566,8 @@ 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);
......@@ -559,8 +590,8 @@ Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) {
double sss = ss_ * __s;
Eigen::Matrix3f wRo;
wRo <<
cc_ , css-s_c, csc+s_s,
sc_ , sss+c_c, ssc-c_s,
cc_ , css - s_c, csc + s_s,
sc_ , sss + c_c, ssc - c_s,
-_s_ , _cs, _cc;
return wRo;
}
......@@ -591,25 +622,30 @@ 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) {
std::string headStr = std::string(data, data+5);
if (s > maxLength)
{
std::string headStr = std::string(data, data + 5);
std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl;
}
// Calculate the number of data segments a 36 bytes
// XPlane always has 5 bytes header: 'DATA@'
unsigned nsegs = (s-5)/36;
unsigned nsegs = (s - 5) / 36;
//qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
#pragma pack(push, 1)
struct payload {
#pragma pack(push, 1)
struct payload
{
int index;
float f[8];
} p;
#pragma pack(pop)
#pragma pack(pop)
bool oldConnectionState = xPlaneConnected;
......@@ -620,15 +656,16 @@ void QGCXPlaneLink::readBytes()
{
xPlaneConnected = true;
if (oldConnectionState != xPlaneConnected) {
if (oldConnectionState != xPlaneConnected)
{
simUpdateFirst = QGC::groundTimeMilliseconds();
}
for (unsigned i = 0; i < nsegs; i++)
{
// Get index
unsigned ioff = (5+i*36);;
memcpy(&(p), data+ioff, sizeof(p));
unsigned ioff = (5 + i * 36);;
memcpy(&(p), data + ioff, sizeof(p));
if (p.index == 3)
{
......@@ -638,13 +675,14 @@ 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
// with v10.3 and earlier) delivers yacc=0 and zacc=0 when the ground speed is very low, which gives e.g. wrong readings
// before launch when waiting on the runway. This might pose a problem for initial state estimation/calibration.
// Instead, we calculate our own accelerations.
if (fabsf(groundspeed)<0.1f && alt_agl<1.0)
if (fabsf(groundspeed) < 0.1f && alt_agl < 1.0)
{
// TODO: Add centrip. acceleration to the current static acceleration implementation.
Eigen::Vector3f g(0, 0, -9.80665f);
......@@ -657,6 +695,7 @@ void QGCXPlaneLink::readBytes()
//qDebug() << "Calculated values" << gr[0] << gr[1] << gr[2];
}
else
{
// Accelerometer readings, directly from X-Plane and including centripetal forces.
......@@ -671,6 +710,7 @@ 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)
{
......@@ -679,6 +719,7 @@ 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)
......@@ -700,6 +741,7 @@ 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];
......@@ -708,19 +750,25 @@ 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);
}
......@@ -754,7 +802,7 @@ void QGCXPlaneLink::readBytes()
dcm[2][1] = sinPhi * cosThe;
dcm[2][2] = cosPhi * cosThe;
Eigen::Matrix3f m = Eigen::Map<Eigen::Matrix3f>((float*)dcm).eval();
Eigen::Matrix3f m = Eigen::Map<Eigen::Matrix3f>((float *)dcm).eval();
Eigen::Vector3f mag(xmag, ymag, zmag);
......@@ -785,6 +833,7 @@ 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];
......@@ -793,6 +842,7 @@ 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];
......@@ -835,7 +885,8 @@ 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;
}
......@@ -843,7 +894,9 @@ 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));
......@@ -890,19 +943,23 @@ void QGCXPlaneLink::readBytes()
int gps_fix_type = 3;
float eph = 0.3f;
float epv = 0.6f;
float vel = sqrt(vx*vx + vy*vy + vz*vz);
float vel = sqrt(vx * vx + vy * vy + vz * vz);
float cog = atan2(vy, vx);
int satellites = 8;
emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites);
} else {
}
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);
......@@ -948,7 +1005,10 @@ bool QGCXPlaneLink::disconnectSimulation()
if (connectState)
{
_should_exit = true;
} else {
}
else
{
emit simulationDisconnected();
emit simulationConnected(false);
}
......@@ -956,7 +1016,7 @@ bool QGCXPlaneLink::disconnectSimulation()
return !connectState;
}
void QGCXPlaneLink::selectAirframe(const QString& plane)
void QGCXPlaneLink::selectAirframe(const QString &plane)
{
airframeName = plane;
......@@ -967,29 +1027,34 @@ 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");
}
}
void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw)
{
#pragma pack(push, 1)
#pragma pack(push, 1)
struct VEH1_struct
{
char header[5];
......@@ -998,7 +1063,7 @@ void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, doub
float psi_the_phi[3];
float gear_flap_vect[3];
} pos;
#pragma pack(pop)
#pragma pack(pop)
pos.header[0] = 'V';
pos.header[1] = 'E';
......@@ -1016,7 +1081,7 @@ void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, doub
pos.gear_flap_vect[1] = 0.0f;
pos.gear_flap_vect[2] = 0.0f;
writeBytesSafe((const char*)&pos, sizeof(pos));
writeBytesSafe((const char *)&pos, sizeof(pos));
// pos.header[0] = 'V';
// pos.header[1] = 'E';
......@@ -1046,8 +1111,8 @@ void QGCXPlaneLink::setRandomPosition()
// Initialize generator
srand(0);
double offLat = rand() / static_cast<double>(RAND_MAX) / 500.0 + 1.0/500.0;
double offLon = rand() / static_cast<double>(RAND_MAX) / 500.0 + 1.0/500.0;
double offLat = rand() / static_cast<double>(RAND_MAX) / 500.0 + 1.0 / 500.0;
double offLon = rand() / static_cast<double>(RAND_MAX) / 500.0 + 1.0 / 500.0;
double offAlt = rand() / static_cast<double>(RAND_MAX) * 200.0 + 100.0;
if (_vehicle->altitudeAMSL()->rawValue().toDouble() + offAlt < 0)
......@@ -1087,9 +1152,13 @@ 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();
......@@ -1123,13 +1192,14 @@ void QGCXPlaneLink::setName(QString name)
void QGCXPlaneLink::sendDataRef(QString ref, float value)
{
#pragma pack(push, 1)
struct payload {
#pragma pack(push, 1)
struct payload
{
char b[5];
float value;
char name[500];
} dref;
#pragma pack(pop)
#pragma pack(pop)
dref.b[0] = 'D';
dref.b[1] = 'R';
......@@ -1147,12 +1217,16 @@ 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));
writeBytesSafe((const char *)&dref, sizeof(dref));
}
src/comm/QGCXPlaneLink.h
View file @ 0f035389
......@@ -36,7 +36,7 @@ class QGCXPlaneLink : public QGCHilLink
//Q_INTERFACES(QGCXPlaneLinkInterface:LinkInterface)
public:
QGCXPlaneLink(Vehicle* vehicle, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress localHost = QHostAddress::Any, quint16 localPort = 49005);
QGCXPlaneLink(Vehicle *vehicle, QString remoteHost = QString("127.0.0.1:49000"), QHostAddress localHost = QHostAddress::Any, quint16 localPort = 49005);
~QGCXPlaneLink();
/**
......@@ -51,7 +51,8 @@ public:
bool isConnected();
qint64 bytesAvailable();
int getPort() const {
int getPort() const
{
return localPort;
}
......@@ -88,11 +89,13 @@ public:
return (int)airframeID;
}
bool sensorHilEnabled() {
bool sensorHilEnabled()
{
return _sensorHilEnabled;
}
bool useHilActuatorControls() {
bool useHilActuatorControls()
{
return _useHilActuatorControls;
}
......@@ -104,7 +107,7 @@ public slots:
// void setAddress(QString address);
void setPort(int port);
/** @brief Add a new host to broadcast messages to */
void setRemoteHost(const QString& host);
void setRemoteHost(const QString &host);
/** @brief Send new control states to the simulation */
void updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode);
/** @brief Send new control commands to the simulation */
......@@ -127,13 +130,15 @@ public slots:
float ctl_15,
quint8 mode);
/** @brief Set the simulator version as text string */
void setVersion(const QString& version);
void setVersion(const QString &version);
/** @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);
}
......@@ -159,7 +164,7 @@ public slots:
* @brief Select airplane model
* @param plane the name of the airplane
*/
void selectAirframe(const QString& airframe);
void selectAirframe(const QString &airframe);
/**
* @brief Set the airplane position and attitude
* @param lat
......@@ -182,14 +187,14 @@ public slots:
void setRandomAttitude();
protected:
Vehicle* _vehicle;
Vehicle *_vehicle;
QString name;
QHostAddress localHost;
quint16 localPort;
QHostAddress remoteHost;
quint16 remotePort;
int id;
QUdpSocket* socket;
QUdpSocket *socket;
bool connectState;
quint64 bitsSentTotal;
......@@ -202,8 +207,8 @@ protected:
QMutex statisticsMutex;
QMutex dataMutex;
QTimer refreshTimer;
QProcess* process;
QProcess* terraSync;
QProcess *process;
QProcess *terraSync;
bool gpsReceived;
bool attitudeReceived;
......
src/uas/UAS.cc
View file @ 0f035389
......@@ -54,7 +54,7 @@ QGC_LOGGING_CATEGORY(UASLog, "UASLog")
* creating the UAS.
*/
UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * firmwarePluginManager) : UASInterface(),
UAS::UAS(MAVLinkProtocol *protocol, Vehicle *vehicle, FirmwarePluginManager *firmwarePluginManager) : UASInterface(),
lipoFull(4.2f),
lipoEmpty(3.5f),
uasId(vehicle->id()),
......@@ -167,7 +167,7 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
_firmwarePluginManager(firmwarePluginManager)
{
for (unsigned int i = 0; i<255;++i)
for (unsigned int i = 0; i < 255; ++i)
{
componentID[i] = -1;
componentMulti[i] = false;
......@@ -201,16 +201,19 @@ 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";
......@@ -240,6 +243,7 @@ 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;
......@@ -251,6 +255,7 @@ void UAS::receiveMessage(mavlink_message_t message)
// Prefer the first component
componentID[message.msgid] = message.compid;
}
else
{
// Got this message already
......@@ -272,6 +277,7 @@ void UAS::receiveMessage(mavlink_message_t message)
{
break;
}
mavlink_heartbeat_t state;
mavlink_msg_heartbeat_decode(&message, &state);
......@@ -303,6 +309,7 @@ void UAS::receiveMessage(mavlink_message_t message)
{
break;
}
mavlink_sys_status_t state;
mavlink_msg_sys_status_decode(&message, &state);
......@@ -318,8 +325,8 @@ void UAS::receiveMessage(mavlink_message_t message)
emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time);
// Process CPU load.
emit loadChanged(this,state.load/10.0f);
emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time);
emit loadChanged(this, state.load / 10.0f);
emit valueChanged(uasId, name.arg("load"), "%", state.load / 10.0f, time);
// control_sensors_enabled:
// relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control
......@@ -336,10 +343,12 @@ 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);
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;
......@@ -361,6 +370,7 @@ void UAS::receiveMessage(mavlink_message_t message)
}
}
break;
case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
{
mavlink_attitude_quaternion_t attitude;
......@@ -390,17 +400,24 @@ 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]);
}
......@@ -422,6 +439,7 @@ void UAS::receiveMessage(mavlink_message_t message)
}
}
break;
case MAVLINK_MSG_ID_HIL_CONTROLS:
{
mavlink_hil_controls_t hil;
......@@ -429,6 +447,7 @@ 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;
......@@ -453,29 +472,33 @@ void UAS::receiveMessage(mavlink_message_t message)
hil.mode);
}
break;
case MAVLINK_MSG_ID_VFR_HUD:
{
mavlink_vfr_hud_t hud;
mavlink_msg_vfr_hud_decode(&message, &hud);
quint64 time = getUnixTime();
// Display updated values
emit thrustChanged(this, hud.throttle/100.0);
emit thrustChanged(this, hud.throttle / 100.0);
if (!attitudeKnown)
{
setYaw(QGC::limitAngleToPMPId((((double)hud.heading)/180.0)*M_PI));
setYaw(QGC::limitAngleToPMPId((((double)hud.heading) / 180.0)*M_PI));
emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
}
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;
......@@ -495,6 +518,7 @@ void UAS::receiveMessage(mavlink_message_t message)
}
}
break;
case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
{
mavlink_global_vision_position_estimate_t pos;
......@@ -503,6 +527,7 @@ 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;
......@@ -512,27 +537,28 @@ void UAS::receiveMessage(mavlink_message_t message)
quint64 time = getUnixTime();
setLatitude(pos.lat/(double)1E7);
setLongitude(pos.lon/(double)1E7);
setAltitudeRelative(pos.relative_alt/1000.0);
setLatitude(pos.lat / (double)1E7);
setLongitude(pos.lon / (double)1E7);
setAltitudeRelative(pos.relative_alt / 1000.0);
globalEstimatorActive = true;
speedX = pos.vx/100.0;
speedY = pos.vy/100.0;
speedZ = pos.vz/100.0;
speedX = pos.vx / 100.0;
speedY = pos.vy / 100.0;
speedZ = pos.vz / 100.0;
emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time);
emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time);
// We had some frame mess here, global and local axes were mixed.
emit velocityChanged_NED(this, speedX, speedY, speedZ, time);
setGroundSpeed(qSqrt(speedX*speedX+speedY*speedY));
setGroundSpeed(qSqrt(speedX * speedX + speedY * speedY));
emit speedChanged(this, groundSpeed, airSpeed, time);
isGlobalPositionKnown = true;
}
break;
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
mavlink_gps_raw_int_t pos;
......@@ -542,33 +568,41 @@ 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)
{
isGlobalPositionKnown = true;
latitude_gps = pos.lat/(double)1E7;
longitude_gps = pos.lon/(double)1E7;
altitude_gps = pos.alt/1000.0;
latitude_gps = pos.lat / (double)1E7;
longitude_gps = pos.lon / (double)1E7;
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;
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));
}
}
......@@ -577,19 +611,24 @@ void UAS::receiveMessage(mavlink_message_t message)
double dtmp;
//-- Raw GPS data
dtmp = pos.eph == 0xFFFF ? 1e10f : pos.eph / 100.0;
if(dtmp != satRawHDOP)
if (dtmp != satRawHDOP)
{
satRawHDOP = dtmp;
emit satRawHDOPChanged(satRawHDOP);
}
dtmp = pos.epv == 0xFFFF ? 1e10f : pos.epv / 100.0;
if(dtmp != satRawVDOP)
if (dtmp != satRawVDOP)
{
satRawVDOP = dtmp;
emit satRawVDOPChanged(satRawVDOP);
}
dtmp = pos.cog == 0xFFFF ? 0.0 : pos.cog / 100.0;
if(dtmp != satRawCOG)
if (dtmp != satRawCOG)
{
satRawCOG = dtmp;
emit satRawCOGChanged(satRawCOG);
......@@ -600,14 +639,17 @@ 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++)
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;
......@@ -624,9 +666,10 @@ void UAS::receiveMessage(mavlink_message_t message)
paramVal.param_float = rawValue.param_value;
paramVal.type = rawValue.param_type;
processParamValueMsg(message, parameterName,rawValue,paramVal);
processParamValueMsg(message, parameterName, rawValue, paramVal);
}
break;
case MAVLINK_MSG_ID_ATTITUDE_TARGET:
{
mavlink_attitude_target_t out;
......@@ -649,12 +692,14 @@ 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;
......@@ -662,14 +707,15 @@ 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;
b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN + 1);
mavlink_msg_statustext_get_text(&message, b.data());
// Ensure NUL-termination
b[b.length()-1] = '\0';
b[b.length() - 1] = '\0';
QString text = QString(b);
int severity = mavlink_msg_statustext_get_severity(&message);
......@@ -681,6 +727,7 @@ 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);
......@@ -723,9 +770,11 @@ 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;
}
......@@ -745,7 +794,7 @@ void UAS::receiveMessage(mavlink_message_t message)
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
{
mavlink_nav_controller_output_t p;
mavlink_msg_nav_controller_output_decode(&message,&p);
mavlink_msg_nav_controller_output_decode(&message, &p);
setDistToWaypoint(p.wp_dist);
setBearingToWaypoint(p.nav_bearing);
emit navigationControllerErrorsChanged(this, p.alt_error, p.aspd_error, p.xtrack_error);
......@@ -777,7 +826,8 @@ void UAS::receiveMessage(mavlink_message_t message)
void UAS::startCalibration(UASInterface::StartCalibrationType calType)
{
if (!_vehicle) {
if (!_vehicle)
{
return;
}
......@@ -788,34 +838,44 @@ 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;
......@@ -841,7 +901,8 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
void UAS::stopCalibration(void)
{
if (!_vehicle) {
if (!_vehicle)
{
return;
}
......@@ -865,16 +926,19 @@ 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;
......@@ -900,7 +964,8 @@ void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
void UAS::stopBusConfig(void)
{
if (!_vehicle) {
if (!_vehicle)
{
return;
}
......@@ -936,6 +1001,7 @@ 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
......@@ -953,6 +1019,7 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
// 1000 milliseconds
// 1000 microseconds
#ifndef _MSC_VER
else if (time < 1261440000000000LLU)
#else
else if (time < 1261440000000000)
......@@ -961,15 +1028,17 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
// qDebug() << "GEN time:" << time/1000 + onboardTimeOffset;
if (onboardTimeOffset == 0)
{
onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
onboardTimeOffset = QGC::groundTimeMilliseconds() - time / 1000;
}
return time/1000 + onboardTimeOffset;
return time / 1000 + onboardTimeOffset;
}
else
{
// Time is not zero and larger than 40 years -> has to be
// a Unix epoch timestamp. Do nothing.
return time/1000;
return time / 1000;
}
}
......@@ -984,7 +1053,7 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
*/
quint64 UAS::getUnixTimeFromMs(quint64 time)
{
return getUnixTime(time*1000);
return getUnixTime(time * 1000);
}
/**
......@@ -999,6 +1068,7 @@ quint64 UAS::getUnixTimeFromMs(quint64 time)
quint64 UAS::getUnixTime(quint64 time)
{
quint64 ret = 0;
if (attitudeStamped)
{
ret = lastAttitude;
......@@ -1008,6 +1078,7 @@ 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
......@@ -1025,6 +1096,7 @@ quint64 UAS::getUnixTime(quint64 time)
// 1000 milliseconds
// 1000 microseconds
#ifndef _MSC_VER
else if (time < 1261440000000000LLU)
#else
else if (time < 1261440000000000)
......@@ -1034,17 +1106,19 @@ quint64 UAS::getUnixTime(quint64 time)
if (onboardTimeOffset == 0 || time < (lastNonNullTime - 100))
{
lastNonNullTime = time;
onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
onboardTimeOffset = QGC::groundTimeMilliseconds() - time / 1000;
}
if (time > lastNonNullTime) lastNonNullTime = time;
ret = time/1000 + onboardTimeOffset;
ret = time / 1000 + onboardTimeOffset;
}
else
{
// Time is not zero and larger than 40 years -> has to be
// a Unix epoch timestamp. Do nothing.
ret = time/1000;
ret = time / 1000;
}
return ret;
......@@ -1055,7 +1129,7 @@ quint64 UAS::getUnixTime(quint64 time)
* Status can be unitialized, booting up, calibrating sensors, active
* standby, cirtical, emergency, shutdown or unknown.
*/
void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription)
void UAS::getStatusForCode(int statusCode, QString &uasState, QString &stateDescription)
{
switch (statusCode)
{
......@@ -1063,30 +1137,37 @@ 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");
......@@ -1125,17 +1206,18 @@ QImage UAS::getImage()
if (imageRecBuffer.isNull())
{
qDebug()<< "could not convertToPGM()";
qDebug() << "could not convertToPGM()";
return QImage();
}
if (!image.loadFromData(tmpImage, "PGM"))
{
qDebug()<< __FILE__ << __LINE__ << "could not create extracted image";
qDebug() << __FILE__ << __LINE__ << "could not create extracted image";
return QImage();
}
}
// BMP with header
else if (imageType == MAVLINK_DATA_STREAM_IMG_BMP ||
imageType == MAVLINK_DATA_STREAM_IMG_JPEG ||
......@@ -1158,7 +1240,8 @@ QImage UAS::getImage()
void UAS::requestImage()
{
if (!_vehicle) {
if (!_vehicle)
{
return;
}
......@@ -1183,10 +1266,11 @@ void UAS::requestImage()
*/
quint64 UAS::getUptime() const
{
if(startTime == 0)
if (startTime == 0)
{
return 0;
}
else
{
return QGC::groundTimeMilliseconds() - startTime;
......@@ -1194,7 +1278,7 @@ quint64 UAS::getUptime() const
}
//TODO update this to use the parameter manager / param data model instead
void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramUnion)
void UAS::processParamValueMsg(mavlink_message_t &msg, const QString &paramName, const mavlink_param_value_t &rawValue, mavlink_param_union_t &paramUnion)
{
int compId = msg.compid;
......@@ -1202,7 +1286,8 @@ 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;
......@@ -1250,7 +1335,8 @@ 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;
}
......@@ -1277,7 +1363,8 @@ 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;
}
......@@ -1295,12 +1382,17 @@ 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
......@@ -1308,7 +1400,8 @@ 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;
......@@ -1318,7 +1411,8 @@ 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);
......@@ -1336,7 +1430,10 @@ 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;
......@@ -1344,8 +1441,8 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
//XXX: find decent scaling
px -= pitch;
py += roll;
pz -= 2.0f*(thrust-0.5);
uint16_t typeMask = (1<<11)|(7<<6)|(7<<3); // select only POSITION control
pz -= 2.0f * (thrust - 0.5);
uint16_t typeMask = (1 << 11) | (7 << 6) | (7 << 3); // select only POSITION control
mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(),
mavlink->getComponentId(),
&message,
......@@ -1366,14 +1463,17 @@ 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);
const float fx = -dcm[0][2] * thrust;
const float fy = -dcm[1][2] * thrust;
const float fz = -dcm[2][2] * thrust;
uint16_t typeMask = (3<<10)|(7<<3)|(7<<0)|(1<<9); // select only FORCE control (disable everything else)
uint16_t typeMask = (3 << 10) | (7 << 3) | (7 << 0) | (1 << 9); // select only FORCE control (disable everything else)
mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(),
mavlink->getComponentId(),
&message,
......@@ -1394,7 +1494,10 @@ 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;
......@@ -1403,9 +1506,9 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
//XXX: find decent scaling
vx -= pitch;
vy += roll;
vz -= 2.0f*(thrust-0.5);
vz -= 2.0f * (thrust - 0.5);
yawrate += yaw; //XXX: not sure what scale to apply here
uint16_t typeMask = (1<<10)|(7<<6)|(7<<0); // select only VELOCITY control
uint16_t typeMask = (1 << 10) | (7 << 6) | (7 << 0); // select only VELOCITY control
mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(),
mavlink->getComponentId(),
&message,
......@@ -1426,7 +1529,10 @@ 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;
......@@ -1460,13 +1566,15 @@ 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
if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
if (((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED))
{
mavlink_message_t message;
float q[4];
......@@ -1491,6 +1599,7 @@ 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";
......@@ -1503,7 +1612,8 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
*/
void UAS::pairRX(int rxType, int rxSubType)
{
if (!_vehicle) {
if (!_vehicle)
{
return;
}
......@@ -1517,17 +1627,21 @@ void UAS::pairRX(int rxType, int rxSubType)
* If enabled, connect the flight gear link.
*/
#ifndef __mobile__
void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject * configuration)
void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject *configuration)
{
Q_UNUSED(configuration);
QGCFlightGearLink* link = dynamic_cast<QGCFlightGearLink*>(simulation);
if (!link) {
QGCFlightGearLink *link = dynamic_cast<QGCFlightGearLink *>(simulation);
if (!link)
{
// Delete wrong sim
if (simulation) {
if (simulation)
{
stopHil();
delete simulation;
}
simulation = new QGCFlightGearLink(_vehicle, options);
}
......@@ -1547,15 +1661,17 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
temperature_var = noise_scaler * 0.7290f;
// Connect Flight Gear Link
link = dynamic_cast<QGCFlightGearLink*>(simulation);
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();
......@@ -1569,22 +1685,29 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
#ifndef __mobile__
void UAS::enableHilJSBSim(bool enable, QString options)
{
QGCJSBSimLink* link = dynamic_cast<QGCJSBSimLink*>(simulation);
if (!link) {
QGCJSBSimLink *link = dynamic_cast<QGCJSBSimLink *>(simulation);
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 = dynamic_cast<QGCJSBSimLink *>(simulation);
link->setStartupArguments(options);
if (enable)
{
startHil();
}
else
{
stopHil();
......@@ -1598,12 +1721,16 @@ void UAS::enableHilJSBSim(bool enable, QString options)
#ifndef __mobile__
void UAS::enableHilXPlane(bool enable)
{
QGCXPlaneLink* link = dynamic_cast<QGCXPlaneLink*>(simulation);
if (!link) {
if (simulation) {
QGCXPlaneLink *link = dynamic_cast<QGCXPlaneLink *>(simulation);
if (!link)
{
if (simulation)
{
stopHil();
delete simulation;
}
simulation = new QGCXPlaneLink(_vehicle);
float noise_scaler = 0.0001f;
......@@ -1621,11 +1748,13 @@ 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();
......@@ -1670,13 +1799,13 @@ void UAS::sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw
emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, getUnixTime());
emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime());
emit valueChanged(uasId, "lat sim", "deg", lat*1e7, getUnixTime());
emit valueChanged(uasId, "lon sim", "deg", lon*1e7, getUnixTime());
emit valueChanged(uasId, "alt sim", "deg", alt*1e3, getUnixTime());
emit valueChanged(uasId, "lat sim", "deg", lat * 1e7, getUnixTime());
emit valueChanged(uasId, "lon sim", "deg", lon * 1e7, getUnixTime());
emit valueChanged(uasId, "alt sim", "deg", alt * 1e3, getUnixTime());
emit valueChanged(uasId, "vx sim", "m/s", vx*1e2, getUnixTime());
emit valueChanged(uasId, "vy sim", "m/s", vy*1e2, getUnixTime());
emit valueChanged(uasId, "vz sim", "m/s", vz*1e2, getUnixTime());
emit valueChanged(uasId, "vx sim", "m/s", vx * 1e2, getUnixTime());
emit valueChanged(uasId, "vy sim", "m/s", vy * 1e2, getUnixTime());
emit valueChanged(uasId, "vz sim", "m/s", vz * 1e2, getUnixTime());
emit valueChanged(uasId, "IAS sim", "m/s", ind_airspeed, getUnixTime());
emit valueChanged(uasId, "TAS sim", "m/s", true_airspeed, getUnixTime());
......@@ -1706,7 +1835,8 @@ 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;
}
......@@ -1732,9 +1862,10 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
mavlink_message_t msg;
mavlink_msg_hil_state_quaternion_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
time_us, q, rollspeed, pitchspeed, yawspeed,
lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, ind_airspeed*100, true_airspeed*100, xacc*1000/9.81, yacc*1000/9.81, zacc*1000/9.81);
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
......@@ -1761,7 +1892,7 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
u1 = rand() * (1.0 / RAND_MAX);
u2 = rand() * (1.0 / RAND_MAX);
}
while ( u1 <= epsilon ); //Have a catch to ensure non-zero for log()
while (u1 <= epsilon); //Have a catch to ensure non-zero for log()
z0 = sqrt(-2.0 * log(u1)) * cos(2.0f * M_PI * u2); //calculate normally distributed variable with mu = 0, var = 1
......@@ -1770,9 +1901,13 @@ 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;
}
}
......@@ -1786,7 +1921,8 @@ 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;
}
......@@ -1795,16 +1931,16 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
float xacc_corrupt = addZeroMeanNoise(xacc, xacc_var);
float yacc_corrupt = addZeroMeanNoise(yacc, yacc_var);
float zacc_corrupt = addZeroMeanNoise(zacc, zacc_var);
float rollspeed_corrupt = addZeroMeanNoise(rollspeed,rollspeed_var);
float pitchspeed_corrupt = addZeroMeanNoise(pitchspeed,pitchspeed_var);
float yawspeed_corrupt = addZeroMeanNoise(yawspeed,yawspeed_var);
float rollspeed_corrupt = addZeroMeanNoise(rollspeed, rollspeed_var);
float pitchspeed_corrupt = addZeroMeanNoise(pitchspeed, pitchspeed_var);
float yawspeed_corrupt = addZeroMeanNoise(yawspeed, yawspeed_var);
float xmag_corrupt = addZeroMeanNoise(xmag, xmag_var);
float ymag_corrupt = addZeroMeanNoise(ymag, ymag_var);
float zmag_corrupt = addZeroMeanNoise(zmag, zmag_var);
float abs_pressure_corrupt = addZeroMeanNoise(abs_pressure,abs_pressure_var);
float abs_pressure_corrupt = addZeroMeanNoise(abs_pressure, abs_pressure_var);
float diff_pressure_corrupt = addZeroMeanNoise(diff_pressure, diff_pressure_var);
float pressure_alt_corrupt = addZeroMeanNoise(pressure_alt, pressure_alt_var);
float temperature_corrupt = addZeroMeanNoise(temperature,temperature_var);
float temperature_corrupt = addZeroMeanNoise(temperature, temperature_var);
mavlink_message_t msg;
mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
......@@ -1814,6 +1950,7 @@ 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
......@@ -1827,7 +1964,8 @@ 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;
}
......@@ -1852,6 +1990,7 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds();
#endif
}
else
{
// Attempt to set HIL mode
......@@ -1865,7 +2004,8 @@ 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;
}
......@@ -1876,18 +2016,21 @@ 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;
mavlink_message_t msg;
mavlink_msg_hil_gps_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, vn*1e2, ve*1e2, vd*1e2, course*1e2, satellites);
time_us, fix_type, lat * 1e7, lon * 1e7, alt * 1e3, eph * 1e2, epv * 1e2, vel * 1e2, vn * 1e2, ve * 1e2, vd * 1e2, course * 1e2, satellites);
lastSendTimeGPS = QGC::groundTimeMilliseconds();
_vehicle->sendMessageOnPriorityLink(msg);
}
else
{
// Attempt to set HIL mode
......@@ -1904,6 +2047,7 @@ 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);
......@@ -1919,11 +2063,13 @@ 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;
}
......@@ -1939,13 +2085,15 @@ 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++)
{
......@@ -1973,13 +2121,15 @@ 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(),
......@@ -1997,7 +2147,7 @@ void UAS::unsetRCToParameterMap()
}
}
void UAS::_say(const QString& text, int severity)
void UAS::_say(const QString &text, int severity)
{
Q_UNUSED(severity);
qgcApp()->toolbox()->audioOutput()->say(text);
......@@ -2007,12 +2157,15 @@ 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;
}
src/uas/UAS.h
View file @ 0f035389
......@@ -50,7 +50,7 @@ class UAS : public UASInterface
{
Q_OBJECT
public:
UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * firmwarePluginManager);
UAS(MAVLinkProtocol *protocol, Vehicle *vehicle, FirmwarePluginManager *firmwarePluginManager);
float lipoFull; ///< 100% charged voltage
float lipoEmpty; ///< Discharged voltage
......@@ -89,8 +89,8 @@ public:
void setGroundSpeed(double val)
{
groundSpeed = val;
emit groundSpeedChanged(val,"groundSpeed");
emit valueChanged(this->uasId,"groundSpeed","m/s",QVariant(val),getUnixTime());
emit groundSpeedChanged(val, "groundSpeed");
emit valueChanged(this->uasId, "groundSpeed", "m/s", QVariant(val), getUnixTime());
}
double getGroundSpeed() const
{
......@@ -100,8 +100,8 @@ public:
void setAirSpeed(double val)
{
airSpeed = val;
emit airSpeedChanged(val,"airSpeed");
emit valueChanged(this->uasId,"airSpeed","m/s",QVariant(val),getUnixTime());
emit airSpeedChanged(val, "airSpeed");
emit valueChanged(this->uasId, "airSpeed", "m/s", QVariant(val), getUnixTime());
}
double getAirSpeed() const
......@@ -112,8 +112,8 @@ public:
void setLocalX(double val)
{
localX = val;
emit localXChanged(val,"localX");
emit valueChanged(this->uasId,"localX","m",QVariant(val),getUnixTime());
emit localXChanged(val, "localX");
emit valueChanged(this->uasId, "localX", "m", QVariant(val), getUnixTime());
}
double getLocalX() const
......@@ -124,8 +124,8 @@ public:
void setLocalY(double val)
{
localY = val;
emit localYChanged(val,"localY");
emit valueChanged(this->uasId,"localY","m",QVariant(val),getUnixTime());
emit localYChanged(val, "localY");
emit valueChanged(this->uasId, "localY", "m", QVariant(val), getUnixTime());
}
double getLocalY() const
{
......@@ -135,8 +135,8 @@ public:
void setLocalZ(double val)
{
localZ = val;
emit localZChanged(val,"localZ");
emit valueChanged(this->uasId,"localZ","m",QVariant(val),getUnixTime());
emit localZChanged(val, "localZ");
emit valueChanged(this->uasId, "localZ", "m", QVariant(val), getUnixTime());
}
double getLocalZ() const
{
......@@ -146,8 +146,8 @@ public:
void setLatitude(double val)
{
latitude = val;
emit latitudeChanged(val,"latitude");
emit valueChanged(this->uasId,"latitude","deg",QVariant(val),getUnixTime());
emit latitudeChanged(val, "latitude");
emit valueChanged(this->uasId, "latitude", "deg", QVariant(val), getUnixTime());
}
double getLatitude() const
......@@ -158,8 +158,8 @@ public:
void setLongitude(double val)
{
longitude = val;
emit longitudeChanged(val,"longitude");
emit valueChanged(this->uasId,"longitude","deg",QVariant(val),getUnixTime());
emit longitudeChanged(val, "longitude");
emit valueChanged(this->uasId, "longitude", "deg", QVariant(val), getUnixTime());
}
double getLongitude() const
......@@ -171,10 +171,10 @@ public:
{
altitudeAMSL = val;
emit altitudeAMSLChanged(val, "altitudeAMSL");
emit valueChanged(this->uasId,"altitudeAMSL","m",QVariant(altitudeAMSL),getUnixTime());
emit valueChanged(this->uasId, "altitudeAMSL", "m", QVariant(altitudeAMSL), getUnixTime());
altitudeAMSLFT = 3.28084 * altitudeAMSL;
emit altitudeAMSLFTChanged(val, "altitudeAMSLFT");
emit valueChanged(this->uasId,"altitudeAMSLFT","m",QVariant(altitudeAMSLFT),getUnixTime());
emit valueChanged(this->uasId, "altitudeAMSLFT", "m", QVariant(altitudeAMSLFT), getUnixTime());
}
double getAltitudeAMSL() const
......@@ -191,7 +191,7 @@ public:
{
altitudeRelative = val;
emit altitudeRelativeChanged(val, "altitudeRelative");
emit valueChanged(this->uasId,"altitudeRelative","m",QVariant(val),getUnixTime());
emit valueChanged(this->uasId, "altitudeRelative", "m", QVariant(val), getUnixTime());
}
double getAltitudeRelative() const
......@@ -217,8 +217,8 @@ public:
void setSatelliteCount(double val)
{
satelliteCount = val;
emit satelliteCountChanged(val,"satelliteCount");
emit valueChanged(this->uasId,"satelliteCount","",QVariant(val),getUnixTime());
emit satelliteCountChanged(val, "satelliteCount");
emit valueChanged(this->uasId, "satelliteCount", "", QVariant(val), getUnixTime());
}
double getSatelliteCount() const
......@@ -234,8 +234,8 @@ public:
void setDistToWaypoint(double val)
{
distToWaypoint = val;
emit distToWaypointChanged(val,"distToWaypoint");
emit valueChanged(this->uasId,"distToWaypoint","m",QVariant(val),getUnixTime());
emit distToWaypointChanged(val, "distToWaypoint");
emit valueChanged(this->uasId, "distToWaypoint", "m", QVariant(val), getUnixTime());
}
double getDistToWaypoint() const
......@@ -246,8 +246,8 @@ public:
void setBearingToWaypoint(double val)
{
bearingToWaypoint = val;
emit bearingToWaypointChanged(val,"bearingToWaypoint");
emit valueChanged(this->uasId,"bearingToWaypoint","deg",QVariant(val),getUnixTime());
emit bearingToWaypointChanged(val, "bearingToWaypoint");
emit valueChanged(this->uasId, "bearingToWaypoint", "deg", QVariant(val), getUnixTime());
}
double getBearingToWaypoint() const
......@@ -259,7 +259,7 @@ public:
void setRoll(double val)
{
roll = val;
emit rollChanged(val,"roll");
emit rollChanged(val, "roll");
}
double getRoll() const
......@@ -270,7 +270,7 @@ public:
void setPitch(double val)
{
pitch = val;
emit pitchChanged(val,"pitch");
emit pitchChanged(val, "pitch");
}
double getPitch() const
......@@ -281,7 +281,7 @@ public:
void setYaw(double val)
{
yaw = val;
emit yawChanged(val,"yaw");
emit yawChanged(val, "yaw");
}
double getYaw() const
......@@ -290,55 +290,68 @@ 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;
}
......@@ -352,7 +365,7 @@ protected: //COMMENTS FOR TEST UNIT
QMap<int, QString> components;///< IDs and names of all detected onboard components
QList<int> unknownPackets; ///< Packet IDs which are unknown and have been received
MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance
MAVLinkProtocol *mavlink; ///< Reference to the MAVLink instance
float receiveDropRate; ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs)
float sendDropRate; ///< Percentage of packets that were not received from the MAV by the GCS
......@@ -450,20 +463,21 @@ protected: //COMMENTS FOR TEST UNIT
/// SIMULATION
#ifndef __mobile__
QGCHilLink* simulation; ///< Hardware in the loop simulation link
QGCHilLink *simulation; ///< Hardware in the loop simulation link
#endif
public:
/** @brief Get the human-readable status message for this code */
void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription);
void getStatusForCode(int statusCode, QString &uasState, QString &stateDescription);
#ifndef __mobile__
virtual FileManager* getFileManager() { return &fileManager; }
virtual FileManager *getFileManager() { return &fileManager; }
#endif
/** @brief Get the HIL simulation */
#ifndef __mobile__
QGCHilLink* getHILSimulation() const {
QGCHilLink *getHILSimulation() const
{
return simulation;
}
#endif
......@@ -480,7 +494,7 @@ public slots:
/** @brief Enable / disable HIL */
#ifndef __mobile__
void enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject * configuration);
void enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject *configuration);
void enableHilJSBSim(bool enable, QString options);
void enableHilXPlane(bool enable);
......@@ -548,45 +562,45 @@ public slots:
/** @brief Send command to disable all bindings/maps between RC and parameters */
void unsetRCToParameterMap();
signals:
void loadChanged(UASInterface* uas, double load);
void loadChanged(UASInterface *uas, double load);
void imageStarted(quint64 timestamp);
/** @brief A new camera image has arrived */
void imageReady(UASInterface* uas);
void imageReady(UASInterface *uas);
/** @brief HIL controls have changed */
void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode);
/** @brief HIL actuator controls (replaces HIL controls) */
void hilActuatorControlsChanged(quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode);
void localXChanged(double val,QString name);
void localYChanged(double val,QString name);
void localZChanged(double val,QString name);
void longitudeChanged(double val,QString name);
void latitudeChanged(double val,QString name);
void altitudeAMSLChanged(double val,QString name);
void altitudeAMSLFTChanged(double val,QString name);
void altitudeRelativeChanged(double val,QString name);
void satRawHDOPChanged (double value);
void satRawVDOPChanged (double value);
void satRawCOGChanged (double value);
void rollChanged(double val,QString name);
void pitchChanged(double val,QString name);
void yawChanged(double val,QString name);
void satelliteCountChanged(double val,QString name);
void distToWaypointChanged(double val,QString name);
void localXChanged(double val, QString name);
void localYChanged(double val, QString name);
void localZChanged(double val, QString name);
void longitudeChanged(double val, QString name);
void latitudeChanged(double val, QString name);
void altitudeAMSLChanged(double val, QString name);
void altitudeAMSLFTChanged(double val, QString name);
void altitudeRelativeChanged(double val, QString name);
void satRawHDOPChanged(double value);
void satRawVDOPChanged(double value);
void satRawCOGChanged(double value);
void rollChanged(double val, QString name);
void pitchChanged(double val, QString name);
void yawChanged(double val, QString name);
void satelliteCountChanged(double val, QString name);
void distToWaypointChanged(double val, QString name);
void groundSpeedChanged(double val, QString name);
void airSpeedChanged(double val, QString name);
void bearingToWaypointChanged(double val,QString name);
void bearingToWaypointChanged(double val, QString name);
protected:
/** @brief Get the UNIX timestamp in milliseconds, enter microseconds */
quint64 getUnixTime(quint64 time=0);
quint64 getUnixTime(quint64 time = 0);
/** @brief Get the UNIX timestamp in milliseconds, enter milliseconds */
quint64 getUnixTimeFromMs(quint64 time);
/** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */
quint64 getUnixReferenceTime(quint64 time);
virtual void processParamValueMsg(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue);
virtual void processParamValueMsg(mavlink_message_t &msg, const QString &paramName, const mavlink_param_value_t &rawValue, mavlink_param_union_t &paramValue);
int componentID[256];
bool componentMulti[256];
......@@ -602,11 +616,11 @@ protected:
quint64 lastSendTimeOpticalFlow; ///< Last HIL Optical Flow message sent
private:
void _say(const QString& text, int severity = 6);
void _say(const QString &text, int severity = 6);
private:
Vehicle* _vehicle;
FirmwarePluginManager* _firmwarePluginManager;
Vehicle *_vehicle;
FirmwarePluginManager *_firmwarePluginManager;
};
......
src/ui/QGCHilXPlaneConfiguration.cc
View file @ 0f035389
......@@ -3,7 +3,7 @@
#include "QGCXPlaneLink.h"
#include "QGCHilConfiguration.h"
QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink* link, QGCHilConfiguration *parent) :
QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink *link, QGCHilConfiguration *parent) :
QWidget(parent),
ui(new Ui::QGCHilXPlaneConfiguration)
{
......@@ -12,7 +12,7 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink* link, QGCHilCon
connect(ui->startButton, &QPushButton::clicked, this, &QGCHilXPlaneConfiguration::toggleSimulation);
connect(ui->hostComboBox, static_cast<void (QComboBox::*)(const QString&)>(&QComboBox::activated),
connect(ui->hostComboBox, static_cast<void (QComboBox::*)(const QString &)>(&QComboBox::activated),
link, &QGCHilLink::setRemoteHost);
connect(link, &QGCHilLink::remoteChanged, ui->hostComboBox, &QComboBox::setEditText);
......@@ -23,7 +23,7 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink* link, QGCHilCon
ui->startButton->setText(tr("Connect"));
QGCXPlaneLink* xplane = dynamic_cast<QGCXPlaneLink*>(link);
QGCXPlaneLink *xplane = dynamic_cast<QGCXPlaneLink *>(link);
if (xplane)
{
......@@ -58,17 +58,20 @@ 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();
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment