Skip to content

Q
qgroundcontrol
Commit ae153170 authored by Bart Slinger's avatar Bart Slinger
Browse files
Options

Revert "fix code style" 

This reverts commit ae976049ca7b4818616e6b8be07a028fc7dfc26f.
parent 0f035389
Show whitespace changes
Inline Side-by-side
Showing with 2808 additions and 3057 deletions
src/comm/QGCXPlaneLink.cc
View file @ ae153170
...@@ -32,7 +32,7 @@ ...@@ -32,7 +32,7 @@
#include "QGCMessageBox.h" #include "QGCMessageBox.h"
#include "HomePositionManager.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), _vehicle(vehicle),
remoteHost(QHostAddress("127.0.0.1")), remoteHost(QHostAddress("127.0.0.1")),
remotePort(49000), remotePort(49000),
...@@ -73,8 +73,7 @@ QGCXPlaneLink::~QGCXPlaneLink() ...@@ -73,8 +73,7 @@ QGCXPlaneLink::~QGCXPlaneLink()
// Tell the thread to exit // Tell the thread to exit
_should_exit = true; _should_exit = true;
if (socket) if (socket) {
{
socket->close(); socket->close();
socket->deleteLater(); socket->deleteLater();
socket = NULL; socket = NULL;
...@@ -107,25 +106,21 @@ void QGCXPlaneLink::storeSettings() ...@@ -107,25 +106,21 @@ void QGCXPlaneLink::storeSettings()
settings.endGroup(); settings.endGroup();
} }
void QGCXPlaneLink::setVersion(const QString &version) void QGCXPlaneLink::setVersion(const QString& version)
{ {
unsigned int oldVersion = xPlaneVersion; unsigned int oldVersion = xPlaneVersion;
if (version.contains("9")) if (version.contains("9"))
{ {
xPlaneVersion = 9; xPlaneVersion = 9;
} }
else if (version.contains("10")) else if (version.contains("10"))
{ {
xPlaneVersion = 10; xPlaneVersion = 10;
} }
else if (version.contains("11")) else if (version.contains("11"))
{ {
xPlaneVersion = 11; xPlaneVersion = 11;
} }
else if (version.contains("12")) else if (version.contains("12"))
{ {
xPlaneVersion = 12; xPlaneVersion = 12;
...@@ -141,23 +136,19 @@ void QGCXPlaneLink::setVersion(unsigned int version) ...@@ -141,23 +136,19 @@ void QGCXPlaneLink::setVersion(unsigned int version)
{ {
bool changed = (xPlaneVersion != version); bool changed = (xPlaneVersion != version);
xPlaneVersion = version; xPlaneVersion = version;
if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion)); if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion));
} }
void QGCXPlaneLink::enableHilActuatorControls(bool enable) void QGCXPlaneLink::enableHilActuatorControls(bool enable)
{ {
if (enable != _useHilActuatorControls) if (enable != _useHilActuatorControls) {
{
_useHilActuatorControls = enable; _useHilActuatorControls = enable;
} }
/* Only use override for new message and specific airframes */ /* Only use override for new message and specific airframes */
MAV_TYPE type = _vehicle->vehicleType(); MAV_TYPE type = _vehicle->vehicleType();
float value = 0.0f; float value = 0.0f;
if (type == MAV_TYPE_VTOL_RESERVED2) {
if (type == MAV_TYPE_VTOL_RESERVED2)
{
value = (enable ? 1.0f : 0.0f); value = (enable ? 1.0f : 0.0f);
} }
...@@ -172,14 +163,12 @@ void QGCXPlaneLink::enableHilActuatorControls(bool enable) ...@@ -172,14 +163,12 @@ void QGCXPlaneLink::enableHilActuatorControls(bool enable)
**/ **/
void QGCXPlaneLink::run() void QGCXPlaneLink::run()
{ {
if (!_vehicle) if (!_vehicle) {
{
emit statusMessage("No MAV present"); emit statusMessage("No MAV present");
return; return;
} }
if (connectState) if (connectState) {
{
emit statusMessage("Already connected"); emit statusMessage("Already connected");
return; return;
} }
...@@ -187,9 +176,7 @@ void QGCXPlaneLink::run() ...@@ -187,9 +176,7 @@ void QGCXPlaneLink::run()
socket = new QUdpSocket(this); socket = new QUdpSocket(this);
socket->moveToThread(this); socket->moveToThread(this);
connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint); connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint);
if (!connectState) {
if (!connectState)
{
emit statusMessage("Binding socket failed!"); emit statusMessage("Binding socket failed!");
...@@ -250,15 +237,14 @@ void QGCXPlaneLink::run() ...@@ -250,15 +237,14 @@ void QGCXPlaneLink::run()
strncpy(ip.str_port_them, localPortStr.toLatin1(), qMin((int)sizeof(ip.str_port_them), 6)); strncpy(ip.str_port_them, localPortStr.toLatin1(), qMin((int)sizeof(ip.str_port_them), 6));
ip.use_ip = 1; 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 */ /* Call function which makes sure individual control override is enabled/disabled */
enableHilActuatorControls(_useHilActuatorControls); enableHilActuatorControls(_useHilActuatorControls);
_should_exit = false; _should_exit = false;
while (!_should_exit) while(!_should_exit) {
{
QCoreApplication::processEvents(); QCoreApplication::processEvents();
QGC::SLEEP::msleep(5); QGC::SLEEP::msleep(5);
} }
...@@ -292,8 +278,7 @@ void QGCXPlaneLink::processError(QProcess::ProcessError err) ...@@ -292,8 +278,7 @@ void QGCXPlaneLink::processError(QProcess::ProcessError err)
{ {
QString msg; QString msg;
switch (err) switch(err) {
{
case QProcess::FailedToStart: case QProcess::FailedToStart:
msg = tr("X-Plane Failed to start. Please check if the path and command is correct"); msg = tr("X-Plane Failed to start. Please check if the path and command is correct");
break; break;
...@@ -329,7 +314,7 @@ QString QGCXPlaneLink::getRemoteHost() ...@@ -329,7 +314,7 @@ QString QGCXPlaneLink::getRemoteHost()
/** /**
* @param newHost Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551 * @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) if (newHost.length() == 0)
return; return;
...@@ -337,13 +322,11 @@ void QGCXPlaneLink::setRemoteHost(const QString &newHost) ...@@ -337,13 +322,11 @@ void QGCXPlaneLink::setRemoteHost(const QString &newHost)
if (newHost.contains(":")) if (newHost.contains(":"))
{ {
QHostInfo info = QHostInfo::fromName(newHost.split(":").first()); QHostInfo info = QHostInfo::fromName(newHost.split(":").first());
if (info.error() == QHostInfo::NoError) if (info.error() == QHostInfo::NoError)
{ {
// Add newHost // Add newHost
QList<QHostAddress> newHostAddresses = info.addresses(); QList<QHostAddress> newHostAddresses = info.addresses();
QHostAddress address; QHostAddress address;
for (int i = 0; i < newHostAddresses.size(); i++) for (int i = 0; i < newHostAddresses.size(); i++)
{ {
// Exclude loopback IPv4 and all IPv6 addresses // Exclude loopback IPv4 and all IPv6 addresses
...@@ -352,22 +335,18 @@ void QGCXPlaneLink::setRemoteHost(const QString &newHost) ...@@ -352,22 +335,18 @@ void QGCXPlaneLink::setRemoteHost(const QString &newHost)
address = newHostAddresses.at(i); address = newHostAddresses.at(i);
} }
} }
remoteHost = address; remoteHost = address;
// Set localPort according to user input // Set localPort according to user input
remotePort = newHost.split(":").last().toInt(); remotePort = newHost.split(":").last().toInt();
} }
} }
else else
{ {
QHostInfo info = QHostInfo::fromName(newHost); QHostInfo info = QHostInfo::fromName(newHost);
if (info.error() == QHostInfo::NoError) if (info.error() == QHostInfo::NoError)
{ {
// Add newHost // Add newHost
remoteHost = info.addresses().first(); remoteHost = info.addresses().first();
if (remotePort == 0) remotePort = 49000; if (remotePort == 0) remotePort = 49000;
} }
} }
...@@ -384,20 +363,17 @@ void QGCXPlaneLink::setRemoteHost(const QString &newHost) ...@@ -384,20 +363,17 @@ void QGCXPlaneLink::setRemoteHost(const QString &newHost)
void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode) void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode)
{ {
/* Only use HIL_CONTROL when the checkbox is unchecked */ /* Only use HIL_CONTROL when the checkbox is unchecked */
if (_useHilActuatorControls) if (_useHilActuatorControls) {
{
//qDebug() << "received HIL_CONTROL but not using it"; //qDebug() << "received HIL_CONTROL but not using it";
return; return;
} }
#pragma pack(push, 1)
#pragma pack(push, 1) struct payload {
struct payload
{
char b[5]; char b[5];
int index; int index;
float f[8]; float f[8];
} p; } p;
#pragma pack(pop) #pragma pack(pop)
p.b[0] = 'D'; p.b[0] = 'D';
p.b[1] = 'A'; p.b[1] = 'A';
...@@ -423,9 +399,8 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch ...@@ -423,9 +399,8 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch
// Direct throttle control // Direct throttle control
p.index = 25; p.index = 25;
writeBytesSafe((const char *)&p, sizeof(p)); writeBytesSafe((const char*)&p, sizeof(p));
} }
else else
{ {
// direct pass-through, normal fixed-wing. // direct pass-through, normal fixed-wing.
...@@ -437,11 +412,11 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch ...@@ -437,11 +412,11 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch
// Send to group 12 // Send to group 12
p.index = 12; p.index = 12;
writeBytesSafe((const char *)&p, sizeof(p)); writeBytesSafe((const char*)&p, sizeof(p));
// Send to group 8, which equals manual controls // Send to group 8, which equals manual controls
p.index = 8; p.index = 8;
writeBytesSafe((const char *)&p, sizeof(p)); writeBytesSafe((const char*)&p, sizeof(p));
// Send throttle to all four motors // Send throttle to all four motors
p.index = 25; p.index = 25;
...@@ -450,14 +425,13 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch ...@@ -450,14 +425,13 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch
p.f[1] = throttle; p.f[1] = throttle;
p.f[2] = throttle; p.f[2] = throttle;
p.f[3] = 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) void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode)
{ {
if (!_useHilActuatorControls) if (!_useHilActuatorControls) {
{
//qDebug() << "received HIL_ACTUATOR_CONTROLS but not using it"; //qDebug() << "received HIL_ACTUATOR_CONTROLS but not using it";
return; return;
} }
...@@ -470,14 +444,13 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct ...@@ -470,14 +444,13 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
Q_UNUSED(ctl_14); Q_UNUSED(ctl_14);
Q_UNUSED(ctl_15); Q_UNUSED(ctl_15);
#pragma pack(push, 1) #pragma pack(push, 1)
struct payload struct payload {
{
char b[5]; char b[5];
int index; int index;
float f[8]; float f[8];
} p; } p;
#pragma pack(pop) #pragma pack(pop)
p.b[0] = 'D'; p.b[0] = 'D';
p.b[1] = 'A'; p.b[1] = 'A';
...@@ -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 */ /* Initialize with zeroes */
memset(p.f, 0, sizeof(p.f)); memset(p.f, 0, sizeof(p.f));
switch (_vehicle->vehicleType()) switch (_vehicle->vehicleType()) {
{
case MAV_TYPE_QUADROTOR: case MAV_TYPE_QUADROTOR:
case MAV_TYPE_HEXAROTOR: case MAV_TYPE_HEXAROTOR:
case MAV_TYPE_OCTOROTOR: case MAV_TYPE_OCTOROTOR:
...@@ -505,10 +477,9 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct ...@@ -505,10 +477,9 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
/* Direct throttle control */ /* Direct throttle control */
p.index = 25; p.index = 25;
writeBytesSafe((const char *)&p, sizeof(p)); writeBytesSafe((const char*)&p, sizeof(p));
break; break;
} }
case MAV_TYPE_VTOL_RESERVED2: case MAV_TYPE_VTOL_RESERVED2:
{ {
/** /**
...@@ -525,7 +496,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct ...@@ -525,7 +496,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
p.f[6] = ctl_6; p.f[6] = ctl_6;
p.f[7] = ctl_7; p.f[7] = ctl_7;
p.index = 25; p.index = 25;
writeBytesSafe((const char *)&p, sizeof(p)); writeBytesSafe((const char*)&p, sizeof(p));
/* Control individual actuators */ /* Control individual actuators */
float max_surface_deflection = 30.0f; // Degrees float max_surface_deflection = 30.0f; // Degrees
...@@ -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; break;
} }
default: default:
{ {
/* direct pass-through, normal fixed-wing. */ /* direct pass-through, normal fixed-wing. */
...@@ -546,7 +516,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct ...@@ -546,7 +516,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
/* Send to group 8, which equals manual controls */ /* Send to group 8, which equals manual controls */
p.index = 8; p.index = 8;
writeBytesSafe((const char *)&p, sizeof(p)); writeBytesSafe((const char*)&p, sizeof(p));
/* Send throttle to all eight motors */ /* Send throttle to all eight motors */
p.index = 25; p.index = 25;
...@@ -558,7 +528,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct ...@@ -558,7 +528,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct
p.f[5] = ctl_3; p.f[5] = ctl_3;
p.f[6] = ctl_3; p.f[6] = ctl_3;
p.f[7] = ctl_3; p.f[7] = ctl_3;
writeBytesSafe((const char *)&p, sizeof(p)); writeBytesSafe((const char*)&p, sizeof(p));
break; break;
} }
...@@ -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(yaw);
double _c_ = cos(pitch); double _c_ = cos(pitch);
double __c = cos(roll); double __c = cos(roll);
...@@ -590,8 +559,8 @@ Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) ...@@ -590,8 +559,8 @@ Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll)
double sss = ss_ * __s; double sss = ss_ * __s;
Eigen::Matrix3f wRo; Eigen::Matrix3f wRo;
wRo << wRo <<
cc_ , css - s_c, csc + s_s, cc_ , css-s_c, csc+s_s,
sc_ , sss + c_c, ssc - c_s, sc_ , sss+c_c, ssc-c_s,
-_s_ , _cs, _cc; -_s_ , _cs, _cc;
return wRo; return wRo;
} }
...@@ -622,30 +591,25 @@ void QGCXPlaneLink::readBytes() ...@@ -622,30 +591,25 @@ void QGCXPlaneLink::readBytes()
quint16 senderPort; quint16 senderPort;
unsigned int s = socket->pendingDatagramSize(); unsigned int s = socket->pendingDatagramSize();
if (s > maxLength) std::cerr << __FILE__ << __LINE__ << " UDP datagram overflow, allowed to read less bytes than datagram size: " << s << std::endl; if (s > maxLength) std::cerr << __FILE__ << __LINE__ << " UDP datagram overflow, allowed to read less bytes than datagram size: " << s << std::endl;
socket->readDatagram(data, maxLength, &sender, &senderPort); socket->readDatagram(data, maxLength, &sender, &senderPort);
if (s > maxLength) {
if (s > maxLength) std::string headStr = std::string(data, data+5);
{
std::string headStr = std::string(data, data + 5);
std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl; std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl;
} }
// Calculate the number of data segments a 36 bytes // Calculate the number of data segments a 36 bytes
// XPlane always has 5 bytes header: 'DATA@' // XPlane always has 5 bytes header: 'DATA@'
unsigned nsegs = (s - 5) / 36; unsigned nsegs = (s-5)/36;
//qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs; //qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs;
#pragma pack(push, 1) #pragma pack(push, 1)
struct payload struct payload {
{
int index; int index;
float f[8]; float f[8];
} p; } p;
#pragma pack(pop) #pragma pack(pop)
bool oldConnectionState = xPlaneConnected; bool oldConnectionState = xPlaneConnected;
...@@ -656,16 +620,15 @@ void QGCXPlaneLink::readBytes() ...@@ -656,16 +620,15 @@ void QGCXPlaneLink::readBytes()
{ {
xPlaneConnected = true; xPlaneConnected = true;
if (oldConnectionState != xPlaneConnected) if (oldConnectionState != xPlaneConnected) {
{
simUpdateFirst = QGC::groundTimeMilliseconds(); simUpdateFirst = QGC::groundTimeMilliseconds();
} }
for (unsigned i = 0; i < nsegs; i++) for (unsigned i = 0; i < nsegs; i++)
{ {
// Get index // Get index
unsigned ioff = (5 + i * 36);; unsigned ioff = (5+i*36);;
memcpy(&(p), data + ioff, sizeof(p)); memcpy(&(p), data+ioff, sizeof(p));
if (p.index == 3) if (p.index == 3)
{ {
...@@ -675,14 +638,13 @@ void QGCXPlaneLink::readBytes() ...@@ -675,14 +638,13 @@ void QGCXPlaneLink::readBytes()
//qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s"; //qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s";
} }
if (p.index == 4) if (p.index == 4)
{ {
// WORKAROUND: IF ground speed <<1m/s and altitude-above-ground <1m, do NOT use the X-Plane data, because X-Plane (tested // WORKAROUND: IF ground speed <<1m/s and altitude-above-ground <1m, do NOT use the X-Plane data, because X-Plane (tested
// with v10.3 and earlier) delivers yacc=0 and zacc=0 when the ground speed is very low, which gives e.g. wrong readings // 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. // before launch when waiting on the runway. This might pose a problem for initial state estimation/calibration.
// Instead, we calculate our own accelerations. // 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. // TODO: Add centrip. acceleration to the current static acceleration implementation.
Eigen::Vector3f g(0, 0, -9.80665f); Eigen::Vector3f g(0, 0, -9.80665f);
...@@ -695,7 +657,6 @@ void QGCXPlaneLink::readBytes() ...@@ -695,7 +657,6 @@ void QGCXPlaneLink::readBytes()
//qDebug() << "Calculated values" << gr[0] << gr[1] << gr[2]; //qDebug() << "Calculated values" << gr[0] << gr[1] << gr[2];
} }
else else
{ {
// Accelerometer readings, directly from X-Plane and including centripetal forces. // Accelerometer readings, directly from X-Plane and including centripetal forces.
...@@ -710,7 +671,6 @@ void QGCXPlaneLink::readBytes() ...@@ -710,7 +671,6 @@ void QGCXPlaneLink::readBytes()
fields_changed |= (1 << 0) | (1 << 1) | (1 << 2); fields_changed |= (1 << 0) | (1 << 1) | (1 << 2);
emitUpdate = true; emitUpdate = true;
} }
// atmospheric pressure aircraft for XPlane 9 and 10 // atmospheric pressure aircraft for XPlane 9 and 10
else if (p.index == 6) else if (p.index == 6)
{ {
...@@ -719,7 +679,6 @@ void QGCXPlaneLink::readBytes() ...@@ -719,7 +679,6 @@ void QGCXPlaneLink::readBytes()
temperature = p.f[1]; temperature = p.f[1];
fields_changed |= (1 << 9) | (1 << 12); fields_changed |= (1 << 9) | (1 << 12);
} }
// Forward controls from X-Plane to MAV, not very useful // Forward controls from X-Plane to MAV, not very useful
// better: Connect Joystick to QGroundControl // better: Connect Joystick to QGroundControl
// else if (p.index == 8) // else if (p.index == 8)
...@@ -741,7 +700,6 @@ void QGCXPlaneLink::readBytes() ...@@ -741,7 +700,6 @@ void QGCXPlaneLink::readBytes()
emitUpdate = true; emitUpdate = true;
} }
else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18)) else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18))
{ {
//qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3]; //qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
...@@ -750,25 +708,19 @@ void QGCXPlaneLink::readBytes() ...@@ -750,25 +708,19 @@ void QGCXPlaneLink::readBytes()
yaw = p.f[2] / 180.0f * M_PI; yaw = p.f[2] / 180.0f * M_PI;
// X-Plane expresses yaw as 0..2 PI // X-Plane expresses yaw as 0..2 PI
if (yaw > M_PI) if (yaw > M_PI) {
{
yaw -= 2.0f * static_cast<float>(M_PI); yaw -= 2.0f * static_cast<float>(M_PI);
} }
if (yaw < -M_PI) {
if (yaw < -M_PI)
{
yaw += 2.0f * static_cast<float>(M_PI); yaw += 2.0f * static_cast<float>(M_PI);
} }
float yawmag = p.f[3] / 180.0f * M_PI; float yawmag = p.f[3] / 180.0f * M_PI;
if (yawmag > M_PI) if (yawmag > M_PI) {
{
yawmag -= 2.0f * static_cast<float>(M_PI); yawmag -= 2.0f * static_cast<float>(M_PI);
} }
if (yawmag < -M_PI) {
if (yawmag < -M_PI)
{
yawmag += 2.0f * static_cast<float>(M_PI); yawmag += 2.0f * static_cast<float>(M_PI);
} }
...@@ -802,7 +754,7 @@ void QGCXPlaneLink::readBytes() ...@@ -802,7 +754,7 @@ void QGCXPlaneLink::readBytes()
dcm[2][1] = sinPhi * cosThe; dcm[2][1] = sinPhi * cosThe;
dcm[2][2] = cosPhi * 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); Eigen::Vector3f mag(xmag, ymag, zmag);
...@@ -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 = p.f[2] * 0.3048f; // convert feet (MSL) to meters
alt_agl = p.f[3] * 0.3048f; //convert feet (AGL) to meters alt_agl = p.f[3] * 0.3048f; //convert feet (AGL) to meters
} }
else if (p.index == 21) else if (p.index == 21)
{ {
vy = p.f[3]; vy = p.f[3];
...@@ -842,7 +793,6 @@ void QGCXPlaneLink::readBytes() ...@@ -842,7 +793,6 @@ void QGCXPlaneLink::readBytes()
// for us. // for us.
vz = -p.f[4]; vz = -p.f[4];
} }
else if (p.index == 12) else if (p.index == 12)
{ {
//qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2]; //qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2];
...@@ -885,8 +835,7 @@ void QGCXPlaneLink::readBytes() ...@@ -885,8 +835,7 @@ void QGCXPlaneLink::readBytes()
} }
// Wait for 0.5s before actually using the data, so that all fields are filled // Wait for 0.5s before actually using the data, so that all fields are filled
if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) {
{
return; return;
} }
...@@ -894,9 +843,7 @@ void QGCXPlaneLink::readBytes() ...@@ -894,9 +843,7 @@ void QGCXPlaneLink::readBytes()
if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 2) if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 2)
{ {
simUpdateHz = simUpdateHz * 0.9f + 0.1f * (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); simUpdateHz = simUpdateHz * 0.9f + 0.1f * (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast));
if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000) {
if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000)
{
emit statusMessage(tr("Receiving from XPlane at %1 Hz").arg(static_cast<int>(simUpdateHz))); emit statusMessage(tr("Receiving from XPlane at %1 Hz").arg(static_cast<int>(simUpdateHz)));
// Reset lowpass with current value // Reset lowpass with current value
simUpdateHz = (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); simUpdateHz = (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast));
...@@ -943,23 +890,19 @@ void QGCXPlaneLink::readBytes() ...@@ -943,23 +890,19 @@ void QGCXPlaneLink::readBytes()
int gps_fix_type = 3; int gps_fix_type = 3;
float eph = 0.3f; float eph = 0.3f;
float epv = 0.6f; 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); float cog = atan2(vy, vx);
int satellites = 8; int satellites = 8;
emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites); emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites);
} } else {
else
{
emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
pitchspeed, yawspeed, lat, lon, alt, pitchspeed, yawspeed, lat, lon, alt,
vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc); vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
} }
// Limit ground truth to 25 Hz // Limit ground truth to 25 Hz
if (QGC::groundTimeMilliseconds() - simUpdateLastGroundTruth > 40) if (QGC::groundTimeMilliseconds() - simUpdateLastGroundTruth > 40) {
{
emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed,
pitchspeed, yawspeed, lat, lon, alt, pitchspeed, yawspeed, lat, lon, alt,
vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc); vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc);
...@@ -1005,10 +948,7 @@ bool QGCXPlaneLink::disconnectSimulation() ...@@ -1005,10 +948,7 @@ bool QGCXPlaneLink::disconnectSimulation()
if (connectState) if (connectState)
{ {
_should_exit = true; _should_exit = true;
} } else {
else
{
emit simulationDisconnected(); emit simulationDisconnected();
emit simulationConnected(false); emit simulationConnected(false);
} }
...@@ -1016,7 +956,7 @@ bool QGCXPlaneLink::disconnectSimulation() ...@@ -1016,7 +956,7 @@ bool QGCXPlaneLink::disconnectSimulation()
return !connectState; return !connectState;
} }
void QGCXPlaneLink::selectAirframe(const QString &plane) void QGCXPlaneLink::selectAirframe(const QString& plane)
{ {
airframeName = plane; airframeName = plane;
...@@ -1027,34 +967,29 @@ void QGCXPlaneLink::selectAirframe(const QString &plane) ...@@ -1027,34 +967,29 @@ void QGCXPlaneLink::selectAirframe(const QString &plane)
airframeID = AIRFRAME_QUAD_X_MK_10INCH_I2C; airframeID = AIRFRAME_QUAD_X_MK_10INCH_I2C;
emit airframeChanged("QRO_X / MK"); emit airframeChanged("QRO_X / MK");
} }
else if (plane.contains("ARDRONE") && airframeID != AIRFRAME_QUAD_X_ARDRONE) else if (plane.contains("ARDRONE") && airframeID != AIRFRAME_QUAD_X_ARDRONE)
{ {
airframeID = AIRFRAME_QUAD_X_ARDRONE; airframeID = AIRFRAME_QUAD_X_ARDRONE;
emit airframeChanged("QRO_X / ARDRONE"); emit airframeChanged("QRO_X / ARDRONE");
} }
else else
{ {
bool changed = (airframeID != AIRFRAME_QUAD_DJI_F450_PWM); bool changed = (airframeID != AIRFRAME_QUAD_DJI_F450_PWM);
airframeID = AIRFRAME_QUAD_DJI_F450_PWM; airframeID = AIRFRAME_QUAD_DJI_F450_PWM;
if (changed) emit airframeChanged("QRO_X / DJI-F450 / PWM"); if (changed) emit airframeChanged("QRO_X / DJI-F450 / PWM");
} }
} }
else else
{ {
bool changed = (airframeID != AIRFRAME_UNKNOWN); bool changed = (airframeID != AIRFRAME_UNKNOWN);
airframeID = AIRFRAME_UNKNOWN; airframeID = AIRFRAME_UNKNOWN;
if (changed) emit airframeChanged("X Plane default"); if (changed) emit airframeChanged("X Plane default");
} }
} }
void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw) 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 struct VEH1_struct
{ {
char header[5]; char header[5];
...@@ -1063,7 +998,7 @@ void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, doub ...@@ -1063,7 +998,7 @@ void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, doub
float psi_the_phi[3]; float psi_the_phi[3];
float gear_flap_vect[3]; float gear_flap_vect[3];
} pos; } pos;
#pragma pack(pop) #pragma pack(pop)
pos.header[0] = 'V'; pos.header[0] = 'V';
pos.header[1] = 'E'; pos.header[1] = 'E';
...@@ -1081,7 +1016,7 @@ void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, doub ...@@ -1081,7 +1016,7 @@ void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, doub
pos.gear_flap_vect[1] = 0.0f; pos.gear_flap_vect[1] = 0.0f;
pos.gear_flap_vect[2] = 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[0] = 'V';
// pos.header[1] = 'E'; // pos.header[1] = 'E';
...@@ -1111,8 +1046,8 @@ void QGCXPlaneLink::setRandomPosition() ...@@ -1111,8 +1046,8 @@ void QGCXPlaneLink::setRandomPosition()
// Initialize generator // Initialize generator
srand(0); srand(0);
double offLat = 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 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; double offAlt = rand() / static_cast<double>(RAND_MAX) * 200.0 + 100.0;
if (_vehicle->altitudeAMSL()->rawValue().toDouble() + offAlt < 0) if (_vehicle->altitudeAMSL()->rawValue().toDouble() + offAlt < 0)
...@@ -1152,13 +1087,9 @@ void QGCXPlaneLink::setRandomAttitude() ...@@ -1152,13 +1087,9 @@ void QGCXPlaneLink::setRandomAttitude()
**/ **/
bool QGCXPlaneLink::connectSimulation() bool QGCXPlaneLink::connectSimulation()
{ {
if (connectState) if (connectState) {
{
qDebug() << "Simulation already active"; qDebug() << "Simulation already active";
} } else {
else
{
qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort; qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort;
// XXX Hack // XXX Hack
storeSettings(); storeSettings();
...@@ -1192,14 +1123,13 @@ void QGCXPlaneLink::setName(QString name) ...@@ -1192,14 +1123,13 @@ void QGCXPlaneLink::setName(QString name)
void QGCXPlaneLink::sendDataRef(QString ref, float value) void QGCXPlaneLink::sendDataRef(QString ref, float value)
{ {
#pragma pack(push, 1) #pragma pack(push, 1)
struct payload struct payload {
{
char b[5]; char b[5];
float value; float value;
char name[500]; char name[500];
} dref; } dref;
#pragma pack(pop) #pragma pack(pop)
dref.b[0] = 'D'; dref.b[0] = 'D';
dref.b[1] = 'R'; dref.b[1] = 'R';
...@@ -1217,16 +1147,12 @@ void QGCXPlaneLink::sendDataRef(QString ref, float value) ...@@ -1217,16 +1147,12 @@ void QGCXPlaneLink::sendDataRef(QString ref, float value)
/* Send command */ /* Send command */
QByteArray ba = ref.toUtf8(); QByteArray ba = ref.toUtf8();
if (ba.length() > 500) {
if (ba.length() > 500)
{
return; return;
} }
for (int i = 0; i < ba.length(); i++) for (int i = 0; i < ba.length(); i++) {
{
dref.name[i] = ba.at(i); dref.name[i] = ba.at(i);
} }
writeBytesSafe((const char*)&dref, sizeof(dref));
writeBytesSafe((const char *)&dref, sizeof(dref));
} }
src/comm/QGCXPlaneLink.h
View file @ ae153170
...@@ -36,7 +36,7 @@ class QGCXPlaneLink : public QGCHilLink ...@@ -36,7 +36,7 @@ class QGCXPlaneLink : public QGCHilLink
//Q_INTERFACES(QGCXPlaneLinkInterface:LinkInterface) //Q_INTERFACES(QGCXPlaneLinkInterface:LinkInterface)
public: 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(); ~QGCXPlaneLink();
/** /**
...@@ -51,8 +51,7 @@ public: ...@@ -51,8 +51,7 @@ public:
bool isConnected(); bool isConnected();
qint64 bytesAvailable(); qint64 bytesAvailable();
int getPort() const int getPort() const {
{
return localPort; return localPort;
} }
...@@ -89,13 +88,11 @@ public: ...@@ -89,13 +88,11 @@ public:
return (int)airframeID; return (int)airframeID;
} }
bool sensorHilEnabled() bool sensorHilEnabled() {
{
return _sensorHilEnabled; return _sensorHilEnabled;
} }
bool useHilActuatorControls() bool useHilActuatorControls() {
{
return _useHilActuatorControls; return _useHilActuatorControls;
} }
...@@ -107,7 +104,7 @@ public slots: ...@@ -107,7 +104,7 @@ public slots:
// void setAddress(QString address); // void setAddress(QString address);
void setPort(int port); void setPort(int port);
/** @brief Add a new host to broadcast messages to */ /** @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 */ /** @brief Send new control states to the simulation */
void updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode); void updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode);
/** @brief Send new control commands to the simulation */ /** @brief Send new control commands to the simulation */
...@@ -130,15 +127,13 @@ public slots: ...@@ -130,15 +127,13 @@ public slots:
float ctl_15, float ctl_15,
quint8 mode); quint8 mode);
/** @brief Set the simulator version as text string */ /** @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 */ /** @brief Set the simulator version as integer */
void setVersion(unsigned int version); void setVersion(unsigned int version);
void enableSensorHIL(bool enable) void enableSensorHIL(bool enable) {
{
if (enable != _sensorHilEnabled) if (enable != _sensorHilEnabled)
_sensorHilEnabled = enable; _sensorHilEnabled = enable;
emit sensorHilChanged(enable); emit sensorHilChanged(enable);
} }
...@@ -164,7 +159,7 @@ public slots: ...@@ -164,7 +159,7 @@ public slots:
* @brief Select airplane model * @brief Select airplane model
* @param plane the name of the airplane * @param plane the name of the airplane
*/ */
void selectAirframe(const QString &airframe); void selectAirframe(const QString& airframe);
/** /**
* @brief Set the airplane position and attitude * @brief Set the airplane position and attitude
* @param lat * @param lat
...@@ -187,14 +182,14 @@ public slots: ...@@ -187,14 +182,14 @@ public slots:
void setRandomAttitude(); void setRandomAttitude();
protected: protected:
Vehicle *_vehicle; Vehicle* _vehicle;
QString name; QString name;
QHostAddress localHost; QHostAddress localHost;
quint16 localPort; quint16 localPort;
QHostAddress remoteHost; QHostAddress remoteHost;
quint16 remotePort; quint16 remotePort;
int id; int id;
QUdpSocket *socket; QUdpSocket* socket;
bool connectState; bool connectState;
quint64 bitsSentTotal; quint64 bitsSentTotal;
...@@ -207,8 +202,8 @@ protected: ...@@ -207,8 +202,8 @@ protected:
QMutex statisticsMutex; QMutex statisticsMutex;
QMutex dataMutex; QMutex dataMutex;
QTimer refreshTimer; QTimer refreshTimer;
QProcess *process; QProcess* process;
QProcess *terraSync; QProcess* terraSync;
bool gpsReceived; bool gpsReceived;
bool attitudeReceived; bool attitudeReceived;
......
src/uas/UAS.cc
View file @ ae153170
...@@ -54,7 +54,7 @@ QGC_LOGGING_CATEGORY(UASLog, "UASLog") ...@@ -54,7 +54,7 @@ QGC_LOGGING_CATEGORY(UASLog, "UASLog")
* creating the UAS. * creating the UAS.
*/ */
UAS::UAS(MAVLinkProtocol *protocol, Vehicle *vehicle, FirmwarePluginManager *firmwarePluginManager) : UASInterface(), UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * firmwarePluginManager) : UASInterface(),
lipoFull(4.2f), lipoFull(4.2f),
lipoEmpty(3.5f), lipoEmpty(3.5f),
uasId(vehicle->id()), uasId(vehicle->id()),
...@@ -167,7 +167,7 @@ UAS::UAS(MAVLinkProtocol *protocol, Vehicle *vehicle, FirmwarePluginManager *fir ...@@ -167,7 +167,7 @@ UAS::UAS(MAVLinkProtocol *protocol, Vehicle *vehicle, FirmwarePluginManager *fir
_firmwarePluginManager(firmwarePluginManager) _firmwarePluginManager(firmwarePluginManager)
{ {
for (unsigned int i = 0; i < 255; ++i) for (unsigned int i = 0; i<255;++i)
{ {
componentID[i] = -1; componentID[i] = -1;
componentMulti[i] = false; componentMulti[i] = false;
...@@ -201,19 +201,16 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -201,19 +201,16 @@ void UAS::receiveMessage(mavlink_message_t message)
componentName = "ANONYMOUS"; componentName = "ANONYMOUS";
break; break;
} }
case MAV_COMP_ID_IMU: case MAV_COMP_ID_IMU:
{ {
componentName = "IMU #1"; componentName = "IMU #1";
break; break;
} }
case MAV_COMP_ID_CAMERA: case MAV_COMP_ID_CAMERA:
{ {
componentName = "CAMERA"; componentName = "CAMERA";
break; break;
} }
case MAV_COMP_ID_MISSIONPLANNER: case MAV_COMP_ID_MISSIONPLANNER:
{ {
componentName = "MISSIONPLANNER"; componentName = "MISSIONPLANNER";
...@@ -243,7 +240,6 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -243,7 +240,6 @@ void UAS::receiveMessage(mavlink_message_t message)
// Prefer IMU 2 over IMU 1 (FIXME) // Prefer IMU 2 over IMU 1 (FIXME)
componentID[message.msgid] = MAV_COMP_ID_IMU_2; componentID[message.msgid] = MAV_COMP_ID_IMU_2;
break; break;
default: default:
// Do nothing // Do nothing
break; break;
...@@ -255,7 +251,6 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -255,7 +251,6 @@ void UAS::receiveMessage(mavlink_message_t message)
// Prefer the first component // Prefer the first component
componentID[message.msgid] = message.compid; componentID[message.msgid] = message.compid;
} }
else else
{ {
// Got this message already // Got this message already
...@@ -277,7 +272,6 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -277,7 +272,6 @@ void UAS::receiveMessage(mavlink_message_t message)
{ {
break; break;
} }
mavlink_heartbeat_t state; mavlink_heartbeat_t state;
mavlink_msg_heartbeat_decode(&message, &state); mavlink_msg_heartbeat_decode(&message, &state);
...@@ -309,7 +303,6 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -309,7 +303,6 @@ void UAS::receiveMessage(mavlink_message_t message)
{ {
break; break;
} }
mavlink_sys_status_t state; mavlink_sys_status_t state;
mavlink_msg_sys_status_decode(&message, &state); mavlink_msg_sys_status_decode(&message, &state);
...@@ -325,8 +318,8 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -325,8 +318,8 @@ void UAS::receiveMessage(mavlink_message_t message)
emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time); emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time);
// Process CPU load. // Process CPU load.
emit loadChanged(this, state.load / 10.0f); emit loadChanged(this,state.load/10.0f);
emit valueChanged(uasId, name.arg("load"), "%", state.load / 10.0f, time); emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time);
// control_sensors_enabled: // control_sensors_enabled:
// relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control // relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control
...@@ -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; state.drop_rate_comm = 10000;
} }
emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f);
emit dropRateChanged(this->getUASID(), state.drop_rate_comm / 100.0f); emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time);
emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm / 100.0f, time);
} }
break; break;
case MAVLINK_MSG_ID_ATTITUDE: case MAVLINK_MSG_ID_ATTITUDE:
{ {
mavlink_attitude_t attitude; mavlink_attitude_t attitude;
...@@ -370,7 +361,6 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -370,7 +361,6 @@ void UAS::receiveMessage(mavlink_message_t message)
} }
} }
break; break;
case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: case MAVLINK_MSG_ID_ATTITUDE_QUATERNION:
{ {
mavlink_attitude_quaternion_t attitude; mavlink_attitude_quaternion_t attitude;
...@@ -400,24 +390,17 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -400,24 +390,17 @@ void UAS::receiveMessage(mavlink_message_t message)
float phi, theta, psi; float phi, theta, psi;
theta = asin(-dcm[2][0]); theta = asin(-dcm[2][0]);
if (fabs(theta - M_PI_2) < 1.0e-3f) if (fabs(theta - M_PI_2) < 1.0e-3f) {
{
phi = 0.0f; phi = 0.0f;
psi = (atan2(dcm[1][2] - dcm[0][1], psi = (atan2(dcm[1][2] - dcm[0][1],
dcm[0][2] + dcm[1][1]) + phi); dcm[0][2] + dcm[1][1]) + phi);
} } else if (fabs(theta + M_PI_2) < 1.0e-3f) {
else if (fabs(theta + M_PI_2) < 1.0e-3f)
{
phi = 0.0f; phi = 0.0f;
psi = atan2f(dcm[1][2] - dcm[0][1], psi = atan2f(dcm[1][2] - dcm[0][1],
dcm[0][2] + dcm[1][1] - phi); dcm[0][2] + dcm[1][1] - phi);
} } else {
else
{
phi = atan2f(dcm[2][1], dcm[2][2]); phi = atan2f(dcm[2][1], dcm[2][2]);
psi = atan2f(dcm[1][0], dcm[0][0]); psi = atan2f(dcm[1][0], dcm[0][0]);
} }
...@@ -439,7 +422,6 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -439,7 +422,6 @@ void UAS::receiveMessage(mavlink_message_t message)
} }
} }
break; break;
case MAVLINK_MSG_ID_HIL_CONTROLS: case MAVLINK_MSG_ID_HIL_CONTROLS:
{ {
mavlink_hil_controls_t hil; mavlink_hil_controls_t hil;
...@@ -447,7 +429,6 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -447,7 +429,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode); emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode);
} }
break; break;
case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS: case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS:
{ {
mavlink_hil_actuator_controls_t hil; mavlink_hil_actuator_controls_t hil;
...@@ -472,33 +453,29 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -472,33 +453,29 @@ void UAS::receiveMessage(mavlink_message_t message)
hil.mode); hil.mode);
} }
break; break;
case MAVLINK_MSG_ID_VFR_HUD: case MAVLINK_MSG_ID_VFR_HUD:
{ {
mavlink_vfr_hud_t hud; mavlink_vfr_hud_t hud;
mavlink_msg_vfr_hud_decode(&message, &hud); mavlink_msg_vfr_hud_decode(&message, &hud);
quint64 time = getUnixTime(); quint64 time = getUnixTime();
// Display updated values // Display updated values
emit thrustChanged(this, hud.throttle / 100.0); emit thrustChanged(this, hud.throttle/100.0);
if (!attitudeKnown) 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); emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
} }
setAltitudeAMSL(hud.alt); setAltitudeAMSL(hud.alt);
setGroundSpeed(hud.groundspeed); setGroundSpeed(hud.groundspeed);
if (!qIsNaN(hud.airspeed)) if (!qIsNaN(hud.airspeed))
setAirSpeed(hud.airspeed); setAirSpeed(hud.airspeed);
speedZ = -hud.climb; speedZ = -hud.climb;
emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time);
emit speedChanged(this, groundSpeed, airSpeed, time); emit speedChanged(this, groundSpeed, airSpeed, time);
} }
break; break;
case MAVLINK_MSG_ID_LOCAL_POSITION_NED: case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
//std::cerr << std::endl; //std::cerr << std::endl;
//std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
...@@ -518,7 +495,6 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -518,7 +495,6 @@ void UAS::receiveMessage(mavlink_message_t message)
} }
} }
break; break;
case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE: case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
{ {
mavlink_global_vision_position_estimate_t pos; mavlink_global_vision_position_estimate_t pos;
...@@ -527,7 +503,6 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -527,7 +503,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time); emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time);
} }
break; break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
//std::cerr << std::endl; //std::cerr << std::endl;
//std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
...@@ -537,28 +512,27 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -537,28 +512,27 @@ void UAS::receiveMessage(mavlink_message_t message)
quint64 time = getUnixTime(); quint64 time = getUnixTime();
setLatitude(pos.lat / (double)1E7); setLatitude(pos.lat/(double)1E7);
setLongitude(pos.lon / (double)1E7); setLongitude(pos.lon/(double)1E7);
setAltitudeRelative(pos.relative_alt / 1000.0); setAltitudeRelative(pos.relative_alt/1000.0);
globalEstimatorActive = true; globalEstimatorActive = true;
speedX = pos.vx / 100.0; speedX = pos.vx/100.0;
speedY = pos.vy / 100.0; speedY = pos.vy/100.0;
speedZ = pos.vz / 100.0; speedZ = pos.vz/100.0;
emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time); emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time);
emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time);
// We had some frame mess here, global and local axes were mixed. // We had some frame mess here, global and local axes were mixed.
emit velocityChanged_NED(this, speedX, speedY, speedZ, time); 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); emit speedChanged(this, groundSpeed, airSpeed, time);
isGlobalPositionKnown = true; isGlobalPositionKnown = true;
} }
break; break;
case MAVLINK_MSG_ID_GPS_RAW_INT: case MAVLINK_MSG_ID_GPS_RAW_INT:
{ {
mavlink_gps_raw_int_t pos; mavlink_gps_raw_int_t pos;
...@@ -568,41 +542,33 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -568,41 +542,33 @@ void UAS::receiveMessage(mavlink_message_t message)
// TODO: track localization state not only for gps but also for other loc. sources // TODO: track localization state not only for gps but also for other loc. sources
int loc_type = pos.fix_type; int loc_type = pos.fix_type;
if (loc_type == 1) if (loc_type == 1)
{ {
loc_type = 0; loc_type = 0;
} }
setSatelliteCount(pos.satellites_visible); setSatelliteCount(pos.satellites_visible);
if (pos.fix_type > 2) if (pos.fix_type > 2)
{ {
isGlobalPositionKnown = true; isGlobalPositionKnown = true;
latitude_gps = pos.lat / (double)1E7; latitude_gps = pos.lat/(double)1E7;
longitude_gps = pos.lon / (double)1E7; longitude_gps = pos.lon/(double)1E7;
altitude_gps = pos.alt / 1000.0; altitude_gps = pos.alt/1000.0;
// If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead. // If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead.
if (!globalEstimatorActive) if (!globalEstimatorActive) {
{
setLatitude(latitude_gps); setLatitude(latitude_gps);
setLongitude(longitude_gps); setLongitude(longitude_gps);
emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time); emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time);
emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time);
float vel = pos.vel / 100.0f; float vel = pos.vel/100.0f;
// Smaller than threshold and not NaN // Smaller than threshold and not NaN
if ((vel < 1000000) && !qIsNaN(vel) && !qIsInf(vel)) if ((vel < 1000000) && !qIsNaN(vel) && !qIsInf(vel)) {
{
setGroundSpeed(vel); setGroundSpeed(vel);
emit speedChanged(this, groundSpeed, airSpeed, time); emit speedChanged(this, groundSpeed, airSpeed, time);
} } else {
else
{
emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_NOTICE, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel)); emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_NOTICE, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel));
} }
} }
...@@ -611,24 +577,19 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -611,24 +577,19 @@ void UAS::receiveMessage(mavlink_message_t message)
double dtmp; double dtmp;
//-- Raw GPS data //-- Raw GPS data
dtmp = pos.eph == 0xFFFF ? 1e10f : pos.eph / 100.0; dtmp = pos.eph == 0xFFFF ? 1e10f : pos.eph / 100.0;
if(dtmp != satRawHDOP)
if (dtmp != satRawHDOP)
{ {
satRawHDOP = dtmp; satRawHDOP = dtmp;
emit satRawHDOPChanged(satRawHDOP); emit satRawHDOPChanged(satRawHDOP);
} }
dtmp = pos.epv == 0xFFFF ? 1e10f : pos.epv / 100.0; dtmp = pos.epv == 0xFFFF ? 1e10f : pos.epv / 100.0;
if(dtmp != satRawVDOP)
if (dtmp != satRawVDOP)
{ {
satRawVDOP = dtmp; satRawVDOP = dtmp;
emit satRawVDOPChanged(satRawVDOP); emit satRawVDOPChanged(satRawVDOP);
} }
dtmp = pos.cog == 0xFFFF ? 0.0 : pos.cog / 100.0; dtmp = pos.cog == 0xFFFF ? 0.0 : pos.cog / 100.0;
if(dtmp != satRawCOG)
if (dtmp != satRawCOG)
{ {
satRawCOG = dtmp; satRawCOG = dtmp;
emit satRawCOGChanged(satRawCOG); emit satRawCOGChanged(satRawCOG);
...@@ -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); emit localizationChanged(this, loc_type);
} }
break; break;
case MAVLINK_MSG_ID_GPS_STATUS: case MAVLINK_MSG_ID_GPS_STATUS:
{ {
mavlink_gps_status_t pos; mavlink_gps_status_t pos;
mavlink_msg_gps_status_decode(&message, &pos); mavlink_msg_gps_status_decode(&message, &pos);
for(int i = 0; i < (int)pos.satellites_visible; i++)
for (int i = 0; i < (int)pos.satellites_visible; i++)
{ {
emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i])); emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i]));
} }
setSatelliteCount(pos.satellites_visible); setSatelliteCount(pos.satellites_visible);
} }
break; break;
...@@ -666,10 +624,9 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -666,10 +624,9 @@ void UAS::receiveMessage(mavlink_message_t message)
paramVal.param_float = rawValue.param_value; paramVal.param_float = rawValue.param_value;
paramVal.type = rawValue.param_type; paramVal.type = rawValue.param_type;
processParamValueMsg(message, parameterName, rawValue, paramVal); processParamValueMsg(message, parameterName,rawValue,paramVal);
} }
break; break;
case MAVLINK_MSG_ID_ATTITUDE_TARGET: case MAVLINK_MSG_ID_ATTITUDE_TARGET:
{ {
mavlink_attitude_target_t out; mavlink_attitude_target_t out;
...@@ -692,14 +649,12 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -692,14 +649,12 @@ void UAS::receiveMessage(mavlink_message_t message)
{ {
break; break;
} }
mavlink_position_target_local_ned_t p; mavlink_position_target_local_ned_t p;
mavlink_msg_position_target_local_ned_decode(&message, &p); mavlink_msg_position_target_local_ned_decode(&message, &p);
quint64 time = getUnixTimeFromMs(p.time_boot_ms); quint64 time = getUnixTimeFromMs(p.time_boot_ms);
emit positionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */, time); emit positionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */, time);
} }
break; break;
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
{ {
mavlink_set_position_target_local_ned_t p; mavlink_set_position_target_local_ned_t p;
...@@ -707,15 +662,14 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -707,15 +662,14 @@ void UAS::receiveMessage(mavlink_message_t message)
emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */); emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */);
} }
break; break;
case MAVLINK_MSG_ID_STATUSTEXT: case MAVLINK_MSG_ID_STATUSTEXT:
{ {
QByteArray b; QByteArray b;
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()); mavlink_msg_statustext_get_text(&message, b.data());
// Ensure NUL-termination // Ensure NUL-termination
b[b.length() - 1] = '\0'; b[b.length()-1] = '\0';
QString text = QString(b); QString text = QString(b);
int severity = mavlink_msg_statustext_get_severity(&message); int severity = mavlink_msg_statustext_get_severity(&message);
...@@ -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); emit textMessageReceived(uasId, message.compid, severity, text);
_say(text.toLower(), severity); _say(text.toLower(), severity);
} }
else else
{ {
emit textMessageReceived(uasId, message.compid, severity, text); emit textMessageReceived(uasId, message.compid, severity, text);
...@@ -770,11 +723,9 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -770,11 +723,9 @@ void UAS::receiveMessage(mavlink_message_t message)
for (int i = 0; i < imagePayload; ++i) for (int i = 0; i < imagePayload; ++i)
{ {
if (pos <= imageSize) if (pos <= imageSize) {
{
imageRecBuffer[pos] = img.data[i]; imageRecBuffer[pos] = img.data[i];
} }
++pos; ++pos;
} }
...@@ -794,7 +745,7 @@ void UAS::receiveMessage(mavlink_message_t message) ...@@ -794,7 +745,7 @@ void UAS::receiveMessage(mavlink_message_t message)
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
{ {
mavlink_nav_controller_output_t p; 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); setDistToWaypoint(p.wp_dist);
setBearingToWaypoint(p.nav_bearing); setBearingToWaypoint(p.nav_bearing);
emit navigationControllerErrorsChanged(this, p.alt_error, p.aspd_error, p.xtrack_error); emit navigationControllerErrorsChanged(this, p.alt_error, p.aspd_error, p.xtrack_error);
...@@ -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) void UAS::startCalibration(UASInterface::StartCalibrationType calType)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; 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 accelCal = 0;
int escCal = 0; int escCal = 0;
switch (calType) switch (calType) {
{
case StartCalibrationGyro: case StartCalibrationGyro:
gyroCal = 1; gyroCal = 1;
break; break;
case StartCalibrationMag: case StartCalibrationMag:
magCal = 1; magCal = 1;
break; break;
case StartCalibrationAirspeed: case StartCalibrationAirspeed:
airspeedCal = 1; airspeedCal = 1;
break; break;
case StartCalibrationRadio: case StartCalibrationRadio:
radioCal = 1; radioCal = 1;
break; break;
case StartCalibrationCopyTrims: case StartCalibrationCopyTrims:
radioCal = 2; radioCal = 2;
break; break;
case StartCalibrationAccel: case StartCalibrationAccel:
accelCal = 1; accelCal = 1;
break; break;
case StartCalibrationLevel: case StartCalibrationLevel:
accelCal = 2; accelCal = 2;
break; break;
case StartCalibrationEsc: case StartCalibrationEsc:
escCal = 1; escCal = 1;
break; break;
case StartCalibrationUavcanEsc: case StartCalibrationUavcanEsc:
escCal = 2; escCal = 2;
break; break;
case StartCalibrationCompassMot: case StartCalibrationCompassMot:
airspeedCal = 1; // ArduPilot, bit of a hack airspeedCal = 1; // ArduPilot, bit of a hack
break; break;
...@@ -901,8 +841,7 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType) ...@@ -901,8 +841,7 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
void UAS::stopCalibration(void) void UAS::stopCalibration(void)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
...@@ -926,19 +865,16 @@ void UAS::stopCalibration(void) ...@@ -926,19 +865,16 @@ void UAS::stopCalibration(void)
void UAS::startBusConfig(UASInterface::StartBusConfigType calType) void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
int actuatorCal = 0; int actuatorCal = 0;
switch (calType) switch (calType) {
{
case StartBusConfigActuators: case StartBusConfigActuators:
actuatorCal = 1; actuatorCal = 1;
break; break;
case EndBusConfigActuators: case EndBusConfigActuators:
actuatorCal = 0; actuatorCal = 0;
break; break;
...@@ -964,8 +900,7 @@ void UAS::startBusConfig(UASInterface::StartBusConfigType calType) ...@@ -964,8 +900,7 @@ void UAS::startBusConfig(UASInterface::StartBusConfigType calType)
void UAS::stopBusConfig(void) void UAS::stopBusConfig(void)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
...@@ -1001,7 +936,6 @@ quint64 UAS::getUnixReferenceTime(quint64 time) ...@@ -1001,7 +936,6 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
// qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds(); // qDebug() << "XNEW time:" <<QGC::groundTimeMilliseconds();
return QGC::groundTimeMilliseconds(); return QGC::groundTimeMilliseconds();
} }
// Check if time is smaller than 40 years, // Check if time is smaller than 40 years,
// assuming no system without Unix timestamp // assuming no system without Unix timestamp
// runs longer than 40 years continuously without // runs longer than 40 years continuously without
...@@ -1019,7 +953,6 @@ quint64 UAS::getUnixReferenceTime(quint64 time) ...@@ -1019,7 +953,6 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
// 1000 milliseconds // 1000 milliseconds
// 1000 microseconds // 1000 microseconds
#ifndef _MSC_VER #ifndef _MSC_VER
else if (time < 1261440000000000LLU) else if (time < 1261440000000000LLU)
#else #else
else if (time < 1261440000000000) else if (time < 1261440000000000)
...@@ -1028,17 +961,15 @@ quint64 UAS::getUnixReferenceTime(quint64 time) ...@@ -1028,17 +961,15 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
// qDebug() << "GEN time:" << time/1000 + onboardTimeOffset; // qDebug() << "GEN time:" << time/1000 + onboardTimeOffset;
if (onboardTimeOffset == 0) if (onboardTimeOffset == 0)
{ {
onboardTimeOffset = QGC::groundTimeMilliseconds() - time / 1000; onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
} }
return time/1000 + onboardTimeOffset;
return time / 1000 + onboardTimeOffset;
} }
else else
{ {
// Time is not zero and larger than 40 years -> has to be // Time is not zero and larger than 40 years -> has to be
// a Unix epoch timestamp. Do nothing. // a Unix epoch timestamp. Do nothing.
return time / 1000; return time/1000;
} }
} }
...@@ -1053,7 +984,7 @@ quint64 UAS::getUnixReferenceTime(quint64 time) ...@@ -1053,7 +984,7 @@ quint64 UAS::getUnixReferenceTime(quint64 time)
*/ */
quint64 UAS::getUnixTimeFromMs(quint64 time) quint64 UAS::getUnixTimeFromMs(quint64 time)
{ {
return getUnixTime(time * 1000); return getUnixTime(time*1000);
} }
/** /**
...@@ -1068,7 +999,6 @@ quint64 UAS::getUnixTimeFromMs(quint64 time) ...@@ -1068,7 +999,6 @@ quint64 UAS::getUnixTimeFromMs(quint64 time)
quint64 UAS::getUnixTime(quint64 time) quint64 UAS::getUnixTime(quint64 time)
{ {
quint64 ret = 0; quint64 ret = 0;
if (attitudeStamped) if (attitudeStamped)
{ {
ret = lastAttitude; ret = lastAttitude;
...@@ -1078,7 +1008,6 @@ quint64 UAS::getUnixTime(quint64 time) ...@@ -1078,7 +1008,6 @@ quint64 UAS::getUnixTime(quint64 time)
{ {
ret = QGC::groundTimeMilliseconds(); ret = QGC::groundTimeMilliseconds();
} }
// Check if time is smaller than 40 years, // Check if time is smaller than 40 years,
// assuming no system without Unix timestamp // assuming no system without Unix timestamp
// runs longer than 40 years continuously without // runs longer than 40 years continuously without
...@@ -1096,7 +1025,6 @@ quint64 UAS::getUnixTime(quint64 time) ...@@ -1096,7 +1025,6 @@ quint64 UAS::getUnixTime(quint64 time)
// 1000 milliseconds // 1000 milliseconds
// 1000 microseconds // 1000 microseconds
#ifndef _MSC_VER #ifndef _MSC_VER
else if (time < 1261440000000000LLU) else if (time < 1261440000000000LLU)
#else #else
else if (time < 1261440000000000) else if (time < 1261440000000000)
...@@ -1106,19 +1034,17 @@ quint64 UAS::getUnixTime(quint64 time) ...@@ -1106,19 +1034,17 @@ quint64 UAS::getUnixTime(quint64 time)
if (onboardTimeOffset == 0 || time < (lastNonNullTime - 100)) if (onboardTimeOffset == 0 || time < (lastNonNullTime - 100))
{ {
lastNonNullTime = time; lastNonNullTime = time;
onboardTimeOffset = QGC::groundTimeMilliseconds() - time / 1000; onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000;
} }
if (time > lastNonNullTime) lastNonNullTime = time; if (time > lastNonNullTime) lastNonNullTime = time;
ret = time / 1000 + onboardTimeOffset; ret = time/1000 + onboardTimeOffset;
} }
else else
{ {
// Time is not zero and larger than 40 years -> has to be // Time is not zero and larger than 40 years -> has to be
// a Unix epoch timestamp. Do nothing. // a Unix epoch timestamp. Do nothing.
ret = time / 1000; ret = time/1000;
} }
return ret; return ret;
...@@ -1129,7 +1055,7 @@ quint64 UAS::getUnixTime(quint64 time) ...@@ -1129,7 +1055,7 @@ quint64 UAS::getUnixTime(quint64 time)
* Status can be unitialized, booting up, calibrating sensors, active * Status can be unitialized, booting up, calibrating sensors, active
* standby, cirtical, emergency, shutdown or unknown. * 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) switch (statusCode)
{ {
...@@ -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"); uasState = tr("UNINIT");
stateDescription = tr("Unitialized, booting up."); stateDescription = tr("Unitialized, booting up.");
break; break;
case MAV_STATE_BOOT: case MAV_STATE_BOOT:
uasState = tr("BOOT"); uasState = tr("BOOT");
stateDescription = tr("Booting system, please wait."); stateDescription = tr("Booting system, please wait.");
break; break;
case MAV_STATE_CALIBRATING: case MAV_STATE_CALIBRATING:
uasState = tr("CALIBRATING"); uasState = tr("CALIBRATING");
stateDescription = tr("Calibrating sensors, please wait."); stateDescription = tr("Calibrating sensors, please wait.");
break; break;
case MAV_STATE_ACTIVE: case MAV_STATE_ACTIVE:
uasState = tr("ACTIVE"); uasState = tr("ACTIVE");
stateDescription = tr("Active, normal operation."); stateDescription = tr("Active, normal operation.");
break; break;
case MAV_STATE_STANDBY: case MAV_STATE_STANDBY:
uasState = tr("STANDBY"); uasState = tr("STANDBY");
stateDescription = tr("Standby mode, ready for launch."); stateDescription = tr("Standby mode, ready for launch.");
break; break;
case MAV_STATE_CRITICAL: case MAV_STATE_CRITICAL:
uasState = tr("CRITICAL"); uasState = tr("CRITICAL");
stateDescription = tr("FAILURE: Continuing operation."); stateDescription = tr("FAILURE: Continuing operation.");
break; break;
case MAV_STATE_EMERGENCY: case MAV_STATE_EMERGENCY:
uasState = tr("EMERGENCY"); uasState = tr("EMERGENCY");
stateDescription = tr("EMERGENCY: Land Immediately!"); stateDescription = tr("EMERGENCY: Land Immediately!");
break; break;
//case MAV_STATE_HILSIM: //case MAV_STATE_HILSIM:
//uasState = tr("HIL SIM"); //uasState = tr("HIL SIM");
//stateDescription = tr("HIL Simulation, Sensors read from SIM"); //stateDescription = tr("HIL Simulation, Sensors read from SIM");
...@@ -1206,18 +1125,17 @@ QImage UAS::getImage() ...@@ -1206,18 +1125,17 @@ QImage UAS::getImage()
if (imageRecBuffer.isNull()) if (imageRecBuffer.isNull())
{ {
qDebug() << "could not convertToPGM()"; qDebug()<< "could not convertToPGM()";
return QImage(); return QImage();
} }
if (!image.loadFromData(tmpImage, "PGM")) if (!image.loadFromData(tmpImage, "PGM"))
{ {
qDebug() << __FILE__ << __LINE__ << "could not create extracted image"; qDebug()<< __FILE__ << __LINE__ << "could not create extracted image";
return QImage(); return QImage();
} }
} }
// BMP with header // BMP with header
else if (imageType == MAVLINK_DATA_STREAM_IMG_BMP || else if (imageType == MAVLINK_DATA_STREAM_IMG_BMP ||
imageType == MAVLINK_DATA_STREAM_IMG_JPEG || imageType == MAVLINK_DATA_STREAM_IMG_JPEG ||
...@@ -1240,8 +1158,7 @@ QImage UAS::getImage() ...@@ -1240,8 +1158,7 @@ QImage UAS::getImage()
void UAS::requestImage() void UAS::requestImage()
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
...@@ -1266,11 +1183,10 @@ void UAS::requestImage() ...@@ -1266,11 +1183,10 @@ void UAS::requestImage()
*/ */
quint64 UAS::getUptime() const quint64 UAS::getUptime() const
{ {
if (startTime == 0) if(startTime == 0)
{ {
return 0; return 0;
} }
else else
{ {
return QGC::groundTimeMilliseconds() - startTime; return QGC::groundTimeMilliseconds() - startTime;
...@@ -1278,7 +1194,7 @@ quint64 UAS::getUptime() const ...@@ -1278,7 +1194,7 @@ quint64 UAS::getUptime() const
} }
//TODO update this to use the parameter manager / param data model instead //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; int compId = msg.compid;
...@@ -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 // Insert with correct type
switch (rawValue.param_type) switch (rawValue.param_type) {
{
case MAV_PARAM_TYPE_REAL32: case MAV_PARAM_TYPE_REAL32:
paramValue = QVariant(paramUnion.param_float); paramValue = QVariant(paramUnion.param_float);
break; break;
...@@ -1335,8 +1250,7 @@ void UAS::processParamValueMsg(mavlink_message_t &msg, const QString &paramName, ...@@ -1335,8 +1250,7 @@ void UAS::processParamValueMsg(mavlink_message_t &msg, const QString &paramName,
void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
...@@ -1363,8 +1277,7 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float ...@@ -1363,8 +1277,7 @@ void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float
*/ */
void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode) void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
...@@ -1382,17 +1295,12 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t ...@@ -1382,17 +1295,12 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
// The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz. // The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz.
bool sendCommand = false; bool sendCommand = false;
if (countSinceLastTransmission++ >= 5) {
if (countSinceLastTransmission++ >= 5)
{
sendCommand = true; sendCommand = true;
countSinceLastTransmission = 0; countSinceLastTransmission = 0;
} } else if ((!qIsNaN(roll) && roll != manualRollAngle) || (!qIsNaN(pitch) && pitch != manualPitchAngle) ||
else if ((!qIsNaN(roll) && roll != manualRollAngle) || (!qIsNaN(pitch) && pitch != manualPitchAngle) ||
(!qIsNaN(yaw) && yaw != manualYawAngle) || (!qIsNaN(thrust) && thrust != manualThrust) || (!qIsNaN(yaw) && yaw != manualYawAngle) || (!qIsNaN(thrust) && thrust != manualThrust) ||
buttons != manualButtons) buttons != manualButtons) {
{
sendCommand = true; sendCommand = true;
// Ensure that another message will be sent the next time this function is called // Ensure that another message will be sent the next time this function is called
...@@ -1400,8 +1308,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t ...@@ -1400,8 +1308,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
} }
// Now if we should trigger an update, let's do that // Now if we should trigger an update, let's do that
if (sendCommand) if (sendCommand) {
{
// Save the new manual control inputs // Save the new manual control inputs
manualRollAngle = roll; manualRollAngle = roll;
manualPitchAngle = pitch; manualPitchAngle = pitch;
...@@ -1411,8 +1318,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t ...@@ -1411,8 +1318,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
mavlink_message_t message; mavlink_message_t message;
if (joystickMode == Vehicle::JoystickModeAttitude) if (joystickMode == Vehicle::JoystickModeAttitude) {
{
// send an external attitude setpoint command (rate control disabled) // send an external attitude setpoint command (rate control disabled)
float attitudeQuaternion[4]; float attitudeQuaternion[4];
mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion); mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion);
...@@ -1430,10 +1336,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t ...@@ -1430,10 +1336,7 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
0, 0,
thrust thrust
); );
} } else if (joystickMode == Vehicle::JoystickModePosition) {
else if (joystickMode == Vehicle::JoystickModePosition)
{
// Send the the local position setpoint (local pos sp external message) // Send the the local position setpoint (local pos sp external message)
static float px = 0; static float px = 0;
static float py = 0; static float py = 0;
...@@ -1441,8 +1344,8 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t ...@@ -1441,8 +1344,8 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
//XXX: find decent scaling //XXX: find decent scaling
px -= pitch; px -= pitch;
py += roll; py += roll;
pz -= 2.0f * (thrust - 0.5); pz -= 2.0f*(thrust-0.5);
uint16_t typeMask = (1 << 11) | (7 << 6) | (7 << 3); // select only POSITION control uint16_t typeMask = (1<<11)|(7<<6)|(7<<3); // select only POSITION control
mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(),
mavlink->getComponentId(), mavlink->getComponentId(),
&message, &message,
...@@ -1463,17 +1366,14 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t ...@@ -1463,17 +1366,14 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
yaw, yaw,
0 0
); );
} } else if (joystickMode == Vehicle::JoystickModeForce) {
else if (joystickMode == Vehicle::JoystickModeForce)
{
// Send the the force setpoint (local pos sp external message) // Send the the force setpoint (local pos sp external message)
float dcm[3][3]; float dcm[3][3];
mavlink_euler_to_dcm(roll, pitch, yaw, dcm); mavlink_euler_to_dcm(roll, pitch, yaw, dcm);
const float fx = -dcm[0][2] * thrust; const float fx = -dcm[0][2] * thrust;
const float fy = -dcm[1][2] * thrust; const float fy = -dcm[1][2] * thrust;
const float fz = -dcm[2][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_msg_set_position_target_local_ned_pack(mavlink->getSystemId(),
mavlink->getComponentId(), mavlink->getComponentId(),
&message, &message,
...@@ -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,
0 0
); );
} } else if (joystickMode == Vehicle::JoystickModeVelocity) {
else if (joystickMode == Vehicle::JoystickModeVelocity)
{
// Send the the local velocity setpoint (local pos sp external message) // Send the the local velocity setpoint (local pos sp external message)
static float vx = 0; static float vx = 0;
static float vy = 0; static float vy = 0;
...@@ -1506,9 +1403,9 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t ...@@ -1506,9 +1403,9 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
//XXX: find decent scaling //XXX: find decent scaling
vx -= pitch; vx -= pitch;
vy += roll; vy += roll;
vz -= 2.0f * (thrust - 0.5); vz -= 2.0f*(thrust-0.5);
yawrate += yaw; //XXX: not sure what scale to apply here 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_msg_set_position_target_local_ned_pack(mavlink->getSystemId(),
mavlink->getComponentId(), mavlink->getComponentId(),
&message, &message,
...@@ -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, 0,
yawrate yawrate
); );
} } else if (joystickMode == Vehicle::JoystickModeRC) {
else if (joystickMode == Vehicle::JoystickModeRC)
{
// Save the new manual control inputs // Save the new manual control inputs
manualRollAngle = roll; manualRollAngle = roll;
...@@ -1566,15 +1460,13 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t ...@@ -1566,15 +1460,13 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
#ifndef __mobile__ #ifndef __mobile__
void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw) void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
const uint8_t base_mode = _vehicle->baseMode(); const uint8_t base_mode = _vehicle->baseMode();
// If system has manual inputs enabled and is armed // If system has manual inputs enabled and is armed
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; mavlink_message_t message;
float q[4]; float q[4];
...@@ -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()); //emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
} }
else else
{ {
qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first"; qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first";
...@@ -1612,8 +1503,7 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll ...@@ -1612,8 +1503,7 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
*/ */
void UAS::pairRX(int rxType, int rxSubType) void UAS::pairRX(int rxType, int rxSubType)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
...@@ -1627,21 +1517,17 @@ void UAS::pairRX(int rxType, int rxSubType) ...@@ -1627,21 +1517,17 @@ void UAS::pairRX(int rxType, int rxSubType)
* If enabled, connect the flight gear link. * If enabled, connect the flight gear link.
*/ */
#ifndef __mobile__ #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); Q_UNUSED(configuration);
QGCFlightGearLink *link = dynamic_cast<QGCFlightGearLink *>(simulation); QGCFlightGearLink* link = dynamic_cast<QGCFlightGearLink*>(simulation);
if (!link) {
if (!link)
{
// Delete wrong sim // Delete wrong sim
if (simulation) if (simulation) {
{
stopHil(); stopHil();
delete simulation; delete simulation;
} }
simulation = new QGCFlightGearLink(_vehicle, options); simulation = new QGCFlightGearLink(_vehicle, options);
} }
...@@ -1661,17 +1547,15 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj ...@@ -1661,17 +1547,15 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
temperature_var = noise_scaler * 0.7290f; temperature_var = noise_scaler * 0.7290f;
// Connect Flight Gear Link // Connect Flight Gear Link
link = dynamic_cast<QGCFlightGearLink *>(simulation); link = dynamic_cast<QGCFlightGearLink*>(simulation);
link->setStartupArguments(options); link->setStartupArguments(options);
link->sensorHilEnabled(sensorHil); link->sensorHilEnabled(sensorHil);
// FIXME: this signal is not on the base hil configuration widget, only on the FG widget // FIXME: this signal is not on the base hil configuration widget, only on the FG widget
//QObject::connect(configuration, SIGNAL(barometerOffsetChanged(float)), link, SLOT(setBarometerOffset(float))); //QObject::connect(configuration, SIGNAL(barometerOffsetChanged(float)), link, SLOT(setBarometerOffset(float)));
if (enable) if (enable)
{ {
startHil(); startHil();
} }
else else
{ {
stopHil(); stopHil();
...@@ -1685,29 +1569,22 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj ...@@ -1685,29 +1569,22 @@ void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObj
#ifndef __mobile__ #ifndef __mobile__
void UAS::enableHilJSBSim(bool enable, QString options) void UAS::enableHilJSBSim(bool enable, QString options)
{ {
QGCJSBSimLink *link = dynamic_cast<QGCJSBSimLink *>(simulation); QGCJSBSimLink* link = dynamic_cast<QGCJSBSimLink*>(simulation);
if (!link) {
if (!link)
{
// Delete wrong sim // Delete wrong sim
if (simulation) if (simulation) {
{
stopHil(); stopHil();
delete simulation; delete simulation;
} }
simulation = new QGCJSBSimLink(_vehicle, options); simulation = new QGCJSBSimLink(_vehicle, options);
} }
// Connect Flight Gear Link // Connect Flight Gear Link
link = dynamic_cast<QGCJSBSimLink *>(simulation); link = dynamic_cast<QGCJSBSimLink*>(simulation);
link->setStartupArguments(options); link->setStartupArguments(options);
if (enable) if (enable)
{ {
startHil(); startHil();
} }
else else
{ {
stopHil(); stopHil();
...@@ -1721,16 +1598,12 @@ void UAS::enableHilJSBSim(bool enable, QString options) ...@@ -1721,16 +1598,12 @@ void UAS::enableHilJSBSim(bool enable, QString options)
#ifndef __mobile__ #ifndef __mobile__
void UAS::enableHilXPlane(bool enable) void UAS::enableHilXPlane(bool enable)
{ {
QGCXPlaneLink *link = dynamic_cast<QGCXPlaneLink *>(simulation); QGCXPlaneLink* link = dynamic_cast<QGCXPlaneLink*>(simulation);
if (!link) {
if (!link) if (simulation) {
{
if (simulation)
{
stopHil(); stopHil();
delete simulation; delete simulation;
} }
simulation = new QGCXPlaneLink(_vehicle); simulation = new QGCXPlaneLink(_vehicle);
float noise_scaler = 0.0001f; float noise_scaler = 0.0001f;
...@@ -1748,13 +1621,11 @@ void UAS::enableHilXPlane(bool enable) ...@@ -1748,13 +1621,11 @@ void UAS::enableHilXPlane(bool enable)
pressure_alt_var = noise_scaler * 0.5604f; pressure_alt_var = noise_scaler * 0.5604f;
temperature_var = noise_scaler * 0.7290f; temperature_var = noise_scaler * 0.7290f;
} }
// Connect X-Plane Link // Connect X-Plane Link
if (enable) if (enable)
{ {
startHil(); startHil();
} }
else else
{ {
stopHil(); stopHil();
...@@ -1799,13 +1670,13 @@ void UAS::sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw ...@@ -1799,13 +1670,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, "pitch rate sim", "rad/s", pitchspeed, getUnixTime());
emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime()); emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime());
emit valueChanged(uasId, "lat sim", "deg", lat * 1e7, getUnixTime()); emit valueChanged(uasId, "lat sim", "deg", lat*1e7, getUnixTime());
emit valueChanged(uasId, "lon sim", "deg", lon * 1e7, getUnixTime()); emit valueChanged(uasId, "lon sim", "deg", lon*1e7, getUnixTime());
emit valueChanged(uasId, "alt sim", "deg", alt * 1e3, getUnixTime()); emit valueChanged(uasId, "alt sim", "deg", alt*1e3, getUnixTime());
emit valueChanged(uasId, "vx sim", "m/s", vx * 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, "vy sim", "m/s", vy*1e2, getUnixTime());
emit valueChanged(uasId, "vz sim", "m/s", vz * 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, "IAS sim", "m/s", ind_airspeed, getUnixTime());
emit valueChanged(uasId, "TAS sim", "m/s", true_airspeed, getUnixTime()); emit valueChanged(uasId, "TAS sim", "m/s", true_airspeed, getUnixTime());
...@@ -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 pitchspeed, float yawspeed, double lat, double lon, double alt,
float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
...@@ -1862,10 +1732,9 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa ...@@ -1862,10 +1732,9 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_hil_state_quaternion_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, mavlink_msg_hil_state_quaternion_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
time_us, q, rollspeed, pitchspeed, yawspeed, 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); _vehicle->sendMessageOnPriorityLink(msg);
} }
else else
{ {
// Attempt to set HIL mode // Attempt to set HIL mode
...@@ -1892,7 +1761,7 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var) ...@@ -1892,7 +1761,7 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
u1 = rand() * (1.0 / RAND_MAX); u1 = rand() * (1.0 / RAND_MAX);
u2 = 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 z0 = sqrt(-2.0 * log(u1)) * cos(2.0f * M_PI * u2); //calculate normally distributed variable with mu = 0, var = 1
...@@ -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 float noise = z0 * sqrt(noise_var); //calculate normally distributed variable with mu = 0, std = var^2
//Finally guard against any case where the noise is not real //Finally guard against any case where the noise is not real
if (std::isfinite(noise)) if(std::isfinite(noise)) {
{
return truth_meas + noise; return truth_meas + noise;
} } else {
else
{
return truth_meas; return truth_meas;
} }
} }
...@@ -1921,8 +1786,7 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var) ...@@ -1921,8 +1786,7 @@ float UAS::addZeroMeanNoise(float truth_meas, float noise_var)
void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed) float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
...@@ -1931,16 +1795,16 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl ...@@ -1931,16 +1795,16 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
float xacc_corrupt = addZeroMeanNoise(xacc, xacc_var); float xacc_corrupt = addZeroMeanNoise(xacc, xacc_var);
float yacc_corrupt = addZeroMeanNoise(yacc, yacc_var); float yacc_corrupt = addZeroMeanNoise(yacc, yacc_var);
float zacc_corrupt = addZeroMeanNoise(zacc, zacc_var); float zacc_corrupt = addZeroMeanNoise(zacc, zacc_var);
float rollspeed_corrupt = addZeroMeanNoise(rollspeed, rollspeed_var); float rollspeed_corrupt = addZeroMeanNoise(rollspeed,rollspeed_var);
float pitchspeed_corrupt = addZeroMeanNoise(pitchspeed, pitchspeed_var); float pitchspeed_corrupt = addZeroMeanNoise(pitchspeed,pitchspeed_var);
float yawspeed_corrupt = addZeroMeanNoise(yawspeed, yawspeed_var); float yawspeed_corrupt = addZeroMeanNoise(yawspeed,yawspeed_var);
float xmag_corrupt = addZeroMeanNoise(xmag, xmag_var); float xmag_corrupt = addZeroMeanNoise(xmag, xmag_var);
float ymag_corrupt = addZeroMeanNoise(ymag, ymag_var); float ymag_corrupt = addZeroMeanNoise(ymag, ymag_var);
float zmag_corrupt = addZeroMeanNoise(zmag, zmag_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 diff_pressure_corrupt = addZeroMeanNoise(diff_pressure, diff_pressure_var);
float pressure_alt_corrupt = addZeroMeanNoise(pressure_alt, pressure_alt_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_message_t msg;
mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
...@@ -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); _vehicle->sendMessageOnPriorityLink(msg);
lastSendTimeSensors = QGC::groundTimeMilliseconds(); lastSendTimeSensors = QGC::groundTimeMilliseconds();
} }
else else
{ {
// Attempt to set HIL mode // Attempt to set HIL mode
...@@ -1964,8 +1827,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl ...@@ -1964,8 +1827,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl
void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
float flow_comp_m_y, quint8 quality, float ground_distance) float flow_comp_m_y, quint8 quality, float ground_distance)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
...@@ -1990,7 +1852,6 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa ...@@ -1990,7 +1852,6 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds(); lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds();
#endif #endif
} }
else else
{ {
// Attempt to set HIL mode // Attempt to set HIL mode
...@@ -2004,8 +1865,7 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa ...@@ -2004,8 +1865,7 @@ void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, floa
#ifndef __mobile__ #ifndef __mobile__
void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites) void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
...@@ -2016,21 +1876,18 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi ...@@ -2016,21 +1876,18 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
if (_vehicle->hilMode()) if (_vehicle->hilMode())
{ {
float course = cog; float course = cog;
// map to 0..2pi // map to 0..2pi
if (course < 0) if (course < 0)
course += 2.0f * static_cast<float>(M_PI); course += 2.0f * static_cast<float>(M_PI);
// scale from radians to degrees // scale from radians to degrees
course = (course / M_PI) * 180.0f; course = (course / M_PI) * 180.0f;
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_hil_gps_pack(mavlink->getSystemId(), mavlink->getComponentId(), &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(); lastSendTimeGPS = QGC::groundTimeMilliseconds();
_vehicle->sendMessageOnPriorityLink(msg); _vehicle->sendMessageOnPriorityLink(msg);
} }
else else
{ {
// Attempt to set HIL mode // Attempt to set HIL mode
...@@ -2047,7 +1904,6 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi ...@@ -2047,7 +1904,6 @@ void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fi
void UAS::startHil() void UAS::startHil()
{ {
if (hilEnabled) return; if (hilEnabled) return;
hilEnabled = true; hilEnabled = true;
sensorHil = false; sensorHil = false;
_vehicle->setHilMode(true); _vehicle->setHilMode(true);
...@@ -2063,13 +1919,11 @@ void UAS::startHil() ...@@ -2063,13 +1919,11 @@ void UAS::startHil()
#ifndef __mobile__ #ifndef __mobile__
void UAS::stopHil() void UAS::stopHil()
{ {
if (simulation && simulation->isConnected()) if (simulation && simulation->isConnected()) {
{
simulation->disconnectSimulation(); simulation->disconnectSimulation();
_vehicle->setHilMode(false); _vehicle->setHilMode(false);
qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to disable."; qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to disable.";
} }
hilEnabled = false; hilEnabled = false;
sensorHil = false; sensorHil = false;
} }
...@@ -2085,15 +1939,13 @@ QMap<int, QString> UAS::getComponents() ...@@ -2085,15 +1939,13 @@ QMap<int, QString> UAS::getComponents()
void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax) void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax)
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
mavlink_message_t message; mavlink_message_t message;
char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {}; char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};
// Copy string into buffer, ensuring not to exceed the buffer size // Copy string into buffer, ensuring not to exceed the buffer size
for (unsigned int i = 0; i < sizeof(param_id_cstr); i++) for (unsigned int i = 0; i < sizeof(param_id_cstr); i++)
{ {
...@@ -2121,15 +1973,13 @@ void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 p ...@@ -2121,15 +1973,13 @@ void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 p
void UAS::unsetRCToParameterMap() void UAS::unsetRCToParameterMap()
{ {
if (!_vehicle) if (!_vehicle) {
{
return; return;
} }
char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {}; char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {};
for (int i = 0; i < 3; i++) for (int i = 0; i < 3; i++) {
{
mavlink_message_t message; mavlink_message_t message;
mavlink_msg_param_map_rc_pack(mavlink->getSystemId(), mavlink_msg_param_map_rc_pack(mavlink->getSystemId(),
mavlink->getComponentId(), mavlink->getComponentId(),
...@@ -2147,7 +1997,7 @@ void UAS::unsetRCToParameterMap() ...@@ -2147,7 +1997,7 @@ void UAS::unsetRCToParameterMap()
} }
} }
void UAS::_say(const QString &text, int severity) void UAS::_say(const QString& text, int severity)
{ {
Q_UNUSED(severity); Q_UNUSED(severity);
qgcApp()->toolbox()->audioOutput()->say(text); qgcApp()->toolbox()->audioOutput()->say(text);
...@@ -2157,15 +2007,12 @@ void UAS::shutdownVehicle(void) ...@@ -2157,15 +2007,12 @@ void UAS::shutdownVehicle(void)
{ {
#ifndef __mobile__ #ifndef __mobile__
stopHil(); stopHil();
if (simulation) {
if (simulation)
{
// wait for the simulator to exit // wait for the simulator to exit
simulation->wait(); simulation->wait();
simulation->disconnectSimulation(); simulation->disconnectSimulation();
simulation->deleteLater(); simulation->deleteLater();
} }
#endif #endif
_vehicle = NULL; _vehicle = NULL;
} }
src/uas/UAS.h
View file @ ae153170
...@@ -50,7 +50,7 @@ class UAS : public UASInterface ...@@ -50,7 +50,7 @@ class UAS : public UASInterface
{ {
Q_OBJECT Q_OBJECT
public: public:
UAS(MAVLinkProtocol *protocol, Vehicle *vehicle, FirmwarePluginManager *firmwarePluginManager); UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * firmwarePluginManager);
float lipoFull; ///< 100% charged voltage float lipoFull; ///< 100% charged voltage
float lipoEmpty; ///< Discharged voltage float lipoEmpty; ///< Discharged voltage
...@@ -89,8 +89,8 @@ public: ...@@ -89,8 +89,8 @@ public:
void setGroundSpeed(double val) void setGroundSpeed(double val)
{ {
groundSpeed = val; groundSpeed = val;
emit groundSpeedChanged(val, "groundSpeed"); emit groundSpeedChanged(val,"groundSpeed");
emit valueChanged(this->uasId, "groundSpeed", "m/s", QVariant(val), getUnixTime()); emit valueChanged(this->uasId,"groundSpeed","m/s",QVariant(val),getUnixTime());
} }
double getGroundSpeed() const double getGroundSpeed() const
{ {
...@@ -100,8 +100,8 @@ public: ...@@ -100,8 +100,8 @@ public:
void setAirSpeed(double val) void setAirSpeed(double val)
{ {
airSpeed = val; airSpeed = val;
emit airSpeedChanged(val, "airSpeed"); emit airSpeedChanged(val,"airSpeed");
emit valueChanged(this->uasId, "airSpeed", "m/s", QVariant(val), getUnixTime()); emit valueChanged(this->uasId,"airSpeed","m/s",QVariant(val),getUnixTime());
} }
double getAirSpeed() const double getAirSpeed() const
...@@ -112,8 +112,8 @@ public: ...@@ -112,8 +112,8 @@ public:
void setLocalX(double val) void setLocalX(double val)
{ {
localX = val; localX = val;
emit localXChanged(val, "localX"); emit localXChanged(val,"localX");
emit valueChanged(this->uasId, "localX", "m", QVariant(val), getUnixTime()); emit valueChanged(this->uasId,"localX","m",QVariant(val),getUnixTime());
} }
double getLocalX() const double getLocalX() const
...@@ -124,8 +124,8 @@ public: ...@@ -124,8 +124,8 @@ public:
void setLocalY(double val) void setLocalY(double val)
{ {
localY = val; localY = val;
emit localYChanged(val, "localY"); emit localYChanged(val,"localY");
emit valueChanged(this->uasId, "localY", "m", QVariant(val), getUnixTime()); emit valueChanged(this->uasId,"localY","m",QVariant(val),getUnixTime());
} }
double getLocalY() const double getLocalY() const
{ {
...@@ -135,8 +135,8 @@ public: ...@@ -135,8 +135,8 @@ public:
void setLocalZ(double val) void setLocalZ(double val)
{ {
localZ = val; localZ = val;
emit localZChanged(val, "localZ"); emit localZChanged(val,"localZ");
emit valueChanged(this->uasId, "localZ", "m", QVariant(val), getUnixTime()); emit valueChanged(this->uasId,"localZ","m",QVariant(val),getUnixTime());
} }
double getLocalZ() const double getLocalZ() const
{ {
...@@ -146,8 +146,8 @@ public: ...@@ -146,8 +146,8 @@ public:
void setLatitude(double val) void setLatitude(double val)
{ {
latitude = val; latitude = val;
emit latitudeChanged(val, "latitude"); emit latitudeChanged(val,"latitude");
emit valueChanged(this->uasId, "latitude", "deg", QVariant(val), getUnixTime()); emit valueChanged(this->uasId,"latitude","deg",QVariant(val),getUnixTime());
} }
double getLatitude() const double getLatitude() const
...@@ -158,8 +158,8 @@ public: ...@@ -158,8 +158,8 @@ public:
void setLongitude(double val) void setLongitude(double val)
{ {
longitude = val; longitude = val;
emit longitudeChanged(val, "longitude"); emit longitudeChanged(val,"longitude");
emit valueChanged(this->uasId, "longitude", "deg", QVariant(val), getUnixTime()); emit valueChanged(this->uasId,"longitude","deg",QVariant(val),getUnixTime());
} }
double getLongitude() const double getLongitude() const
...@@ -171,10 +171,10 @@ public: ...@@ -171,10 +171,10 @@ public:
{ {
altitudeAMSL = val; altitudeAMSL = val;
emit altitudeAMSLChanged(val, "altitudeAMSL"); 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; altitudeAMSLFT = 3.28084 * altitudeAMSL;
emit altitudeAMSLFTChanged(val, "altitudeAMSLFT"); 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 double getAltitudeAMSL() const
...@@ -191,7 +191,7 @@ public: ...@@ -191,7 +191,7 @@ public:
{ {
altitudeRelative = val; altitudeRelative = val;
emit altitudeRelativeChanged(val, "altitudeRelative"); 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 double getAltitudeRelative() const
...@@ -217,8 +217,8 @@ public: ...@@ -217,8 +217,8 @@ public:
void setSatelliteCount(double val) void setSatelliteCount(double val)
{ {
satelliteCount = val; satelliteCount = val;
emit satelliteCountChanged(val, "satelliteCount"); emit satelliteCountChanged(val,"satelliteCount");
emit valueChanged(this->uasId, "satelliteCount", "", QVariant(val), getUnixTime()); emit valueChanged(this->uasId,"satelliteCount","",QVariant(val),getUnixTime());
} }
double getSatelliteCount() const double getSatelliteCount() const
...@@ -234,8 +234,8 @@ public: ...@@ -234,8 +234,8 @@ public:
void setDistToWaypoint(double val) void setDistToWaypoint(double val)
{ {
distToWaypoint = val; distToWaypoint = val;
emit distToWaypointChanged(val, "distToWaypoint"); emit distToWaypointChanged(val,"distToWaypoint");
emit valueChanged(this->uasId, "distToWaypoint", "m", QVariant(val), getUnixTime()); emit valueChanged(this->uasId,"distToWaypoint","m",QVariant(val),getUnixTime());
} }
double getDistToWaypoint() const double getDistToWaypoint() const
...@@ -246,8 +246,8 @@ public: ...@@ -246,8 +246,8 @@ public:
void setBearingToWaypoint(double val) void setBearingToWaypoint(double val)
{ {
bearingToWaypoint = val; bearingToWaypoint = val;
emit bearingToWaypointChanged(val, "bearingToWaypoint"); emit bearingToWaypointChanged(val,"bearingToWaypoint");
emit valueChanged(this->uasId, "bearingToWaypoint", "deg", QVariant(val), getUnixTime()); emit valueChanged(this->uasId,"bearingToWaypoint","deg",QVariant(val),getUnixTime());
} }
double getBearingToWaypoint() const double getBearingToWaypoint() const
...@@ -259,7 +259,7 @@ public: ...@@ -259,7 +259,7 @@ public:
void setRoll(double val) void setRoll(double val)
{ {
roll = val; roll = val;
emit rollChanged(val, "roll"); emit rollChanged(val,"roll");
} }
double getRoll() const double getRoll() const
...@@ -270,7 +270,7 @@ public: ...@@ -270,7 +270,7 @@ public:
void setPitch(double val) void setPitch(double val)
{ {
pitch = val; pitch = val;
emit pitchChanged(val, "pitch"); emit pitchChanged(val,"pitch");
} }
double getPitch() const double getPitch() const
...@@ -281,7 +281,7 @@ public: ...@@ -281,7 +281,7 @@ public:
void setYaw(double val) void setYaw(double val)
{ {
yaw = val; yaw = val;
emit yawChanged(val, "yaw"); emit yawChanged(val,"yaw");
} }
double getYaw() const double getYaw() const
...@@ -290,68 +290,55 @@ public: ...@@ -290,68 +290,55 @@ public:
} }
// Setters for HIL noise variance // Setters for HIL noise variance
void setXaccVar(float var) void setXaccVar(float var){
{
xacc_var = var; xacc_var = var;
} }
void setYaccVar(float var) void setYaccVar(float var){
{
yacc_var = var; yacc_var = var;
} }
void setZaccVar(float var) void setZaccVar(float var){
{
zacc_var = var; zacc_var = var;
} }
void setRollSpeedVar(float var) void setRollSpeedVar(float var){
{
rollspeed_var = var; rollspeed_var = var;
} }
void setPitchSpeedVar(float var) void setPitchSpeedVar(float var){
{
pitchspeed_var = var; pitchspeed_var = var;
} }
void setYawSpeedVar(float var) void setYawSpeedVar(float var){
{
pitchspeed_var = var; pitchspeed_var = var;
} }
void setXmagVar(float var) void setXmagVar(float var){
{
xmag_var = var; xmag_var = var;
} }
void setYmagVar(float var) void setYmagVar(float var){
{
ymag_var = var; ymag_var = var;
} }
void setZmagVar(float var) void setZmagVar(float var){
{
zmag_var = var; zmag_var = var;
} }
void setAbsPressureVar(float var) void setAbsPressureVar(float var){
{
abs_pressure_var = var; abs_pressure_var = var;
} }
void setDiffPressureVar(float var) void setDiffPressureVar(float var){
{
diff_pressure_var = var; diff_pressure_var = var;
} }
void setPressureAltVar(float var) void setPressureAltVar(float var){
{
pressure_alt_var = var; pressure_alt_var = var;
} }
void setTemperatureVar(float var) void setTemperatureVar(float var){
{
temperature_var = var; temperature_var = var;
} }
...@@ -365,7 +352,7 @@ protected: //COMMENTS FOR TEST UNIT ...@@ -365,7 +352,7 @@ protected: //COMMENTS FOR TEST UNIT
QMap<int, QString> components;///< IDs and names of all detected onboard components QMap<int, QString> components;///< IDs and names of all detected onboard components
QList<int> unknownPackets; ///< Packet IDs which are unknown and have been received 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 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 float sendDropRate; ///< Percentage of packets that were not received from the MAV by the GCS
...@@ -463,21 +450,20 @@ protected: //COMMENTS FOR TEST UNIT ...@@ -463,21 +450,20 @@ protected: //COMMENTS FOR TEST UNIT
/// SIMULATION /// SIMULATION
#ifndef __mobile__ #ifndef __mobile__
QGCHilLink *simulation; ///< Hardware in the loop simulation link QGCHilLink* simulation; ///< Hardware in the loop simulation link
#endif #endif
public: public:
/** @brief Get the human-readable status message for this code */ /** @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__ #ifndef __mobile__
virtual FileManager *getFileManager() { return &fileManager; } virtual FileManager* getFileManager() { return &fileManager; }
#endif #endif
/** @brief Get the HIL simulation */ /** @brief Get the HIL simulation */
#ifndef __mobile__ #ifndef __mobile__
QGCHilLink *getHILSimulation() const QGCHilLink* getHILSimulation() const {
{
return simulation; return simulation;
} }
#endif #endif
...@@ -494,7 +480,7 @@ public slots: ...@@ -494,7 +480,7 @@ public slots:
/** @brief Enable / disable HIL */ /** @brief Enable / disable HIL */
#ifndef __mobile__ #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 enableHilJSBSim(bool enable, QString options);
void enableHilXPlane(bool enable); void enableHilXPlane(bool enable);
...@@ -562,45 +548,45 @@ public slots: ...@@ -562,45 +548,45 @@ public slots:
/** @brief Send command to disable all bindings/maps between RC and parameters */ /** @brief Send command to disable all bindings/maps between RC and parameters */
void unsetRCToParameterMap(); void unsetRCToParameterMap();
signals: signals:
void loadChanged(UASInterface *uas, double load); void loadChanged(UASInterface* uas, double load);
void imageStarted(quint64 timestamp); void imageStarted(quint64 timestamp);
/** @brief A new camera image has arrived */ /** @brief A new camera image has arrived */
void imageReady(UASInterface *uas); void imageReady(UASInterface* uas);
/** @brief HIL controls have changed */ /** @brief HIL controls have changed */
void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode); void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode);
/** @brief HIL actuator controls (replaces HIL controls) */ /** @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 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 localXChanged(double val,QString name);
void localYChanged(double val, QString name); void localYChanged(double val,QString name);
void localZChanged(double val, QString name); void localZChanged(double val,QString name);
void longitudeChanged(double val, QString name); void longitudeChanged(double val,QString name);
void latitudeChanged(double val, QString name); void latitudeChanged(double val,QString name);
void altitudeAMSLChanged(double val, QString name); void altitudeAMSLChanged(double val,QString name);
void altitudeAMSLFTChanged(double val, QString name); void altitudeAMSLFTChanged(double val,QString name);
void altitudeRelativeChanged(double val, QString name); void altitudeRelativeChanged(double val,QString name);
void satRawHDOPChanged(double value); void satRawHDOPChanged (double value);
void satRawVDOPChanged(double value); void satRawVDOPChanged (double value);
void satRawCOGChanged(double value); void satRawCOGChanged (double value);
void rollChanged(double val, QString name); void rollChanged(double val,QString name);
void pitchChanged(double val, QString name); void pitchChanged(double val,QString name);
void yawChanged(double val, QString name); void yawChanged(double val,QString name);
void satelliteCountChanged(double val, QString name); void satelliteCountChanged(double val,QString name);
void distToWaypointChanged(double val, QString name); void distToWaypointChanged(double val,QString name);
void groundSpeedChanged(double val, QString name); void groundSpeedChanged(double val, QString name);
void airSpeedChanged(double val, QString name); void airSpeedChanged(double val, QString name);
void bearingToWaypointChanged(double val, QString name); void bearingToWaypointChanged(double val,QString name);
protected: protected:
/** @brief Get the UNIX timestamp in milliseconds, enter microseconds */ /** @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 */ /** @brief Get the UNIX timestamp in milliseconds, enter milliseconds */
quint64 getUnixTimeFromMs(quint64 time); quint64 getUnixTimeFromMs(quint64 time);
/** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */ /** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */
quint64 getUnixReferenceTime(quint64 time); 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]; int componentID[256];
bool componentMulti[256]; bool componentMulti[256];
...@@ -616,11 +602,11 @@ protected: ...@@ -616,11 +602,11 @@ protected:
quint64 lastSendTimeOpticalFlow; ///< Last HIL Optical Flow message sent quint64 lastSendTimeOpticalFlow; ///< Last HIL Optical Flow message sent
private: private:
void _say(const QString &text, int severity = 6); void _say(const QString& text, int severity = 6);
private: private:
Vehicle *_vehicle; Vehicle* _vehicle;
FirmwarePluginManager *_firmwarePluginManager; FirmwarePluginManager* _firmwarePluginManager;
}; };
......
src/ui/QGCHilXPlaneConfiguration.cc
View file @ ae153170
...@@ -3,7 +3,7 @@ ...@@ -3,7 +3,7 @@
#include "QGCXPlaneLink.h" #include "QGCXPlaneLink.h"
#include "QGCHilConfiguration.h" #include "QGCHilConfiguration.h"
QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink *link, QGCHilConfiguration *parent) : QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink* link, QGCHilConfiguration *parent) :
QWidget(parent), QWidget(parent),
ui(new Ui::QGCHilXPlaneConfiguration) ui(new Ui::QGCHilXPlaneConfiguration)
{ {
...@@ -12,7 +12,7 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink *link, QGCHilCon ...@@ -12,7 +12,7 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink *link, QGCHilCon
connect(ui->startButton, &QPushButton::clicked, this, &QGCHilXPlaneConfiguration::toggleSimulation); 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); link, &QGCHilLink::setRemoteHost);
connect(link, &QGCHilLink::remoteChanged, ui->hostComboBox, &QComboBox::setEditText); connect(link, &QGCHilLink::remoteChanged, ui->hostComboBox, &QComboBox::setEditText);
...@@ -23,7 +23,7 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink *link, QGCHilCon ...@@ -23,7 +23,7 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink *link, QGCHilCon
ui->startButton->setText(tr("Connect")); ui->startButton->setText(tr("Connect"));
QGCXPlaneLink *xplane = dynamic_cast<QGCXPlaneLink *>(link); QGCXPlaneLink* xplane = dynamic_cast<QGCXPlaneLink*>(link);
if (xplane) if (xplane)
{ {
...@@ -58,20 +58,17 @@ void QGCHilXPlaneConfiguration::setVersion(int version) ...@@ -58,20 +58,17 @@ void QGCHilXPlaneConfiguration::setVersion(int version)
void QGCHilXPlaneConfiguration::toggleSimulation(bool connect) void QGCHilXPlaneConfiguration::toggleSimulation(bool connect)
{ {
if (!link) if (!link) {
{
return; return;
} }
Q_UNUSED(connect); Q_UNUSED(connect);
if (!link->isConnected()) if (!link->isConnected())
{ {
link->setRemoteHost(ui->hostComboBox->currentText()); link->setRemoteHost(ui->hostComboBox->currentText());
link->connectSimulation(); link->connectSimulation();
ui->startButton->setText(tr("Disconnect")); ui->startButton->setText(tr("Disconnect"));
} }
else else
{ {
link->disconnectSimulation(); link->disconnectSimulation();
......
Markdown is supported
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