/*===================================================================== QGroundControl Open Source Ground Control Station (c) 2009 - 2011 QGROUNDCONTROL PROJECT This file is part of the QGROUNDCONTROL project QGROUNDCONTROL is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. QGROUNDCONTROL is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with QGROUNDCONTROL. If not, see . ======================================================================*/ /** * @file QGCXPlaneLink.cc * Implementation of X-Plane interface * @author Lorenz Meier * */ #include #include #include #include #include #include #include "QGCXPlaneLink.h" #include "QGC.h" #include #include "UAS.h" #include "UASInterface.h" #include "QGCMessageBox.h" #include "HomePositionManager.h" QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress localHost, quint16 localPort) : mav(mav), remoteHost(QHostAddress("127.0.0.1")), remotePort(49000), socket(NULL), process(NULL), terraSync(NULL), barometerOffsetkPa(-8.0f), airframeID(QGCXPlaneLink::AIRFRAME_UNKNOWN), xPlaneConnected(false), xPlaneVersion(0), simUpdateLast(QGC::groundTimeMilliseconds()), simUpdateFirst(0), simUpdateLastText(QGC::groundTimeMilliseconds()), simUpdateLastGroundTruth(QGC::groundTimeMilliseconds()), simUpdateHz(0), _sensorHilEnabled(true), _should_exit(false) { // We're doing it wrong - because the Qt folks got the API wrong: // http://blog.qt.digia.com/blog/2010/06/17/youre-doing-it-wrong/ moveToThread(this); setTerminationEnabled(false); this->localHost = localHost; this->localPort = localPort/*+mav->getUASID()*/; connectState = false; this->name = tr("X-Plane Link (localPort:%1)").arg(localPort); setRemoteHost(remoteHost); loadSettings(); } QGCXPlaneLink::~QGCXPlaneLink() { storeSettings(); // Tell the thread to exit _should_exit = true; if (socket) { socket->close(); socket->deleteLater(); socket = NULL; } } void QGCXPlaneLink::loadSettings() { // Load defaults from settings QSettings settings; settings.beginGroup("QGC_XPLANE_LINK"); setRemoteHost(settings.value("REMOTE_HOST", QString("%1:%2").arg(remoteHost.toString()).arg(remotePort)).toString()); setVersion(settings.value("XPLANE_VERSION", 10).toInt()); selectAirframe(settings.value("AIRFRAME", "default").toString()); _sensorHilEnabled = settings.value("SENSOR_HIL", _sensorHilEnabled).toBool(); settings.endGroup(); } void QGCXPlaneLink::storeSettings() { // Store settings QSettings settings; settings.beginGroup("QGC_XPLANE_LINK"); settings.setValue("REMOTE_HOST", QString("%1:%2").arg(remoteHost.toString()).arg(remotePort)); settings.setValue("XPLANE_VERSION", xPlaneVersion); settings.setValue("AIRFRAME", airframeName); settings.setValue("SENSOR_HIL", _sensorHilEnabled); settings.endGroup(); } void QGCXPlaneLink::setVersion(const QString& version) { unsigned int oldVersion = xPlaneVersion; if (version.contains("9")) { xPlaneVersion = 9; } else if (version.contains("10")) { xPlaneVersion = 10; } else if (version.contains("11")) { xPlaneVersion = 11; } else if (version.contains("12")) { xPlaneVersion = 12; } if (oldVersion != xPlaneVersion) { emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion)); } } void QGCXPlaneLink::setVersion(unsigned int version) { bool changed = (xPlaneVersion != version); xPlaneVersion = version; if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion)); } /** * @brief Runs the thread * **/ void QGCXPlaneLink::run() { if (!mav) { emit statusMessage("No MAV present"); return; } if (connectState) { emit statusMessage("Already connected"); return; } socket = new QUdpSocket(this); socket->moveToThread(this); connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint); if (!connectState) { emit statusMessage("Binding socket failed!"); socket->deleteLater(); socket = NULL; return; } emit statusMessage(tr("Waiting for XPlane..")); QObject::connect(socket, SIGNAL(readyRead()), this, SLOT(readBytes())); UAS* uas = dynamic_cast(mav); if (uas) { connect(uas, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8)), Qt::QueuedConnection); connect(uas, SIGNAL(hilActuatorsChanged(quint64, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(quint64,float,float,float,float,float,float,float,float)), Qt::QueuedConnection); connect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), uas, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), Qt::QueuedConnection); connect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), uas, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), Qt::QueuedConnection); connect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), uas, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), Qt::QueuedConnection); connect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), uas, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), Qt::QueuedConnection); uas->startHil(); } else { emit statusMessage(tr("Failed to connect to drone instance")); return; } #pragma pack(push, 1) struct iset_struct { char b[5]; int index; // (0->20 in the lsit below) char str_ipad_them[16]; char str_port_them[6]; char padding[2]; int use_ip; } ip; // to use this option, 0 not to. #pragma pack(pop) ip.b[0] = 'I'; ip.b[1] = 'S'; ip.b[2] = 'E'; ip.b[3] = 'T'; ip.b[4] = '0'; QList hostAddresses = QNetworkInterface::allAddresses(); QString localAddrStr; QString localPortStr = QString("%1").arg(localPort); for (int i = 0; i < hostAddresses.size(); i++) { // Exclude loopback IPv4 and all IPv6 addresses if (hostAddresses.at(i) != QHostAddress("127.0.0.1") && !hostAddresses.at(i).toString().contains(":")) { localAddrStr = hostAddresses.at(i).toString(); break; } } ip.index = 0; strncpy(ip.str_ipad_them, localAddrStr.toLatin1(), qMin((int)sizeof(ip.str_ipad_them), 16)); strncpy(ip.str_port_them, localPortStr.toLatin1(), qMin((int)sizeof(ip.str_port_them), 6)); ip.use_ip = 1; writeBytes((const char*)&ip, sizeof(ip)); _should_exit = false; while(!_should_exit) { QCoreApplication::processEvents(); QGC::SLEEP::msleep(5); } uas = dynamic_cast(mav); if (uas) { disconnect(uas, SIGNAL(hilControlsChanged(quint64, float, float, float, float, quint8, quint8)), this, SLOT(updateControls(quint64,float,float,float,float,quint8,quint8))); disconnect(uas, SIGNAL(hilActuatorsChanged(quint64, float, float, float, float, float, float, float, float)), this, SLOT(updateActuators(quint64,float,float,float,float,float,float,float,float))); disconnect(this, SIGNAL(hilGroundTruthChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), uas, SLOT(sendHilGroundTruth(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); disconnect(this, SIGNAL(hilStateChanged(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float)), uas, SLOT(sendHilState(quint64,float,float,float,float,float,float,double,double,double,float,float,float,float,float,float,float,float))); disconnect(this, SIGNAL(sensorHilGpsChanged(quint64,double,double,double,int,float,float,float,float,float,float,float,int)), uas, SLOT(sendHilGps(quint64,double,double,double,int,float,float,float,float,float,float,float,int))); disconnect(this, SIGNAL(sensorHilRawImuChanged(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32)), uas, SLOT(sendHilSensors(quint64,float,float,float,float,float,float,float,float,float,float,float,float,float,quint32))); // Do not toggle HIL state on the UAS - this is not the job of this link, but of the // UAS object } connectState = false; QObject::disconnect(socket, SIGNAL(readyRead()), this, SLOT(readBytes())); socket->close(); socket->deleteLater(); socket = NULL; emit simulationDisconnected(); emit simulationConnected(false); } void QGCXPlaneLink::setPort(int localPort) { this->localPort = localPort; disconnectSimulation(); connectSimulation(); } void QGCXPlaneLink::processError(QProcess::ProcessError err) { QString msg; switch(err) { case QProcess::FailedToStart: msg = tr("X-Plane Failed to start. Please check if the path and command is correct"); break; case QProcess::Crashed: msg = tr("X-Plane crashed. This is an X-Plane-related problem, check for X-Plane upgrade."); break; case QProcess::Timedout: msg = tr("X-Plane start timed out. Please check if the path and command is correct"); break; case QProcess::ReadError: case QProcess::WriteError: msg = tr("Could not communicate with X-Plane. Please check if the path and command are correct"); break; case QProcess::UnknownError: default: msg = tr("X-Plane error occurred. Please check if the path and command is correct."); break; } QGCMessageBox::critical(tr("X-Plane HIL"), msg); } QString QGCXPlaneLink::getRemoteHost() { return QString("%1:%2").arg(remoteHost.toString()).arg(remotePort); } /** * @param newHost Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551 */ void QGCXPlaneLink::setRemoteHost(const QString& newHost) { if (newHost.length() == 0) return; if (newHost.contains(":")) { QHostInfo info = QHostInfo::fromName(newHost.split(":").first()); if (info.error() == QHostInfo::NoError) { // Add newHost QList newHostAddresses = info.addresses(); QHostAddress address; for (int i = 0; i < newHostAddresses.size(); i++) { // Exclude loopback IPv4 and all IPv6 addresses if (!newHostAddresses.at(i).toString().contains(":")) { address = newHostAddresses.at(i); } } remoteHost = address; // Set localPort according to user input remotePort = newHost.split(":").last().toInt(); } } else { QHostInfo info = QHostInfo::fromName(newHost); if (info.error() == QHostInfo::NoError) { // Add newHost remoteHost = info.addresses().first(); if (remotePort == 0) remotePort = 49000; } } if (isConnected()) { disconnectSimulation(); connectSimulation(); } emit remoteChanged(QString("%1:%2").arg(remoteHost.toString()).arg(remotePort)); } void QGCXPlaneLink::updateActuators(quint64 time, float act1, float act2, float act3, float act4, float act5, float act6, float act7, float act8) { if (mav->getSystemType() == MAV_TYPE_QUADROTOR) // Only update this for multirotors { Q_UNUSED(time); Q_UNUSED(act5); Q_UNUSED(act6); Q_UNUSED(act7); Q_UNUSED(act8); #pragma pack(push, 1) struct payload { char b[5]; int index; float f[8]; } p; #pragma pack(pop) p.b[0] = 'D'; p.b[1] = 'A'; p.b[2] = 'T'; p.b[3] = 'A'; p.b[4] = '\0'; p.index = 25; memset(p.f, 0, sizeof(p.f)); p.f[0] = act1; p.f[1] = act2; p.f[2] = act3; p.f[3] = act4; // XXX the system corrects for the scale onboard, do not scale again // if (airframeID == AIRFRAME_QUAD_X_MK_10INCH_I2C) // { // p.f[0] = act1 / 255.0f; // p.f[1] = act2 / 255.0f; // p.f[2] = act3 / 255.0f; // p.f[3] = act4 / 255.0f; // } // else if (airframeID == AIRFRAME_QUAD_X_ARDRONE) // { // p.f[0] = act1 / 500.0f; // p.f[1] = act2 / 500.0f; // p.f[2] = act3 / 500.0f; // p.f[3] = act4 / 500.0f; // } // else // { // p.f[0] = (act1 - 1000.0f) / 1000.0f; // p.f[1] = (act2 - 1000.0f) / 1000.0f; // p.f[2] = (act3 - 1000.0f) / 1000.0f; // p.f[3] = (act4 - 1000.0f) / 1000.0f; // } // Throttle writeBytes((const char*)&p, sizeof(p)); } } void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode) { #pragma pack(push, 1) struct payload { char b[5]; int index; float f[8]; } p; #pragma pack(pop) p.b[0] = 'D'; p.b[1] = 'A'; p.b[2] = 'T'; p.b[3] = 'A'; p.b[4] = '\0'; Q_UNUSED(time); Q_UNUSED(systemMode); Q_UNUSED(navMode); bool isFixedWing = true; if (mav->getAirframe() == UASInterface::QGC_AIRFRAME_X8 || mav->getAirframe() == UASInterface::QGC_AIRFRAME_VIPER_2_0 || mav->getAirframe() == UASInterface::QGC_AIRFRAME_CAMFLYER_Q) { // de-mix delta-mixed inputs // pitch input - mixed roll and pitch channels p.f[0] = 0.5f * (rollAilerons - pitchElevator); // roll input - mixed roll and pitch channels p.f[1] = 0.5f * (rollAilerons + pitchElevator); // yaw p.f[2] = 0.0f; } else if (mav->getSystemType() == MAV_TYPE_QUADROTOR) { qDebug() << "MAV_TYPE_QUADROTOR"; // Individual effort will be provided directly to the actuators on Xplane quadrotor. p.f[0] = yawRudder; p.f[1] = rollAilerons; p.f[2] = throttle; p.f[3] = pitchElevator; isFixedWing = false; } else { // direct pass-through, normal fixed-wing. p.f[0] = -pitchElevator; p.f[1] = rollAilerons; p.f[2] = yawRudder; } if(isFixedWing) { // Ail / Elevon / Rudder p.index = 12; // XPlane, wing sweep writeBytes((const char*)&p, sizeof(p)); p.index = 8; // XPlane, joystick? why? writeBytes((const char*)&p, sizeof(p)); p.index = 25; // Thrust memset(p.f, 0, sizeof(p.f)); p.f[0] = throttle; p.f[1] = throttle; p.f[2] = throttle; p.f[3] = throttle; // Throttle writeBytes((const char*)&p, sizeof(p)); } else { qDebug() << "Transmitting p.index = 25"; p.index = 25; // XPlane, throttle command. writeBytes((const char*)&p, sizeof(p)); } } Eigen::Matrix3f euler_to_wRo(double yaw, double pitch, double roll) { double c__ = cos(yaw); double _c_ = cos(pitch); double __c = cos(roll); double s__ = sin(yaw); double _s_ = sin(pitch); double __s = sin(roll); double cc_ = c__ * _c_; double cs_ = c__ * _s_; double sc_ = s__ * _c_; double ss_ = s__ * _s_; double c_c = c__ * __c; double c_s = c__ * __s; double s_c = s__ * __c; double s_s = s__ * __s; double _cc = _c_ * __c; double _cs = _c_ * __s; double csc = cs_ * __c; double css = cs_ * __s; double ssc = ss_ * __c; double sss = ss_ * __s; Eigen::Matrix3f wRo; wRo << cc_ , css-s_c, csc+s_s, sc_ , sss+c_c, ssc-c_s, -_s_ , _cs, _cc; return wRo; } void QGCXPlaneLink::writeBytes(const char* data, qint64 size) { if (!data) return; // If socket exists and is connected, transmit the data if (socket && connectState) { socket->writeDatagram(data, size, remoteHost, remotePort); } } /** * @brief Read all pending packets from the interface. **/ void QGCXPlaneLink::readBytes() { // Only emit updates on attitude message bool emitUpdate = false; quint16 fields_changed = 0; const qint64 maxLength = 1000; char data[maxLength]; QHostAddress sender; quint16 senderPort; unsigned int s = socket->pendingDatagramSize(); if (s > maxLength) std::cerr << __FILE__ << __LINE__ << " UDP datagram overflow, allowed to read less bytes than datagram size: " << s << std::endl; socket->readDatagram(data, maxLength, &sender, &senderPort); if (s > maxLength) { std::string headStr = std::string(data, data+5); std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl; } // Calculate the number of data segments a 36 bytes // XPlane always has 5 bytes header: 'DATA@' unsigned nsegs = (s-5)/36; //qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs; #pragma pack(push, 1) struct payload { int index; float f[8]; } p; #pragma pack(pop) bool oldConnectionState = xPlaneConnected; if (data[0] == 'D' && data[1] == 'A' && data[2] == 'T' && data[3] == 'A') { xPlaneConnected = true; if (oldConnectionState != xPlaneConnected) { simUpdateFirst = QGC::groundTimeMilliseconds(); } for (unsigned i = 0; i < nsegs; i++) { // Get index unsigned ioff = (5+i*36);; memcpy(&(p), data+ioff, sizeof(p)); if (p.index == 3) { ind_airspeed = p.f[5] * 0.44704f; true_airspeed = p.f[6] * 0.44704f; groundspeed = p.f[7] * 0.44704; //qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s"; } if (p.index == 4) { // WORKAROUND: IF ground speed <<1m/s and altitude-above-ground <1m, do NOT use the X-Plane data, because X-Plane (tested // with v10.3 and earlier) delivers yacc=0 and zacc=0 when the ground speed is very low, which gives e.g. wrong readings // before launch when waiting on the runway. This might pose a problem for initial state estimation/calibration. // Instead, we calculate our own accelerations. if (fabsf(groundspeed)<0.1f && alt_agl<1.0) { // TODO: Add centrip. acceleration to the current static acceleration implementation. Eigen::Vector3f g(0, 0, -9.81f); Eigen::Matrix3f R = euler_to_wRo(yaw, pitch, roll); Eigen::Vector3f gr = R.transpose().eval() * g; xacc = gr[0]; yacc = gr[1]; zacc = gr[2]; //qDebug() << "Calculated values" << gr[0] << gr[1] << gr[2]; } else { // Accelerometer readings, directly from X-Plane and including centripetal forces. const float one_g = 9.80665f; xacc = p.f[5] * one_g; yacc = p.f[6] * one_g; zacc = -p.f[4] * one_g; //qDebug() << "X-Plane values" << xacc << yacc << zacc; } fields_changed |= (1 << 0) | (1 << 1) | (1 << 2); } // atmospheric pressure aircraft for XPlane 9 and 10 else if (p.index == 6) { // inHg to hPa (hecto Pascal / millibar) abs_pressure = p.f[0] * 33.863886666718317f; temperature = p.f[1]; fields_changed |= (1 << 9) | (1 << 12); } // Forward controls from X-Plane to MAV, not very useful // better: Connect Joystick to QGroundControl // else if (p.index == 8) // { // //qDebug() << "MAN:" << p.f[0] << p.f[3] << p.f[7]; // man_roll = p.f[0]; // man_pitch = p.f[1]; // man_yaw = p.f[2]; // UAS* uas = dynamic_cast(mav); // if (uas) uas->setManualControlCommands(man_roll, man_pitch, man_yaw, 0.6); // } else if ((xPlaneVersion == 10 && p.index == 16) || (xPlaneVersion == 9 && p.index == 17)) { // Cross checked with XPlane flight pitchspeed = p.f[0]; rollspeed = p.f[1]; yawspeed = p.f[2]; fields_changed |= (1 << 3) | (1 << 4) | (1 << 5); } 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]; pitch = p.f[0] / 180.0f * M_PI; roll = p.f[1] / 180.0f * M_PI; yaw = p.f[2] / 180.0f * M_PI; // X-Plane expresses yaw as 0..2 PI if (yaw > M_PI) { yaw -= 2.0f * static_cast(M_PI); } if (yaw < -M_PI) { yaw += 2.0f * static_cast(M_PI); } float yawmag = p.f[3] / 180.0f * M_PI; if (yawmag > M_PI) { yawmag -= 2.0f * static_cast(M_PI); } if (yawmag < -M_PI) { yawmag += 2.0f * static_cast(M_PI); } // Normal rotation matrix, but since we rotate the // vector [0.25 0 0.45]', we end up with these relevant // matrix parts. xmag = cos(-yawmag) * 0.25f; ymag = sin(-yawmag) * 0.25f; zmag = 0.45f; fields_changed |= (1 << 6) | (1 << 7) | (1 << 8); double cosPhi = cos(roll); double sinPhi = sin(roll); double cosThe = cos(pitch); double sinThe = sin(pitch); double cosPsi = cos(0.0); double sinPsi = sin(0.0); float dcm[3][3]; dcm[0][0] = cosThe * cosPsi; dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; dcm[1][0] = cosThe * sinPsi; dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; dcm[2][0] = -sinThe; dcm[2][1] = sinPhi * cosThe; dcm[2][2] = cosPhi * cosThe; Eigen::Matrix3f m = Eigen::Map((float*)dcm).eval(); Eigen::Vector3f mag(xmag, ymag, zmag); Eigen::Vector3f magbody = m * mag; // qDebug() << "yaw mag:" << p.f[2] << "x" << xmag << "y" << ymag; // qDebug() << "yaw mag in body:" << magbody(0) << magbody(1) << magbody(2); xmag = magbody(0); ymag = magbody(1); zmag = magbody(2); // Rotate the measurement vector into the body frame using roll and pitch emitUpdate = true; } // else if (p.index == 19) // { // qDebug() << "ATT:" << p.f[0] << p.f[1] << p.f[2]; // } else if (p.index == 20) { //qDebug() << "LAT/LON/ALT:" << p.f[0] << p.f[1] << p.f[2]; lat = p.f[0]; lon = p.f[1]; alt = p.f[2] * 0.3048f; // convert feet (MSL) to meters alt_agl = p.f[3] * 0.3048f; //convert feet (AGL) to meters } else if (p.index == 21) { vy = p.f[3]; vx = -p.f[5]; // moving 'up' in XPlane is positive, but its negative in NED // for us. vz = -p.f[4]; } else if (p.index == 12) { //qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2]; } else if (p.index == 25) { //qDebug() << "THROTTLE" << p.f[0] << p.f[1] << p.f[2] << p.f[3]; } else if (p.index == 0) { //qDebug() << "STATS" << "fgraphics/s" << p.f[0] << "fsim/s" << p.f[2] << "t frame" << p.f[3] << "cpu load" << p.f[4] << "grnd ratio" << p.f[5] << "filt ratio" << p.f[6]; } else if (p.index == 11) { //qDebug() << "CONTROLS" << "ail" << p.f[0] << "elev" << p.f[1] << "rudder" << p.f[2] << "nwheel" << p.f[3]; } else { //qDebug() << "UNKNOWN #" << p.index << p.f[0] << p.f[1] << p.f[2] << p.f[3]; } } } else if (data[0] == 'S' && data[1] == 'N' && data[2] == 'A' && data[3] == 'P') { } else if (data[0] == 'S' && data[1] == 'T' && data[2] == 'A' && data[3] == 'T') { } else { qDebug() << "UNKNOWN PACKET:" << data; } // Wait for 0.5s before actually using the data, so that all fields are filled if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) { return; } // Send updated state if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 2) { simUpdateHz = simUpdateHz * 0.9f + 0.1f * (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000) { emit statusMessage(tr("Receiving from XPlane at %1 Hz").arg(static_cast(simUpdateHz))); // Reset lowpass with current value simUpdateHz = (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); // Set state simUpdateLastText = QGC::groundTimeMilliseconds(); } simUpdateLast = QGC::groundTimeMilliseconds(); if (_sensorHilEnabled) { diff_pressure = (ind_airspeed * ind_airspeed * 1.225f) / 2.0f; /* tropospheric properties (0-11km) for standard atmosphere */ const double T1 = 15.0 + 273.15; /* temperature at base height in Kelvin */ const double a = -6.5 / 1000; /* temperature gradient in degrees per metre */ const double g = 9.80665; /* gravity constant in m/s/s */ const double R = 287.05; /* ideal gas constant in J/kg/K */ /* current pressure at MSL in kPa */ double p1 = 1013.25 / 10.0; /* measured pressure in hPa, plus offset to simulate weather effects / offsets */ double p = abs_pressure / 10.0 + barometerOffsetkPa; /* * Solve: * * / -(aR / g) \ * | (p / p1) . T1 | - T1 * \ / * h = ------------------------------- + h1 * a */ pressure_alt = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; // set pressure alt to changed fields_changed |= (1 << 11); emit sensorHilRawImuChanged(QGC::groundTimeUsecs(), xacc, yacc, zacc, rollspeed, pitchspeed, yawspeed, xmag, ymag, zmag, abs_pressure, diff_pressure / 100.0, pressure_alt, temperature, fields_changed); // XXX make these GUI-configurable and add randomness int gps_fix_type = 3; float eph = 0.3f; float epv = 0.6f; float vel = sqrt(vx*vx + vy*vy + vz*vz); float cog = atan2(vy, vx); int satellites = 8; emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites); } else { emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc); } // Limit ground truth to 25 Hz if (QGC::groundTimeMilliseconds() - simUpdateLastGroundTruth > 40) { emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc); simUpdateLastGroundTruth = QGC::groundTimeMilliseconds(); } } if (!oldConnectionState && xPlaneConnected) { emit statusMessage(tr("Receiving from XPlane.")); } // // Echo data for debugging purposes // std::cerr << __FILE__ << __LINE__ << "Received datagram:" << std::endl; // int i; // for (i=0; ipendingDatagramSize(); } /** * @brief Disconnect the connection. * * @return True if connection has been disconnected, false if connection couldn't be disconnected. **/ bool QGCXPlaneLink::disconnectSimulation() { if (connectState) { _should_exit = true; } else { emit simulationDisconnected(); emit simulationConnected(false); } return !connectState; } void QGCXPlaneLink::selectAirframe(const QString& plane) { airframeName = plane; if (plane.contains("QRO")) { if (plane.contains("MK") && airframeID != AIRFRAME_QUAD_X_MK_10INCH_I2C) { airframeID = AIRFRAME_QUAD_X_MK_10INCH_I2C; emit airframeChanged("QRO_X / MK"); } else if (plane.contains("ARDRONE") && airframeID != AIRFRAME_QUAD_X_ARDRONE) { airframeID = AIRFRAME_QUAD_X_ARDRONE; emit airframeChanged("QRO_X / ARDRONE"); } else { bool changed = (airframeID != AIRFRAME_QUAD_DJI_F450_PWM); airframeID = AIRFRAME_QUAD_DJI_F450_PWM; if (changed) emit airframeChanged("QRO_X / DJI-F450 / PWM"); } } else { bool changed = (airframeID != AIRFRAME_UNKNOWN); airframeID = AIRFRAME_UNKNOWN; if (changed) emit airframeChanged("X Plane default"); } } void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw) { #pragma pack(push, 1) struct VEH1_struct { char header[5]; quint32 p; double lat_lon_ele[3]; float psi_the_phi[3]; float gear_flap_vect[3]; } pos; #pragma pack(pop) pos.header[0] = 'V'; pos.header[1] = 'E'; pos.header[2] = 'H'; pos.header[3] = '1'; pos.header[4] = '0'; pos.p = 0; pos.lat_lon_ele[0] = lat; pos.lat_lon_ele[1] = lon; pos.lat_lon_ele[2] = alt; pos.psi_the_phi[0] = roll; pos.psi_the_phi[1] = pitch; pos.psi_the_phi[2] = yaw; pos.gear_flap_vect[0] = 0.0f; pos.gear_flap_vect[1] = 0.0f; pos.gear_flap_vect[2] = 0.0f; writeBytes((const char*)&pos, sizeof(pos)); // pos.header[0] = 'V'; // pos.header[1] = 'E'; // pos.header[2] = 'H'; // pos.header[3] = '1'; // pos.header[4] = '0'; // pos.p = 0; // pos.lat_lon_ele[0] = -999; // pos.lat_lon_ele[1] = -999; // pos.lat_lon_ele[2] = -999; // pos.psi_the_phi[0] = -999; // pos.psi_the_phi[1] = -999; // pos.psi_the_phi[2] = -999; // pos.gear_flap_vect[0] = -999; // pos.gear_flap_vect[1] = -999; // pos.gear_flap_vect[2] = -999; // writeBytes((const char*)&pos, sizeof(pos)); } /** * Sets a random position with an offset of max 1/1000 degree * and max 100 m altitude */ void QGCXPlaneLink::setRandomPosition() { // Initialize generator srand(0); double offLat = rand() / static_cast(RAND_MAX) / 500.0 + 1.0/500.0; double offLon = rand() / static_cast(RAND_MAX) / 500.0 + 1.0/500.0; double offAlt = rand() / static_cast(RAND_MAX) * 200.0 + 100.0; if (mav->getAltitudeAMSL() + offAlt < 0) { offAlt *= -1.0; } setPositionAttitude(mav->getLatitude() + offLat, mav->getLongitude() + offLon, mav->getAltitudeAMSL() + offAlt, mav->getRoll(), mav->getPitch(), mav->getYaw()); } void QGCXPlaneLink::setRandomAttitude() { // Initialize generator srand(0); double roll = rand() / static_cast(RAND_MAX) * 2.0 - 1.0; double pitch = rand() / static_cast(RAND_MAX) * 2.0 - 1.0; double yaw = rand() / static_cast(RAND_MAX) * 2.0 - 1.0; setPositionAttitude(mav->getLatitude(), mav->getLongitude(), mav->getAltitudeAMSL(), roll, pitch, yaw); } /** * @brief Connect the connection. * * @return True if connection has been established, false if connection couldn't be established. **/ bool QGCXPlaneLink::connectSimulation() { if (connectState) { qDebug() << "Simulation already active"; } else { qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort; // XXX Hack storeSettings(); start(HighPriority); } return true; } /** * @brief Check if connection is active. * * @return True if link is connected, false otherwise. **/ bool QGCXPlaneLink::isConnected() { return connectState; } QString QGCXPlaneLink::getName() { return name; } void QGCXPlaneLink::setName(QString name) { this->name = name; // emit nameChanged(this->name); }