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

Revert "fix code style" 

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

388
src/comm/QGCXPlaneLink.cc
Unescape View File

@ -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()
// 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()
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)
{ {
bool changed = (xPlaneVersion != version); bool changed = (xPlaneVersion != version);
xPlaneVersion = version; xPlaneVersion = version;
if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion)); if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion));
} }
void QGCXPlaneLink::enableHilActuatorControls(bool enable) void QGCXPlaneLink::enableHilActuatorControls(bool enable)
{ {
if (enable != _useHilActuatorControls) if (enable != _useHilActuatorControls) {
{
_useHilActuatorControls = enable; _useHilActuatorControls = enable;
} }
/* Only use override for new message and specific airframes */ /* Only use override for new message and specific airframes */
MAV_TYPE type = _vehicle->vehicleType(); MAV_TYPE type = _vehicle->vehicleType();
float value = 0.0f; float value = 0.0f;
if (type == MAV_TYPE_VTOL_RESERVED2) {
if (type == MAV_TYPE_VTOL_RESERVED2)
{
value = (enable ? 1.0f : 0.0f); value = (enable ? 1.0f : 0.0f);
} }
@ -172,14 +163,12 @@ void QGCXPlaneLink::enableHilActuatorControls(bool enable)
**/ **/
void QGCXPlaneLink::run() void QGCXPlaneLink::run()
{ {
if (!_vehicle) if (!_vehicle) {
{
emit statusMessage("No MAV present"); emit statusMessage("No MAV present");
return; return;
} }
if (connectState) if (connectState) {
{
emit statusMessage("Already connected"); emit statusMessage("Already connected");
return; return;
} }
@ -187,9 +176,7 @@ void QGCXPlaneLink::run()
socket = new QUdpSocket(this); socket = new QUdpSocket(this);
socket->moveToThread(this); socket->moveToThread(this);
connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint); connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint);
if (!connectState) {
if (!connectState)
{
emit statusMessage("Binding socket failed!"); emit statusMessage("Binding socket failed!");
@ -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,29 +278,28 @@ 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;
} }
@ -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)
if (newHost.contains(":")) if (newHost.contains(":"))
{ {
QHostInfo info = QHostInfo::fromName(newHost.split(":").first()); QHostInfo info = QHostInfo::fromName(newHost.split(":").first());
if (info.error() == QHostInfo::NoError) if (info.error() == QHostInfo::NoError)
{ {
// Add newHost // Add newHost
QList<QHostAddress> newHostAddresses = info.addresses(); QList<QHostAddress> newHostAddresses = info.addresses();
QHostAddress address; QHostAddress address;
for (int i = 0; i < newHostAddresses.size(); i++) for (int i = 0; i < newHostAddresses.size(); i++)
{ {
// Exclude loopback IPv4 and all IPv6 addresses // Exclude loopback IPv4 and all IPv6 addresses
@ -352,22 +335,18 @@ void QGCXPlaneLink::setRemoteHost(const QString &newHost)
address = newHostAddresses.at(i); address = newHostAddresses.at(i);
} }
} }
remoteHost = address; remoteHost = address;
// Set localPort according to user input // Set localPort according to user input
remotePort = newHost.split(":").last().toInt(); remotePort = newHost.split(":").last().toInt();
} }
} }
else else
{ {
QHostInfo info = QHostInfo::fromName(newHost); QHostInfo info = QHostInfo::fromName(newHost);
if (info.error() == QHostInfo::NoError) if (info.error() == QHostInfo::NoError)
{ {
// Add newHost // Add newHost
remoteHost = info.addresses().first(); remoteHost = info.addresses().first();
if (remotePort == 0) remotePort = 49000; if (remotePort == 0) remotePort = 49000;
} }
} }
@ -384,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
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
// 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
// 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
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
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
/* 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
/* 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
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
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
/* 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
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
} }
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()
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()
{ {
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()
//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()
temperature = p.f[1]; temperature = p.f[1];
fields_changed |= (1 << 9) | (1 << 12); fields_changed |= (1 << 9) | (1 << 12);
} }
// Forward controls from X-Plane to MAV, not very useful // Forward controls from X-Plane to MAV, not very useful
// better: Connect Joystick to QGroundControl // better: Connect Joystick to QGroundControl
// else if (p.index == 8) // else if (p.index == 8)
@ -741,7 +700,6 @@ void QGCXPlaneLink::readBytes()
emitUpdate = true; emitUpdate = true;
} }
else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18)) else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18))
{ {
//qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3]; //qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3];
@ -750,25 +708,19 @@ void QGCXPlaneLink::readBytes()
yaw = p.f[2] / 180.0f * M_PI; yaw = p.f[2] / 180.0f * M_PI;
// X-Plane expresses yaw as 0..2 PI // X-Plane expresses yaw as 0..2 PI
if (yaw > M_PI) if (yaw > M_PI) {
{
yaw -= 2.0f * static_cast<float>(M_PI); yaw -= 2.0f * static_cast<float>(M_PI);
} }
if (yaw < -M_PI) {
if (yaw < -M_PI)
{
yaw += 2.0f * static_cast<float>(M_PI); yaw += 2.0f * static_cast<float>(M_PI);
} }
float yawmag = p.f[3] / 180.0f * M_PI; float yawmag = p.f[3] / 180.0f * M_PI;
if (yawmag > M_PI) if (yawmag > M_PI) {
{
yawmag -= 2.0f * static_cast<float>(M_PI); yawmag -= 2.0f * static_cast<float>(M_PI);
} }
if (yawmag < -M_PI) {
if (yawmag < -M_PI)
{
yawmag += 2.0f * static_cast<float>(M_PI); yawmag += 2.0f * static_cast<float>(M_PI);
} }
@ -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()
// { // {
// 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()
// 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()
} }
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()
} }
// Wait for 0.5s before actually using the data, so that all fields are filled // Wait for 0.5s before actually using the data, so that all fields are filled
if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) {
{
return; return;
} }
@ -894,9 +843,7 @@ void QGCXPlaneLink::readBytes()
if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 2) if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 2)
{ {
simUpdateHz = simUpdateHz * 0.9f + 0.1f * (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); simUpdateHz = simUpdateHz * 0.9f + 0.1f * (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast));
if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000) {
if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000)
{
emit statusMessage(tr("Receiving from XPlane at %1 Hz").arg(static_cast<int>(simUpdateHz))); emit statusMessage(tr("Receiving from XPlane at %1 Hz").arg(static_cast<int>(simUpdateHz)));
// Reset lowpass with current value // Reset lowpass with current value
simUpdateHz = (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); simUpdateHz = (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast));
@ -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()
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()
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)
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
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
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()
// 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()
**/ **/
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)
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)
/* 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));
} }

31
src/comm/QGCXPlaneLink.h
Unescape View File

@ -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:
bool isConnected(); bool isConnected();
qint64 bytesAvailable(); qint64 bytesAvailable();
int getPort() const int getPort() const {
{
return localPort; return localPort;
} }
@ -89,13 +88,11 @@ public:
return (int)airframeID; return (int)airframeID;
} }
bool sensorHilEnabled() bool sensorHilEnabled() {
{
return _sensorHilEnabled; return _sensorHilEnabled;
} }
bool useHilActuatorControls() bool useHilActuatorControls() {
{
return _useHilActuatorControls; return _useHilActuatorControls;
} }
@ -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:
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:
* @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:
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:
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;

1311
src/uas/UAS.cc
View File

File diff suppressed because it is too large Load Diff

166
src/uas/UAS.h
Unescape View File

@ -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:
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:
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:
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:
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:
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:
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:
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:
{ {
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:
{ {
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:
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:
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:
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:
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:
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:
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:
} }
// 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
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
/// 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,18 +480,18 @@ 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);
/** @brief Send the full HIL state to the MAV */ /** @brief Send the full HIL state to the MAV */
void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate, void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate,
float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt, 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 vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc);
void sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate, void sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate,
float pitchRotationRate, float yawRotationRate, double lat, double lon, double alt, 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 vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc);
/** @brief RAW sensors for sensor HIL */ /** @brief RAW sensors for sensor HIL */
void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed,
@ -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:
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;
}; };

11
src/ui/QGCHilXPlaneConfiguration.cc
Unescape View File

@ -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
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
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)
void QGCHilXPlaneConfiguration::toggleSimulation(bool connect) void QGCHilXPlaneConfiguration::toggleSimulation(bool connect)
{ {
if (!link) if (!link) {
{
return; return;
} }
Q_UNUSED(connect); Q_UNUSED(connect);
if (!link->isConnected()) if (!link->isConnected())
{ {
link->setRemoteHost(ui->hostComboBox->currentText()); link->setRemoteHost(ui->hostComboBox->currentText());
link->connectSimulation(); link->connectSimulation();
ui->startButton->setText(tr("Disconnect")); ui->startButton->setText(tr("Disconnect"));
} }
else else
{ {
link->disconnectSimulation(); link->disconnectSimulation();

Write Preview
Loading…
Cancel
Save