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
Hide 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);
} }
...@@ -291,33 +277,32 @@ void QGCXPlaneLink::setPort(int localPort) ...@@ -291,33 +277,32 @@ void QGCXPlaneLink::setPort(int localPort)
void QGCXPlaneLink::processError(QProcess::ProcessError err) 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;
case QProcess::Crashed:
case QProcess::Crashed: msg = tr("X-Plane crashed. This is an X-Plane-related problem, check for X-Plane upgrade.");
msg = tr("X-Plane crashed. This is an X-Plane-related problem, check for X-Plane upgrade."); break;
break;
case QProcess::Timedout:
case QProcess::Timedout: msg = tr("X-Plane start timed out. Please check if the path and command is correct");
msg = tr("X-Plane start timed out. Please check if the path and command is correct"); break;
break;
case QProcess::ReadError:
case QProcess::ReadError: case QProcess::WriteError:
case QProcess::WriteError: msg = tr("Could not communicate with X-Plane. Please check if the path and command are correct");
msg = tr("Could not communicate with X-Plane. Please check if the path and command are correct"); break;
break;
case QProcess::UnknownError:
case QProcess::UnknownError: default:
default: msg = tr("X-Plane error occurred. Please check if the path and command is correct.");
msg = tr("X-Plane error occurred. Please check if the path and command is correct."); break;
break;
} }
QGCMessageBox::critical(tr("X-Plane HIL"), msg); QGCMessageBox::critical(tr("X-Plane HIL"), msg);
} }
...@@ -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';
...@@ -410,8 +386,8 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch ...@@ -410,8 +386,8 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch
Q_UNUSED(navMode); Q_UNUSED(navMode);
if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR
|| _vehicle->vehicleType() == MAV_TYPE_HEXAROTOR || _vehicle->vehicleType() == MAV_TYPE_HEXAROTOR
|| _vehicle->vehicleType() == MAV_TYPE_OCTOROTOR) || _vehicle->vehicleType() == MAV_TYPE_OCTOROTOR)
{ {
qDebug() << "MAV_TYPE_QUADROTOR"; qDebug() << "MAV_TYPE_QUADROTOR";
...@@ -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,11 +461,10 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct ...@@ -488,11 +461,10 @@ 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:
{ {
p.f[0] = ctl_0; ///< X-Plane Engine 1 p.f[0] = ctl_0; ///< X-Plane Engine 1
p.f[1] = ctl_1; ///< X-Plane Engine 2 p.f[1] = ctl_1; ///< X-Plane Engine 2
...@@ -505,11 +477,10 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct ...@@ -505,11 +477,10 @@ 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:
{ {
/** /**
* Tailsitter with four control flaps and eight motors. * Tailsitter with four control flaps and eight motors.
...@@ -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,8 +507,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct ...@@ -536,8 +507,7 @@ 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. */
p.f[0] = -ctl_1; ///< X-Plane Elevator p.f[0] = -ctl_1; ///< X-Plane Elevator
...@@ -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,34 +536,33 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct ...@@ -566,34 +536,33 @@ 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); double s__ = sin(yaw);
double s__ = sin(yaw); double _s_ = sin(pitch);
double _s_ = sin(pitch); double __s = sin(roll);
double __s = sin(roll); double cc_ = c__ * _c_;
double cc_ = c__ * _c_; double cs_ = c__ * _s_;
double cs_ = c__ * _s_; double sc_ = s__ * _c_;
double sc_ = s__ * _c_; double ss_ = s__ * _s_;
double ss_ = s__ * _s_; double c_c = c__ * __c;
double c_c = c__ * __c; double c_s = c__ * __s;
double c_s = c__ * __s; double s_c = s__ * __c;
double s_c = s__ * __c; double s_s = s__ * __s;
double s_s = s__ * __s; double _cc = _c_ * __c;
double _cc = _c_ * __c; double _cs = _c_ * __s;
double _cs = _c_ * __s; double csc = cs_ * __c;
double csc = cs_ * __c; double css = cs_ * __s;
double css = cs_ * __s; double ssc = ss_ * __c;
double ssc = ss_ * __c; 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;
} }
void QGCXPlaneLink::_writeBytes(const QByteArray data) void QGCXPlaneLink::_writeBytes(const QByteArray data)
...@@ -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::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl;
std::string headStr = std::string(data, data + 5);
std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl;
} }
// Calculate the number of data segments a 36 bytes // 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,42 +638,39 @@ void QGCXPlaneLink::readBytes() ...@@ -675,42 +638,39 @@ 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);
Eigen::Matrix3f R = euler_to_wRo(yaw, pitch, roll); Eigen::Matrix3f R = euler_to_wRo(yaw, pitch, roll);
Eigen::Vector3f gr = R.transpose().eval() * g; Eigen::Vector3f gr = R.transpose().eval() * g;
xacc = gr[0]; xacc = gr[0];
yacc = gr[1]; yacc = gr[1];
zacc = gr[2]; zacc = gr[2];
//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. const float one_g = 9.80665f;
const float one_g = 9.80665f; xacc = p.f[5] * one_g;
xacc = p.f[5] * one_g; yacc = p.f[6] * one_g;
yacc = p.f[6] * one_g; zacc = -p.f[4] * one_g;
zacc = -p.f[4] * one_g;
//qDebug() << "X-Plane values" << xacc << yacc << zacc;
//qDebug() << "X-Plane values" << xacc << yacc << zacc; }
}
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);
...@@ -825,15 +777,14 @@ void QGCXPlaneLink::readBytes() ...@@ -825,15 +777,14 @@ void QGCXPlaneLink::readBytes()
// { // {
// qDebug() << "ATT:" << p.f[0] << p.f[1] << p.f[2]; // qDebug() << "ATT:" << p.f[0] << p.f[1] << p.f[2];
// } // }
else if (p.index == 20) else if (p.index == 20)
{ {
//qDebug() << "LAT/LON/ALT:" << p.f[0] << p.f[1] << p.f[2]; //qDebug() << "LAT/LON/ALT:" << p.f[0] << p.f[1] << p.f[2];
lat = p.f[0]; lat = p.f[0];
lon = p.f[1]; lon = p.f[1];
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];
...@@ -873,9 +823,9 @@ void QGCXPlaneLink::readBytes() ...@@ -873,9 +823,9 @@ void QGCXPlaneLink::readBytes()
} }
else if (data[0] == 'S' && else if (data[0] == 'S' &&
data[1] == 'T' && data[1] == 'T' &&
data[2] == 'A' && data[2] == 'A' &&
data[3] == 'T') data[3] == 'T')
{ {
} }
...@@ -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,16 +127,14 @@ public slots: ...@@ -130,16 +127,14 @@ 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);
} }
void enableHilActuatorControls(bool enable); void enableHilActuatorControls(bool 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
This source diff could not be displayed because it is too large. You can view the blob instead.
src/uas/UAS.h
View file @ ae153170
/**************************************************************************** /****************************************************************************
* *
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org> * (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
* *
* QGroundControl is licensed according to the terms in the file * QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory. * COPYING.md in the root of the source code directory.
* *
****************************************************************************/ ****************************************************************************/
/** /**
* @file * @file
* @brief Definition of Unmanned Aerial Vehicle object * @brief Definition of Unmanned Aerial Vehicle object
* *
* @author Lorenz Meier <mavteam@student.ethz.ch> * @author Lorenz Meier <mavteam@student.ethz.ch>
* *
*/ */
#ifndef _UAS_H_ #ifndef _UAS_H_
#define _UAS_H_ #define _UAS_H_
#include "UASInterface.h" #include "UASInterface.h"
#include <MAVLinkProtocol.h> #include <MAVLinkProtocol.h>
#include <QVector3D> #include <QVector3D>
#include "QGCMAVLink.h" #include "QGCMAVLink.h"
#include "Vehicle.h" #include "Vehicle.h"
#include "FirmwarePluginManager.h" #include "FirmwarePluginManager.h"
#ifndef __mobile__ #ifndef __mobile__
#include "FileManager.h" #include "FileManager.h"
#include "QGCHilLink.h" #include "QGCHilLink.h"
#include "QGCFlightGearLink.h" #include "QGCFlightGearLink.h"
#include "QGCJSBSimLink.h" #include "QGCJSBSimLink.h"
#include "QGCXPlaneLink.h" #include "QGCXPlaneLink.h"
#endif #endif
Q_DECLARE_LOGGING_CATEGORY(UASLog) Q_DECLARE_LOGGING_CATEGORY(UASLog)
class Vehicle; class Vehicle;
/** /**
* @brief A generic MAVLINK-connected MAV/UAV * @brief A generic MAVLINK-connected MAV/UAV
* *
* This class represents one vehicle. It can be used like the real vehicle, e.g. a call to halt() * This class represents one vehicle. It can be used like the real vehicle, e.g. a call to halt()
* will automatically send the appropriate messages to the vehicle. The vehicle state will also be * will automatically send the appropriate messages to the vehicle. The vehicle state will also be
* automatically updated by the comm architecture, so when writing code to e.g. control the vehicle * automatically updated by the comm architecture, so when writing code to e.g. control the vehicle
* no knowledge of the communication infrastructure is needed. * no knowledge of the communication infrastructure is needed.
*/ */
class UAS : public UASInterface 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
/* MANAGEMENT */ /* MANAGEMENT */
/** @brief Get the unique system id */ /** @brief Get the unique system id */
int getUASID() const; int getUASID() const;
/** @brief Get the components */ /** @brief Get the components */
QMap<int, QString> getComponents(); QMap<int, QString> getComponents();
/** @brief The time interval the robot is switched on */ /** @brief The time interval the robot is switched on */
quint64 getUptime() const; quint64 getUptime() const;
Q_PROPERTY(double latitude READ getLatitude WRITE setLatitude NOTIFY latitudeChanged) Q_PROPERTY(double latitude READ getLatitude WRITE setLatitude NOTIFY latitudeChanged)
Q_PROPERTY(double longitude READ getLongitude WRITE setLongitude NOTIFY longitudeChanged) Q_PROPERTY(double longitude READ getLongitude WRITE setLongitude NOTIFY longitudeChanged)
Q_PROPERTY(double satelliteCount READ getSatelliteCount WRITE setSatelliteCount NOTIFY satelliteCountChanged) Q_PROPERTY(double satelliteCount READ getSatelliteCount WRITE setSatelliteCount NOTIFY satelliteCountChanged)
Q_PROPERTY(bool isGlobalPositionKnown READ globalPositionKnown) Q_PROPERTY(bool isGlobalPositionKnown READ globalPositionKnown)
Q_PROPERTY(double roll READ getRoll WRITE setRoll NOTIFY rollChanged) Q_PROPERTY(double roll READ getRoll WRITE setRoll NOTIFY rollChanged)
Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged) Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged)
Q_PROPERTY(double yaw READ getYaw WRITE setYaw NOTIFY yawChanged) Q_PROPERTY(double yaw READ getYaw WRITE setYaw NOTIFY yawChanged)
Q_PROPERTY(double distToWaypoint READ getDistToWaypoint WRITE setDistToWaypoint NOTIFY distToWaypointChanged) Q_PROPERTY(double distToWaypoint READ getDistToWaypoint WRITE setDistToWaypoint NOTIFY distToWaypointChanged)
Q_PROPERTY(double airSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY airSpeedChanged) Q_PROPERTY(double airSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY airSpeedChanged)
Q_PROPERTY(double groundSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY groundSpeedChanged) Q_PROPERTY(double groundSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY groundSpeedChanged)
Q_PROPERTY(double bearingToWaypoint READ getBearingToWaypoint WRITE setBearingToWaypoint NOTIFY bearingToWaypointChanged) Q_PROPERTY(double bearingToWaypoint READ getBearingToWaypoint WRITE setBearingToWaypoint NOTIFY bearingToWaypointChanged)
Q_PROPERTY(double altitudeAMSL READ getAltitudeAMSL WRITE setAltitudeAMSL NOTIFY altitudeAMSLChanged) Q_PROPERTY(double altitudeAMSL READ getAltitudeAMSL WRITE setAltitudeAMSL NOTIFY altitudeAMSLChanged)
Q_PROPERTY(double altitudeAMSLFT READ getAltitudeAMSLFT NOTIFY altitudeAMSLFTChanged) Q_PROPERTY(double altitudeAMSLFT READ getAltitudeAMSLFT NOTIFY altitudeAMSLFTChanged)
Q_PROPERTY(double altitudeRelative READ getAltitudeRelative WRITE setAltitudeRelative NOTIFY altitudeRelativeChanged) Q_PROPERTY(double altitudeRelative READ getAltitudeRelative WRITE setAltitudeRelative NOTIFY altitudeRelativeChanged)
Q_PROPERTY(double satRawHDOP READ getSatRawHDOP NOTIFY satRawHDOPChanged) Q_PROPERTY(double satRawHDOP READ getSatRawHDOP NOTIFY satRawHDOPChanged)
Q_PROPERTY(double satRawVDOP READ getSatRawVDOP NOTIFY satRawVDOPChanged) Q_PROPERTY(double satRawVDOP READ getSatRawVDOP NOTIFY satRawVDOPChanged)
Q_PROPERTY(double satRawCOG READ getSatRawCOG NOTIFY satRawCOGChanged) Q_PROPERTY(double satRawCOG READ getSatRawCOG NOTIFY satRawCOGChanged)
/// Vehicle is about to go away /// Vehicle is about to go away
void shutdownVehicle(void); void shutdownVehicle(void);
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
{ {
return groundSpeed; return groundSpeed;
} }
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
{ {
return airSpeed; return airSpeed;
} }
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
{ {
return localX; return localX;
} }
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
{ {
return localY; return localY;
} }
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
{ {
return localZ; return localZ;
} }
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
{ {
return latitude; return latitude;
} }
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
{ {
return longitude; return longitude;
} }
void setAltitudeAMSL(double val) void setAltitudeAMSL(double val)
{ {
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
{ {
return altitudeAMSL; return altitudeAMSL;
} }
double getAltitudeAMSLFT() const double getAltitudeAMSLFT() const
{ {
return altitudeAMSLFT; return altitudeAMSLFT;
} }
void setAltitudeRelative(double val) void setAltitudeRelative(double val)
{ {
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
{ {
return altitudeRelative; return altitudeRelative;
} }
double getSatRawHDOP() const double getSatRawHDOP() const
{ {
return satRawHDOP; return satRawHDOP;
} }
double getSatRawVDOP() const double getSatRawVDOP() const
{ {
return satRawVDOP; return satRawVDOP;
} }
double getSatRawCOG() const double getSatRawCOG() const
{ {
return satRawCOG; return satRawCOG;
} }
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
{ {
return satelliteCount; return satelliteCount;
} }
virtual bool globalPositionKnown() const virtual bool globalPositionKnown() const
{ {
return isGlobalPositionKnown; return isGlobalPositionKnown;
} }
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
{ {
return distToWaypoint; return distToWaypoint;
} }
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
{ {
return bearingToWaypoint; return bearingToWaypoint;
} }
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
{ {
return roll; return roll;
} }
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
{ {
return pitch; return pitch;
} }
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
{ {
return yaw; return yaw;
} }
// 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){
zacc_var = var;
void setZaccVar(float var) }
{
zacc_var = var; void setRollSpeedVar(float var){
} rollspeed_var = var;
}
void setRollSpeedVar(float var)
{ void setPitchSpeedVar(float var){
rollspeed_var = var; pitchspeed_var = var;
} }
void setPitchSpeedVar(float var) void setYawSpeedVar(float var){
{ pitchspeed_var = var;
pitchspeed_var = var; }
}
void setXmagVar(float var){
void setYawSpeedVar(float var) xmag_var = var;
{ }
pitchspeed_var = var;
} void setYmagVar(float var){
ymag_var = var;
void setXmagVar(float var) }
{
xmag_var = var; void setZmagVar(float var){
} zmag_var = var;
}
void setYmagVar(float var)
{ void setAbsPressureVar(float var){
ymag_var = var; abs_pressure_var = var;
} }
void setZmagVar(float var) void setDiffPressureVar(float var){
{ diff_pressure_var = var;
zmag_var = var; }
}
void setPressureAltVar(float var){
void setAbsPressureVar(float var) pressure_alt_var = var;
{ }
abs_pressure_var = var;
} void setTemperatureVar(float var){
temperature_var = var;
void setDiffPressureVar(float var) }
{
diff_pressure_var = var; #ifndef __mobile__
} friend class FileManager;
#endif
void setPressureAltVar(float var)
{ protected: //COMMENTS FOR TEST UNIT
pressure_alt_var = var; /// LINK ID AND STATUS
} int uasId; ///< Unique system ID
QMap<int, QString> components;///< IDs and names of all detected onboard components
void setTemperatureVar(float var)
{ QList<int> unknownPackets; ///< Packet IDs which are unknown and have been received
temperature_var = var; MAVLinkProtocol* mavlink; ///< Reference to the MAVLink instance
} float receiveDropRate; ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs)
float sendDropRate; ///< Percentage of packets that were not received from the MAV by the GCS
#ifndef __mobile__
friend class FileManager; /// BASIC UAS TYPE, NAME AND STATE
#endif int status; ///< The current status of the MAV
protected: //COMMENTS FOR TEST UNIT /// TIMEKEEPING
/// LINK ID AND STATUS quint64 startTime; ///< The time the UAS was switched on
int uasId; ///< Unique system ID quint64 onboardTimeOffset;
QMap<int, QString> components;///< IDs and names of all detected onboard components
/// MANUAL CONTROL
QList<int> unknownPackets; ///< Packet IDs which are unknown and have been received bool controlRollManual; ///< status flag, true if roll is controlled manually
MAVLinkProtocol *mavlink; ///< Reference to the MAVLink instance bool controlPitchManual; ///< status flag, true if pitch is controlled manually
float receiveDropRate; ///< Percentage of packets that were dropped on the MAV's receiving link (from GCS and other MAVs) bool controlYawManual; ///< status flag, true if yaw is controlled manually
float sendDropRate; ///< Percentage of packets that were not received from the MAV by the GCS bool controlThrustManual; ///< status flag, true if thrust is controlled manually
/// BASIC UAS TYPE, NAME AND STATE double manualRollAngle; ///< Roll angle set by human pilot (radians)
int status; ///< The current status of the MAV double manualPitchAngle; ///< Pitch angle set by human pilot (radians)
double manualYawAngle; ///< Yaw angle set by human pilot (radians)
/// TIMEKEEPING double manualThrust; ///< Thrust set by human pilot (radians)
quint64 startTime; ///< The time the UAS was switched on
quint64 onboardTimeOffset; /// POSITION
bool isGlobalPositionKnown; ///< If the global position has been received for this MAV
/// MANUAL CONTROL
bool controlRollManual; ///< status flag, true if roll is controlled manually double localX;
bool controlPitchManual; ///< status flag, true if pitch is controlled manually double localY;
bool controlYawManual; ///< status flag, true if yaw is controlled manually double localZ;
bool controlThrustManual; ///< status flag, true if thrust is controlled manually
double latitude; ///< Global latitude as estimated by position estimator
double manualRollAngle; ///< Roll angle set by human pilot (radians) double longitude; ///< Global longitude as estimated by position estimator
double manualPitchAngle; ///< Pitch angle set by human pilot (radians) double altitudeAMSL; ///< Global altitude as estimated by position estimator, AMSL
double manualYawAngle; ///< Yaw angle set by human pilot (radians) double altitudeAMSLFT; ///< Global altitude as estimated by position estimator, AMSL
double manualThrust; ///< Thrust set by human pilot (radians) double altitudeRelative; ///< Altitude above home as estimated by position estimator
/// POSITION double satRawHDOP;
bool isGlobalPositionKnown; ///< If the global position has been received for this MAV double satRawVDOP;
double satRawCOG;
double localX;
double localY; double satelliteCount; ///< Number of satellites visible to raw GPS
double localZ; bool globalEstimatorActive; ///< Global position estimator present, do not fall back to GPS raw for position
double latitude_gps; ///< Global latitude as estimated by raw GPS
double latitude; ///< Global latitude as estimated by position estimator double longitude_gps; ///< Global longitude as estimated by raw GPS
double longitude; ///< Global longitude as estimated by position estimator double altitude_gps; ///< Global altitude as estimated by raw GPS
double altitudeAMSL; ///< Global altitude as estimated by position estimator, AMSL double speedX; ///< True speed in X axis
double altitudeAMSLFT; ///< Global altitude as estimated by position estimator, AMSL double speedY; ///< True speed in Y axis
double altitudeRelative; ///< Altitude above home as estimated by position estimator double speedZ; ///< True speed in Z axis
double satRawHDOP; /// WAYPOINT NAVIGATION
double satRawVDOP; double distToWaypoint; ///< Distance to next waypoint
double satRawCOG; double airSpeed; ///< Airspeed
double groundSpeed; ///< Groundspeed
double satelliteCount; ///< Number of satellites visible to raw GPS double bearingToWaypoint; ///< Bearing to next waypoint
bool globalEstimatorActive; ///< Global position estimator present, do not fall back to GPS raw for position #ifndef __mobile__
double latitude_gps; ///< Global latitude as estimated by raw GPS FileManager fileManager;
double longitude_gps; ///< Global longitude as estimated by raw GPS #endif
double altitude_gps; ///< Global altitude as estimated by raw GPS
double speedX; ///< True speed in X axis /// ATTITUDE
double speedY; ///< True speed in Y axis bool attitudeKnown; ///< True if attitude was received, false else
double speedZ; ///< True speed in Z axis bool attitudeStamped; ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV
quint64 lastAttitude; ///< Timestamp of last attitude measurement
/// WAYPOINT NAVIGATION double roll;
double distToWaypoint; ///< Distance to next waypoint double pitch;
double airSpeed; ///< Airspeed double yaw;
double groundSpeed; ///< Groundspeed
double bearingToWaypoint; ///< Bearing to next waypoint // dongfang: This looks like a candidate for being moved off to a separate class.
#ifndef __mobile__ /// IMAGING
FileManager fileManager; int imageSize; ///< Image size being transmitted (bytes)
#endif int imagePackets; ///< Number of data packets being sent for this image
int imagePacketsArrived; ///< Number of data packets received
/// ATTITUDE int imagePayload; ///< Payload size per transmitted packet (bytes). Standard is 254, and decreases when image resolution increases.
bool attitudeKnown; ///< True if attitude was received, false else int imageQuality; ///< Quality of the transmitted image (percentage)
bool attitudeStamped; ///< Should arriving data be timestamped with the last attitude? This helps with broken system time clocks on the MAV int imageType; ///< Type of the transmitted image (BMP, PNG, JPEG, RAW 8 bit, RAW 32 bit)
quint64 lastAttitude; ///< Timestamp of last attitude measurement int imageWidth; ///< Width of the image stream
double roll; int imageHeight; ///< Width of the image stream
double pitch; QByteArray imageRecBuffer; ///< Buffer for the incoming bytestream
double yaw; QImage image; ///< Image data of last completely transmitted image
quint64 imageStart;
// dongfang: This looks like a candidate for being moved off to a separate class. bool blockHomePositionChanges; ///< Block changes to the home position
/// IMAGING bool receivedMode; ///< True if mode was retrieved from current conenction to UAS
int imageSize; ///< Image size being transmitted (bytes)
int imagePackets; ///< Number of data packets being sent for this image /// SIMULATION NOISE
int imagePacketsArrived; ///< Number of data packets received float xacc_var; ///< variance of x acclerometer noise for HIL sim (mg)
int imagePayload; ///< Payload size per transmitted packet (bytes). Standard is 254, and decreases when image resolution increases. float yacc_var; ///< variance of y acclerometer noise for HIL sim (mg)
int imageQuality; ///< Quality of the transmitted image (percentage) float zacc_var; ///< variance of z acclerometer noise for HIL sim (mg)
int imageType; ///< Type of the transmitted image (BMP, PNG, JPEG, RAW 8 bit, RAW 32 bit) float rollspeed_var; ///< variance of x gyroscope noise for HIL sim (rad/s)
int imageWidth; ///< Width of the image stream float pitchspeed_var; ///< variance of y gyroscope noise for HIL sim (rad/s)
int imageHeight; ///< Width of the image stream float yawspeed_var; ///< variance of z gyroscope noise for HIL sim (rad/s)
QByteArray imageRecBuffer; ///< Buffer for the incoming bytestream float xmag_var; ///< variance of x magnatometer noise for HIL sim (???)
QImage image; ///< Image data of last completely transmitted image float ymag_var; ///< variance of y magnatometer noise for HIL sim (???)
quint64 imageStart; float zmag_var; ///< variance of z magnatometer noise for HIL sim (???)
bool blockHomePositionChanges; ///< Block changes to the home position float abs_pressure_var; ///< variance of absolute pressure noise for HIL sim (hPa)
bool receivedMode; ///< True if mode was retrieved from current conenction to UAS float diff_pressure_var; ///< variance of differential pressure noise for HIL sim (hPa)
float pressure_alt_var; ///< variance of altitude pressure noise for HIL sim (hPa)
/// SIMULATION NOISE float temperature_var; ///< variance of temperature noise for HIL sim (C)
float xacc_var; ///< variance of x acclerometer noise for HIL sim (mg)
float yacc_var; ///< variance of y acclerometer noise for HIL sim (mg) /// SIMULATION
float zacc_var; ///< variance of z acclerometer noise for HIL sim (mg) #ifndef __mobile__
float rollspeed_var; ///< variance of x gyroscope noise for HIL sim (rad/s) QGCHilLink* simulation; ///< Hardware in the loop simulation link
float pitchspeed_var; ///< variance of y gyroscope noise for HIL sim (rad/s) #endif
float yawspeed_var; ///< variance of z gyroscope noise for HIL sim (rad/s)
float xmag_var; ///< variance of x magnatometer noise for HIL sim (???) public:
float ymag_var; ///< variance of y magnatometer noise for HIL sim (???) /** @brief Get the human-readable status message for this code */
float zmag_var; ///< variance of z magnatometer noise for HIL sim (???) void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription);
float abs_pressure_var; ///< variance of absolute pressure noise for HIL sim (hPa)
float diff_pressure_var; ///< variance of differential pressure noise for HIL sim (hPa) #ifndef __mobile__
float pressure_alt_var; ///< variance of altitude pressure noise for HIL sim (hPa) virtual FileManager* getFileManager() { return &fileManager; }
float temperature_var; ///< variance of temperature noise for HIL sim (C) #endif
/// SIMULATION /** @brief Get the HIL simulation */
#ifndef __mobile__ #ifndef __mobile__
QGCHilLink *simulation; ///< Hardware in the loop simulation link QGCHilLink* getHILSimulation() const {
#endif return simulation;
}
public: #endif
/** @brief Get the human-readable status message for this code */
void getStatusForCode(int statusCode, QString &uasState, QString &stateDescription); QImage getImage();
void requestImage();
#ifndef __mobile__
virtual FileManager *getFileManager() { return &fileManager; } public slots:
#endif /** @brief Executes a command with 7 params */
void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component);
/** @brief Get the HIL simulation */
#ifndef __mobile__ /** @brief Order the robot to pair its receiver **/
QGCHilLink *getHILSimulation() const void pairRX(int rxType, int rxSubType);
{
return simulation; /** @brief Enable / disable HIL */
} #ifndef __mobile__
#endif void enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject * configuration);
void enableHilJSBSim(bool enable, QString options);
QImage getImage(); void enableHilXPlane(bool enable);
void requestImage();
/** @brief Send the full HIL state to the MAV */
public slots: void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate,
/** @brief Executes a command with 7 params */ float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt,
void executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component); float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc);
/** @brief Order the robot to pair its receiver **/ void sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate,
void pairRX(int rxType, int rxSubType); float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt,
float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc);
/** @brief Enable / disable HIL */
#ifndef __mobile__ /** @brief RAW sensors for sensor HIL */
void enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject *configuration); void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
void enableHilJSBSim(bool enable, QString options); float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed);
void enableHilXPlane(bool enable);
/** @brief Send Optical Flow sensor message for HIL, (arguments and units accoding to mavlink documentation*/
/** @brief Send the full HIL state to the MAV */ void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x,
void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate, float flow_comp_m_y, quint8 quality, float ground_distance);
float pitchRotationRate, float yawRotationRate, 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 addZeroMeanNoise(float truth_meas, float noise_var);
void sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate, /**
float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt, * @param time_us
float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc); * @param lat
* @param lon
/** @brief RAW sensors for sensor HIL */ * @param alt
void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, * @param fix_type
float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed); * @param eph
* @param epv
/** @brief Send Optical Flow sensor message for HIL, (arguments and units accoding to mavlink documentation*/ * @param vel
void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, * @param cog course over ground, in radians, -pi..pi
float flow_comp_m_y, quint8 quality, float ground_distance); * @param satellites
*/
float addZeroMeanNoise(float truth_meas, float noise_var); void 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);
/**
* @param time_us /** @brief Places the UAV in Hardware-in-the-Loop simulation status **/
* @param lat void startHil();
* @param lon
* @param alt /** @brief Stops the UAV's Hardware-in-the-Loop simulation status **/
* @param fix_type void stopHil();
* @param eph #endif
* @param epv
* @param vel /** @brief Set the values for the manual control of the vehicle */
* @param cog course over ground, in radians, -pi..pi void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode);
* @param satellites
*/ /** @brief Set the values for the 6dof manual control of the vehicle */
void 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); #ifndef __mobile__
void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw);
#endif
/** @brief Places the UAV in Hardware-in-the-Loop simulation status **/
void startHil(); /** @brief Receive a message from one of the communication links. */
virtual void receiveMessage(mavlink_message_t message);
/** @brief Stops the UAV's Hardware-in-the-Loop simulation status **/
void stopHil(); void startCalibration(StartCalibrationType calType);
#endif void stopCalibration(void);
/** @brief Set the values for the manual control of the vehicle */ void startBusConfig(StartBusConfigType calType);
void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode); void stopBusConfig(void);
/** @brief Set the values for the 6dof manual control of the vehicle */ /** @brief Send command to map a RC channel to a parameter */
#ifndef __mobile__ void sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax);
void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw);
#endif /** @brief Send command to disable all bindings/maps between RC and parameters */
void unsetRCToParameterMap();
/** @brief Receive a message from one of the communication links. */ signals:
virtual void receiveMessage(mavlink_message_t message); void loadChanged(UASInterface* uas, double load);
void imageStarted(quint64 timestamp);
void startCalibration(StartCalibrationType calType); /** @brief A new camera image has arrived */
void stopCalibration(void); void imageReady(UASInterface* uas);
/** @brief HIL controls have changed */
void startBusConfig(StartBusConfigType calType); void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode);
void stopBusConfig(void); /** @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);
/** @brief Send command to map a RC channel to a parameter */
void sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax); void localXChanged(double val,QString name);
void localYChanged(double val,QString name);
/** @brief Send command to disable all bindings/maps between RC and parameters */ void localZChanged(double val,QString name);
void unsetRCToParameterMap(); void longitudeChanged(double val,QString name);
signals: void latitudeChanged(double val,QString name);
void loadChanged(UASInterface *uas, double load); void altitudeAMSLChanged(double val,QString name);
void imageStarted(quint64 timestamp); void altitudeAMSLFTChanged(double val,QString name);
/** @brief A new camera image has arrived */ void altitudeRelativeChanged(double val,QString name);
void imageReady(UASInterface *uas);
/** @brief HIL controls have changed */ void satRawHDOPChanged (double value);
void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode); void satRawVDOPChanged (double value);
/** @brief HIL actuator controls (replaces HIL controls) */ void satRawCOGChanged (double value);
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 rollChanged(double val,QString name);
void localXChanged(double val, QString name); void pitchChanged(double val,QString name);
void localYChanged(double val, QString name); void yawChanged(double val,QString name);
void localZChanged(double val, QString name); void satelliteCountChanged(double val,QString name);
void longitudeChanged(double val, QString name); void distToWaypointChanged(double val,QString name);
void latitudeChanged(double val, QString name); void groundSpeedChanged(double val, QString name);
void altitudeAMSLChanged(double val, QString name); void airSpeedChanged(double val, QString name);
void altitudeAMSLFTChanged(double val, QString name); void bearingToWaypointChanged(double val,QString name);
void altitudeRelativeChanged(double val, QString name); protected:
/** @brief Get the UNIX timestamp in milliseconds, enter microseconds */
void satRawHDOPChanged(double value); quint64 getUnixTime(quint64 time=0);
void satRawVDOPChanged(double value); /** @brief Get the UNIX timestamp in milliseconds, enter milliseconds */
void satRawCOGChanged(double value); quint64 getUnixTimeFromMs(quint64 time);
/** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */
void rollChanged(double val, QString name); quint64 getUnixReferenceTime(quint64 time);
void pitchChanged(double val, QString name);
void yawChanged(double val, QString name); virtual void processParamValueMsg(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue);
void satelliteCountChanged(double val, QString name);
void distToWaypointChanged(double val, QString name); int componentID[256];
void groundSpeedChanged(double val, QString name); bool componentMulti[256];
void airSpeedChanged(double val, QString name); bool connectionLost; ///< Flag indicates a timed out connection
void bearingToWaypointChanged(double val, QString name); quint64 connectionLossTime; ///< Time the connection was interrupted
protected: quint64 lastVoltageWarning; ///< Time at which the last voltage warning occurred
/** @brief Get the UNIX timestamp in milliseconds, enter microseconds */ quint64 lastNonNullTime; ///< The last timestamp from the MAV that was not null
quint64 getUnixTime(quint64 time = 0); unsigned int onboardTimeOffsetInvalidCount; ///< Count when the offboard time offset estimation seemed wrong
/** @brief Get the UNIX timestamp in milliseconds, enter milliseconds */ bool hilEnabled;
quint64 getUnixTimeFromMs(quint64 time); bool sensorHil; ///< True if sensor HIL is enabled
/** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */ quint64 lastSendTimeGPS; ///< Last HIL GPS message sent
quint64 getUnixReferenceTime(quint64 time); quint64 lastSendTimeSensors; ///< Last HIL Sensors message sent
quint64 lastSendTimeOpticalFlow; ///< Last HIL Optical Flow message sent
virtual void processParamValueMsg(mavlink_message_t &msg, const QString &paramName, const mavlink_param_value_t &rawValue, mavlink_param_union_t &paramValue);
private:
int componentID[256]; void _say(const QString& text, int severity = 6);
bool componentMulti[256];
bool connectionLost; ///< Flag indicates a timed out connection private:
quint64 connectionLossTime; ///< Time the connection was interrupted Vehicle* _vehicle;
quint64 lastVoltageWarning; ///< Time at which the last voltage warning occurred FirmwarePluginManager* _firmwarePluginManager;
quint64 lastNonNullTime; ///< The last timestamp from the MAV that was not null };
unsigned int onboardTimeOffsetInvalidCount; ///< Count when the offboard time offset estimation seemed wrong
bool hilEnabled;
bool sensorHil; ///< True if sensor HIL is enabled #endif // _UAS_H_
quint64 lastSendTimeGPS; ///< Last HIL GPS message sent
quint64 lastSendTimeSensors; ///< Last HIL Sensors message sent
quint64 lastSendTimeOpticalFlow; ///< Last HIL Optical Flow message sent
private:
void _say(const QString &text, int severity = 6);
private:
Vehicle *_vehicle;
FirmwarePluginManager *_firmwarePluginManager;
};
#endif // _UAS_H_
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