diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index 7fb5e91f79fddf1fea542c8dd4b056e2198b6bac..74838577fd8407d2f3df2afdf2a2c4e8feaf6527 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -32,7 +32,7 @@ #include "QGCMessageBox.h" #include "HomePositionManager.h" -QGCXPlaneLink::QGCXPlaneLink(Vehicle *vehicle, QString remoteHost, QHostAddress localHost, quint16 localPort) : +QGCXPlaneLink::QGCXPlaneLink(Vehicle* vehicle, QString remoteHost, QHostAddress localHost, quint16 localPort) : _vehicle(vehicle), remoteHost(QHostAddress("127.0.0.1")), remotePort(49000), @@ -73,8 +73,7 @@ QGCXPlaneLink::~QGCXPlaneLink() // Tell the thread to exit _should_exit = true; - if (socket) - { + if (socket) { socket->close(); socket->deleteLater(); socket = NULL; @@ -107,25 +106,21 @@ void QGCXPlaneLink::storeSettings() settings.endGroup(); } -void QGCXPlaneLink::setVersion(const QString &version) +void QGCXPlaneLink::setVersion(const QString& version) { unsigned int oldVersion = xPlaneVersion; - if (version.contains("9")) { xPlaneVersion = 9; } - else if (version.contains("10")) { xPlaneVersion = 10; } - else if (version.contains("11")) { xPlaneVersion = 11; } - else if (version.contains("12")) { xPlaneVersion = 12; @@ -141,23 +136,19 @@ void QGCXPlaneLink::setVersion(unsigned int version) { bool changed = (xPlaneVersion != version); xPlaneVersion = version; - if (changed) emit versionChanged(QString("X-Plane %1").arg(xPlaneVersion)); } void QGCXPlaneLink::enableHilActuatorControls(bool enable) { - if (enable != _useHilActuatorControls) - { + if (enable != _useHilActuatorControls) { _useHilActuatorControls = enable; } /* Only use override for new message and specific airframes */ MAV_TYPE type = _vehicle->vehicleType(); float value = 0.0f; - - if (type == MAV_TYPE_VTOL_RESERVED2) - { + if (type == MAV_TYPE_VTOL_RESERVED2) { value = (enable ? 1.0f : 0.0f); } @@ -172,14 +163,12 @@ void QGCXPlaneLink::enableHilActuatorControls(bool enable) **/ void QGCXPlaneLink::run() { - if (!_vehicle) - { + if (!_vehicle) { emit statusMessage("No MAV present"); return; } - if (connectState) - { + if (connectState) { emit statusMessage("Already connected"); return; } @@ -187,9 +176,7 @@ void QGCXPlaneLink::run() socket = new QUdpSocket(this); socket->moveToThread(this); connectState = socket->bind(localHost, localPort, QAbstractSocket::ReuseAddressHint); - - if (!connectState) - { + if (!connectState) { emit statusMessage("Binding socket failed!"); @@ -250,15 +237,14 @@ void QGCXPlaneLink::run() strncpy(ip.str_port_them, localPortStr.toLatin1(), qMin((int)sizeof(ip.str_port_them), 6)); ip.use_ip = 1; - writeBytesSafe((const char *)&ip, sizeof(ip)); + writeBytesSafe((const char*)&ip, sizeof(ip)); /* Call function which makes sure individual control override is enabled/disabled */ enableHilActuatorControls(_useHilActuatorControls); _should_exit = false; - while (!_should_exit) - { + while(!_should_exit) { QCoreApplication::processEvents(); QGC::SLEEP::msleep(5); } @@ -291,33 +277,32 @@ void QGCXPlaneLink::setPort(int localPort) 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; + + 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); } @@ -329,7 +314,7 @@ QString QGCXPlaneLink::getRemoteHost() /** * @param newHost Hostname in standard formatting, e.g. localhost:14551 or 192.168.1.1:14551 */ -void QGCXPlaneLink::setRemoteHost(const QString &newHost) +void QGCXPlaneLink::setRemoteHost(const QString& newHost) { if (newHost.length() == 0) return; @@ -337,13 +322,11 @@ void QGCXPlaneLink::setRemoteHost(const QString &newHost) 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 @@ -352,22 +335,18 @@ void QGCXPlaneLink::setRemoteHost(const QString &newHost) address = newHostAddresses.at(i); } } - remoteHost = address; // Set localPort according to user input remotePort = newHost.split(":").last().toInt(); } } - else { QHostInfo info = QHostInfo::fromName(newHost); - if (info.error() == QHostInfo::NoError) { // Add newHost remoteHost = info.addresses().first(); - if (remotePort == 0) remotePort = 49000; } } @@ -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) { /* Only use HIL_CONTROL when the checkbox is unchecked */ - if (_useHilActuatorControls) - { + if (_useHilActuatorControls) { //qDebug() << "received HIL_CONTROL but not using it"; return; } - -#pragma pack(push, 1) - struct payload - { + #pragma pack(push, 1) + struct payload { char b[5]; int index; float f[8]; } p; -#pragma pack(pop) + #pragma pack(pop) p.b[0] = 'D'; p.b[1] = 'A'; @@ -410,8 +386,8 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch Q_UNUSED(navMode); if (_vehicle->vehicleType() == MAV_TYPE_QUADROTOR - || _vehicle->vehicleType() == MAV_TYPE_HEXAROTOR - || _vehicle->vehicleType() == MAV_TYPE_OCTOROTOR) + || _vehicle->vehicleType() == MAV_TYPE_HEXAROTOR + || _vehicle->vehicleType() == MAV_TYPE_OCTOROTOR) { qDebug() << "MAV_TYPE_QUADROTOR"; @@ -423,9 +399,8 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch // Direct throttle control p.index = 25; - writeBytesSafe((const char *)&p, sizeof(p)); + writeBytesSafe((const char*)&p, sizeof(p)); } - else { // direct pass-through, normal fixed-wing. @@ -437,11 +412,11 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch // Send to group 12 p.index = 12; - writeBytesSafe((const char *)&p, sizeof(p)); + writeBytesSafe((const char*)&p, sizeof(p)); // Send to group 8, which equals manual controls p.index = 8; - writeBytesSafe((const char *)&p, sizeof(p)); + writeBytesSafe((const char*)&p, sizeof(p)); // Send throttle to all four motors p.index = 25; @@ -450,14 +425,13 @@ void QGCXPlaneLink::updateControls(quint64 time, float rollAilerons, float pitch p.f[1] = throttle; p.f[2] = throttle; p.f[3] = throttle; - writeBytesSafe((const char *)&p, sizeof(p)); + writeBytesSafe((const char*)&p, sizeof(p)); } } void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode) { - if (!_useHilActuatorControls) - { + if (!_useHilActuatorControls) { //qDebug() << "received HIL_ACTUATOR_CONTROLS but not using it"; return; } @@ -470,14 +444,13 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct Q_UNUSED(ctl_14); Q_UNUSED(ctl_15); -#pragma pack(push, 1) - struct payload - { + #pragma pack(push, 1) + struct payload { char b[5]; int index; float f[8]; } p; -#pragma pack(pop) + #pragma pack(pop) p.b[0] = 'D'; p.b[1] = 'A'; @@ -488,11 +461,10 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct /* Initialize with zeroes */ memset(p.f, 0, sizeof(p.f)); - switch (_vehicle->vehicleType()) - { - case MAV_TYPE_QUADROTOR: - case MAV_TYPE_HEXAROTOR: - case MAV_TYPE_OCTOROTOR: + switch (_vehicle->vehicleType()) { + case MAV_TYPE_QUADROTOR: + case MAV_TYPE_HEXAROTOR: + case MAV_TYPE_OCTOROTOR: { p.f[0] = ctl_0; ///< X-Plane Engine 1 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 */ p.index = 25; - writeBytesSafe((const char *)&p, sizeof(p)); + writeBytesSafe((const char*)&p, sizeof(p)); break; } - - case MAV_TYPE_VTOL_RESERVED2: + case MAV_TYPE_VTOL_RESERVED2: { /** * 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[7] = ctl_7; p.index = 25; - writeBytesSafe((const char *)&p, sizeof(p)); + writeBytesSafe((const char*)&p, sizeof(p)); /* Control individual actuators */ float max_surface_deflection = 30.0f; // Degrees @@ -536,8 +507,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct break; } - - default: + default: { /* direct pass-through, normal fixed-wing. */ 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 */ p.index = 8; - writeBytesSafe((const char *)&p, sizeof(p)); + writeBytesSafe((const char*)&p, sizeof(p)); /* Send throttle to all eight motors */ p.index = 25; @@ -558,7 +528,7 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct p.f[5] = ctl_3; p.f[6] = ctl_3; p.f[7] = ctl_3; - writeBytesSafe((const char *)&p, sizeof(p)); + writeBytesSafe((const char*)&p, sizeof(p)); break; } @@ -566,34 +536,33 @@ void QGCXPlaneLink::updateActuatorControls(quint64 time, quint64 flags, float ct } -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; +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 QByteArray data) @@ -622,30 +591,25 @@ void QGCXPlaneLink::readBytes() quint16 senderPort; unsigned int s = socket->pendingDatagramSize(); - if (s > maxLength) std::cerr << __FILE__ << __LINE__ << " UDP datagram overflow, allowed to read less bytes than datagram size: " << s << std::endl; - socket->readDatagram(data, maxLength, &sender, &senderPort); - - if (s > maxLength) - { - std::string headStr = std::string(data, data + 5); - std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl; + if (s > maxLength) { + std::string headStr = std::string(data, data+5); + std::cerr << __FILE__ << __LINE__ << " UDP datagram header: " << headStr << std::endl; } // Calculate the number of data segments a 36 bytes // XPlane always has 5 bytes header: 'DATA@' - unsigned nsegs = (s - 5) / 36; + unsigned nsegs = (s-5)/36; //qDebug() << "XPLANE:" << "LEN:" << s << "segs:" << nsegs; -#pragma pack(push, 1) - struct payload - { + #pragma pack(push, 1) + struct payload { int index; float f[8]; } p; -#pragma pack(pop) + #pragma pack(pop) bool oldConnectionState = xPlaneConnected; @@ -656,16 +620,15 @@ void QGCXPlaneLink::readBytes() { xPlaneConnected = true; - if (oldConnectionState != xPlaneConnected) - { + if (oldConnectionState != xPlaneConnected) { simUpdateFirst = QGC::groundTimeMilliseconds(); } for (unsigned i = 0; i < nsegs; i++) { // Get index - unsigned ioff = (5 + i * 36);; - memcpy(&(p), data + ioff, sizeof(p)); + unsigned ioff = (5+i*36);; + memcpy(&(p), data+ioff, sizeof(p)); if (p.index == 3) { @@ -675,42 +638,39 @@ void QGCXPlaneLink::readBytes() //qDebug() << "SPEEDS:" << "airspeed" << airspeed << "m/s, groundspeed" << groundspeed << "m/s"; } - if (p.index == 4) { - // WORKAROUND: IF ground speed <<1m/s and altitude-above-ground <1m, do NOT use the X-Plane data, because X-Plane (tested - // with v10.3 and earlier) delivers yacc=0 and zacc=0 when the ground speed is very low, which gives e.g. wrong readings - // before launch when waiting on the runway. This might pose a problem for initial state estimation/calibration. - // Instead, we calculate our own accelerations. - if (fabsf(groundspeed) < 0.1f && alt_agl < 1.0) - { - // TODO: Add centrip. acceleration to the current static acceleration implementation. + // 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.80665f); - 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); + 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); emitUpdate = true; } - // atmospheric pressure aircraft for XPlane 9 and 10 else if (p.index == 6) { @@ -719,7 +679,6 @@ void QGCXPlaneLink::readBytes() temperature = p.f[1]; fields_changed |= (1 << 9) | (1 << 12); } - // Forward controls from X-Plane to MAV, not very useful // better: Connect Joystick to QGroundControl // else if (p.index == 8) @@ -741,7 +700,6 @@ void QGCXPlaneLink::readBytes() emitUpdate = true; } - else if ((xPlaneVersion == 10 && p.index == 17) || (xPlaneVersion == 9 && p.index == 18)) { //qDebug() << "HDNG" << "pitch" << p.f[0] << "roll" << p.f[1] << "hding true" << p.f[2] << "hding mag" << p.f[3]; @@ -750,25 +708,19 @@ void QGCXPlaneLink::readBytes() yaw = p.f[2] / 180.0f * M_PI; // X-Plane expresses yaw as 0..2 PI - if (yaw > M_PI) - { + if (yaw > M_PI) { yaw -= 2.0f * static_cast(M_PI); } - - if (yaw < -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) - { + if (yawmag > M_PI) { yawmag -= 2.0f * static_cast(M_PI); } - - if (yawmag < -M_PI) - { + if (yawmag < -M_PI) { yawmag += 2.0f * static_cast(M_PI); } @@ -802,7 +754,7 @@ void QGCXPlaneLink::readBytes() dcm[2][1] = sinPhi * cosThe; dcm[2][2] = cosPhi * cosThe; - Eigen::Matrix3f m = Eigen::Map((float *)dcm).eval(); + Eigen::Matrix3f m = Eigen::Map((float*)dcm).eval(); Eigen::Vector3f mag(xmag, ymag, zmag); @@ -825,15 +777,14 @@ void QGCXPlaneLink::readBytes() // { // 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 == 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]; @@ -842,7 +793,6 @@ void QGCXPlaneLink::readBytes() // for us. vz = -p.f[4]; } - else if (p.index == 12) { //qDebug() << "AIL/ELEV/RUD" << p.f[0] << p.f[1] << p.f[2]; @@ -873,9 +823,9 @@ void QGCXPlaneLink::readBytes() } else if (data[0] == 'S' && - data[1] == 'T' && - data[2] == 'A' && - data[3] == 'T') + data[1] == 'T' && + data[2] == 'A' && + 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 - if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) - { + if (QGC::groundTimeMilliseconds() - simUpdateFirst < 500) { return; } @@ -894,9 +843,7 @@ void QGCXPlaneLink::readBytes() if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 2) { simUpdateHz = simUpdateHz * 0.9f + 0.1f * (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); - - if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000) - { + if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000) { emit statusMessage(tr("Receiving from XPlane at %1 Hz").arg(static_cast(simUpdateHz))); // Reset lowpass with current value simUpdateHz = (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); @@ -943,23 +890,19 @@ void QGCXPlaneLink::readBytes() int gps_fix_type = 3; float eph = 0.3f; float epv = 0.6f; - float vel = sqrt(vx * vx + vy * vy + vz * vz); + float vel = sqrt(vx*vx + vy*vy + vz*vz); float cog = atan2(vy, vx); int satellites = 8; emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, vx, vy, vz, cog, satellites); - } - - else - { + } else { emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc); } // Limit ground truth to 25 Hz - if (QGC::groundTimeMilliseconds() - simUpdateLastGroundTruth > 40) - { + if (QGC::groundTimeMilliseconds() - simUpdateLastGroundTruth > 40) { emit hilGroundTruthChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, ind_airspeed, true_airspeed, xacc, yacc, zacc); @@ -1005,10 +948,7 @@ bool QGCXPlaneLink::disconnectSimulation() if (connectState) { _should_exit = true; - } - - else - { + } else { emit simulationDisconnected(); emit simulationConnected(false); } @@ -1016,7 +956,7 @@ bool QGCXPlaneLink::disconnectSimulation() return !connectState; } -void QGCXPlaneLink::selectAirframe(const QString &plane) +void QGCXPlaneLink::selectAirframe(const QString& plane) { airframeName = plane; @@ -1027,34 +967,29 @@ void QGCXPlaneLink::selectAirframe(const QString &plane) airframeID = AIRFRAME_QUAD_X_MK_10INCH_I2C; emit airframeChanged("QRO_X / MK"); } - else if (plane.contains("ARDRONE") && airframeID != AIRFRAME_QUAD_X_ARDRONE) { airframeID = AIRFRAME_QUAD_X_ARDRONE; emit airframeChanged("QRO_X / ARDRONE"); } - else { bool changed = (airframeID != AIRFRAME_QUAD_DJI_F450_PWM); airframeID = AIRFRAME_QUAD_DJI_F450_PWM; - if (changed) emit airframeChanged("QRO_X / DJI-F450 / PWM"); } } - else { bool changed = (airframeID != AIRFRAME_UNKNOWN); airframeID = AIRFRAME_UNKNOWN; - if (changed) emit airframeChanged("X Plane default"); } } void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, double roll, double pitch, double yaw) { -#pragma pack(push, 1) + #pragma pack(push, 1) struct VEH1_struct { char header[5]; @@ -1063,7 +998,7 @@ void QGCXPlaneLink::setPositionAttitude(double lat, double lon, double alt, doub float psi_the_phi[3]; float gear_flap_vect[3]; } pos; -#pragma pack(pop) + #pragma pack(pop) pos.header[0] = 'V'; pos.header[1] = 'E'; @@ -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[2] = 0.0f; - writeBytesSafe((const char *)&pos, sizeof(pos)); + writeBytesSafe((const char*)&pos, sizeof(pos)); // pos.header[0] = 'V'; // pos.header[1] = 'E'; @@ -1111,8 +1046,8 @@ 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 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 (_vehicle->altitudeAMSL()->rawValue().toDouble() + offAlt < 0) @@ -1152,13 +1087,9 @@ void QGCXPlaneLink::setRandomAttitude() **/ bool QGCXPlaneLink::connectSimulation() { - if (connectState) - { + if (connectState) { qDebug() << "Simulation already active"; - } - - else - { + } else { qDebug() << "STARTING X-PLANE LINK, CONNECTING TO" << remoteHost << ":" << remotePort; // XXX Hack storeSettings(); @@ -1192,14 +1123,13 @@ void QGCXPlaneLink::setName(QString name) void QGCXPlaneLink::sendDataRef(QString ref, float value) { -#pragma pack(push, 1) - struct payload - { + #pragma pack(push, 1) + struct payload { char b[5]; float value; char name[500]; } dref; -#pragma pack(pop) + #pragma pack(pop) dref.b[0] = 'D'; dref.b[1] = 'R'; @@ -1217,16 +1147,12 @@ void QGCXPlaneLink::sendDataRef(QString ref, float value) /* Send command */ QByteArray ba = ref.toUtf8(); - - if (ba.length() > 500) - { + if (ba.length() > 500) { return; } - for (int i = 0; i < ba.length(); i++) - { + for (int i = 0; i < ba.length(); i++) { dref.name[i] = ba.at(i); } - - writeBytesSafe((const char *)&dref, sizeof(dref)); + writeBytesSafe((const char*)&dref, sizeof(dref)); } diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h index 6fa5640e49914d1222deb30b67c8e55238442f84..e6f9222107cd92a49bd94eae31fdfdde706c8a9f 100644 --- a/src/comm/QGCXPlaneLink.h +++ b/src/comm/QGCXPlaneLink.h @@ -36,7 +36,7 @@ class QGCXPlaneLink : public QGCHilLink //Q_INTERFACES(QGCXPlaneLinkInterface:LinkInterface) public: - QGCXPlaneLink(Vehicle *vehicle, QString remoteHost = QString("127.0.0.1:49000"), QHostAddress localHost = QHostAddress::Any, quint16 localPort = 49005); + QGCXPlaneLink(Vehicle* vehicle, QString remoteHost=QString("127.0.0.1:49000"), QHostAddress localHost = QHostAddress::Any, quint16 localPort = 49005); ~QGCXPlaneLink(); /** @@ -51,8 +51,7 @@ public: bool isConnected(); qint64 bytesAvailable(); - int getPort() const - { + int getPort() const { return localPort; } @@ -89,13 +88,11 @@ public: return (int)airframeID; } - bool sensorHilEnabled() - { + bool sensorHilEnabled() { return _sensorHilEnabled; } - bool useHilActuatorControls() - { + bool useHilActuatorControls() { return _useHilActuatorControls; } @@ -107,7 +104,7 @@ public slots: // void setAddress(QString address); void setPort(int port); /** @brief Add a new host to broadcast messages to */ - void setRemoteHost(const QString &host); + void setRemoteHost(const QString& host); /** @brief Send new control states to the simulation */ void updateControls(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode); /** @brief Send new control commands to the simulation */ @@ -130,16 +127,14 @@ public slots: float ctl_15, quint8 mode); /** @brief Set the simulator version as text string */ - void setVersion(const QString &version); + void setVersion(const QString& version); /** @brief Set the simulator version as integer */ void setVersion(unsigned int version); - void enableSensorHIL(bool enable) - { + void enableSensorHIL(bool enable) { if (enable != _sensorHilEnabled) _sensorHilEnabled = enable; - - emit sensorHilChanged(enable); + emit sensorHilChanged(enable); } void enableHilActuatorControls(bool enable); @@ -164,7 +159,7 @@ public slots: * @brief Select airplane model * @param plane the name of the airplane */ - void selectAirframe(const QString &airframe); + void selectAirframe(const QString& airframe); /** * @brief Set the airplane position and attitude * @param lat @@ -187,14 +182,14 @@ public slots: void setRandomAttitude(); protected: - Vehicle *_vehicle; + Vehicle* _vehicle; QString name; QHostAddress localHost; quint16 localPort; QHostAddress remoteHost; quint16 remotePort; int id; - QUdpSocket *socket; + QUdpSocket* socket; bool connectState; quint64 bitsSentTotal; @@ -207,8 +202,8 @@ protected: QMutex statisticsMutex; QMutex dataMutex; QTimer refreshTimer; - QProcess *process; - QProcess *terraSync; + QProcess* process; + QProcess* terraSync; bool gpsReceived; bool attitudeReceived; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 275775715dfd951afabafb29c83ac22316290b6e..8b760d2c72402ad6d66164603bd20bfdab5fb2bb 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1,2171 +1,2018 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -/** - * @file - * @brief Represents one unmanned aerial vehicle - * - * @author Lorenz Meier - * - */ - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include "UAS.h" -#include "LinkInterface.h" -#include "HomePositionManager.h" -#include "QGC.h" -#include "GAudioOutput.h" -#include "MAVLinkProtocol.h" -#include "QGCMAVLink.h" -#include "LinkManager.h" -#ifndef __ios__ -#include "SerialLink.h" -#endif -#include "FirmwarePluginManager.h" -#include "QGCLoggingCategory.h" -#include "Vehicle.h" -#include "Joystick.h" -#include "QGCApplication.h" - -QGC_LOGGING_CATEGORY(UASLog, "UASLog") - -/** -* Gets the settings from the previous UAS (name, airframe, autopilot, battery specs) -* by calling readSettings. This means the new UAS will have the same settings -* as the previous one created unless one calls deleteSettings in the code after -* creating the UAS. -*/ - -UAS::UAS(MAVLinkProtocol *protocol, Vehicle *vehicle, FirmwarePluginManager *firmwarePluginManager) : UASInterface(), - lipoFull(4.2f), - lipoEmpty(3.5f), - uasId(vehicle->id()), - unknownPackets(), - mavlink(protocol), - receiveDropRate(0), - sendDropRate(0), - - status(-1), - - startTime(QGC::groundTimeMilliseconds()), - onboardTimeOffset(0), - - controlRollManual(true), - controlPitchManual(true), - controlYawManual(true), - controlThrustManual(true), - manualRollAngle(0), - manualPitchAngle(0), - manualYawAngle(0), - manualThrust(0), - - isGlobalPositionKnown(false), - - latitude(0.0), - longitude(0.0), - altitudeAMSL(0.0), - altitudeAMSLFT(0.0), - altitudeRelative(0.0), - - satRawHDOP(1e10f), - satRawVDOP(1e10f), - satRawCOG(0.0), - - globalEstimatorActive(false), - - latitude_gps(0.0), - longitude_gps(0.0), - altitude_gps(0.0), - - speedX(0.0), - speedY(0.0), - speedZ(0.0), - - airSpeed(std::numeric_limits::quiet_NaN()), - groundSpeed(std::numeric_limits::quiet_NaN()), -#ifndef __mobile__ - fileManager(this, vehicle), -#endif - - attitudeKnown(false), - attitudeStamped(false), - lastAttitude(0), - - roll(0.0), - pitch(0.0), - yaw(0.0), - - imagePackets(0), // We must initialize to 0, otherwise extended data packets maybe incorrectly thought to be images - - blockHomePositionChanges(false), - receivedMode(false), - - // Note variances calculated from flight case from this log: http://dash.oznet.ch/view/MRjW8NUNYQSuSZkbn8dEjY - // TODO: calibrate stand-still pixhawk variances - xacc_var(0.6457f), - yacc_var(0.7048f), - zacc_var(0.97885f), - rollspeed_var(0.8126f), - pitchspeed_var(0.6145f), - yawspeed_var(0.5852f), - xmag_var(0.2393f), - ymag_var(0.2283f), - zmag_var(0.1665f), - abs_pressure_var(0.5802f), - diff_pressure_var(0.5802f), - pressure_alt_var(0.5802f), - temperature_var(0.7145f), - /* - xacc_var(0.0f), - yacc_var(0.0f), - zacc_var(0.0f), - rollspeed_var(0.0f), - pitchspeed_var(0.0f), - yawspeed_var(0.0f), - xmag_var(0.0f), - ymag_var(0.0f), - zmag_var(0.0f), - abs_pressure_var(0.0f), - diff_pressure_var(0.0f), - pressure_alt_var(0.0f), - temperature_var(0.0f), - */ - -#ifndef __mobile__ - simulation(0), -#endif - - // The protected members. - connectionLost(false), - lastVoltageWarning(0), - lastNonNullTime(0), - onboardTimeOffsetInvalidCount(0), - hilEnabled(false), - sensorHil(false), - lastSendTimeGPS(0), - lastSendTimeSensors(0), - lastSendTimeOpticalFlow(0), - _vehicle(vehicle), - _firmwarePluginManager(firmwarePluginManager) -{ - - for (unsigned int i = 0; i < 255; ++i) - { - componentID[i] = -1; - componentMulti[i] = false; - } - -#ifndef __mobile__ - connect(_vehicle, &Vehicle::mavlinkMessageReceived, &fileManager, &FileManager::receiveMessage); -#endif - - color = UASInterface::getNextColor(); -} - -/** -* @ return the id of the uas -*/ -int UAS::getUASID() const -{ - return uasId; -} - -void UAS::receiveMessage(mavlink_message_t message) -{ - if (!components.contains(message.compid)) - { - QString componentName; - - switch (message.compid) - { - case MAV_COMP_ID_ALL: - { - componentName = "ANONYMOUS"; - break; - } - - case MAV_COMP_ID_IMU: - { - componentName = "IMU #1"; - break; - } - - case MAV_COMP_ID_CAMERA: - { - componentName = "CAMERA"; - break; - } - - case MAV_COMP_ID_MISSIONPLANNER: - { - componentName = "MISSIONPLANNER"; - break; - } - } - - components.insert(message.compid, componentName); - } - - // qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq; - - // Only accept messages from this system (condition 1) - // and only then if a) attitudeStamped is disabled OR b) attitudeStamped is enabled - // and we already got one attitude packet - if (message.sysid == uasId && (!attitudeStamped || (attitudeStamped && (lastAttitude != 0)) || message.msgid == MAVLINK_MSG_ID_ATTITUDE)) - { - QString uasState; - QString stateDescription; - - bool multiComponentSourceDetected = false; - bool wrongComponent = false; - - switch (message.compid) - { - case MAV_COMP_ID_IMU_2: - // Prefer IMU 2 over IMU 1 (FIXME) - componentID[message.msgid] = MAV_COMP_ID_IMU_2; - break; - - default: - // Do nothing - break; - } - - // Store component ID - if (componentID[message.msgid] == -1) - { - // Prefer the first component - componentID[message.msgid] = message.compid; - } - - else - { - // Got this message already - if (componentID[message.msgid] != message.compid) - { - componentMulti[message.msgid] = true; - wrongComponent = true; - } - } - - if (componentMulti[message.msgid] == true) multiComponentSourceDetected = true; - - - switch (message.msgid) - { - case MAVLINK_MSG_ID_HEARTBEAT: - { - if (multiComponentSourceDetected && wrongComponent) - { - break; - } - - mavlink_heartbeat_t state; - mavlink_msg_heartbeat_decode(&message, &state); - - // Send the base_mode and system_status values to the plotter. This uses the ground time - // so the Ground Time checkbox must be ticked for these values to display - quint64 time = getUnixTime(); - QString name = QString("M%1:HEARTBEAT.%2").arg(message.sysid); - emit valueChanged(uasId, name.arg("base_mode"), "bits", state.base_mode, time); - emit valueChanged(uasId, name.arg("custom_mode"), "bits", state.custom_mode, time); - emit valueChanged(uasId, name.arg("system_status"), "-", state.system_status, time); - - if ((state.system_status != this->status) && state.system_status != MAV_STATE_UNINIT) - { - this->status = state.system_status; - getStatusForCode((int)state.system_status, uasState, stateDescription); - emit statusChanged(this, uasState, stateDescription); - emit statusChanged(this->status); - } - - // We got the mode - receivedMode = true; - } - - break; - - case MAVLINK_MSG_ID_SYS_STATUS: - { - if (multiComponentSourceDetected && wrongComponent) - { - break; - } - - mavlink_sys_status_t state; - mavlink_msg_sys_status_decode(&message, &state); - - // Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present. - quint64 time = getUnixTime(); - QString name = QString("M%1:SYS_STATUS.%2").arg(message.sysid); - emit valueChanged(uasId, name.arg("sensors_enabled"), "bits", state.onboard_control_sensors_enabled, time); - emit valueChanged(uasId, name.arg("sensors_health"), "bits", state.onboard_control_sensors_health, time); - emit valueChanged(uasId, name.arg("errors_comm"), "-", state.errors_comm, time); - emit valueChanged(uasId, name.arg("errors_count1"), "-", state.errors_count1, time); - emit valueChanged(uasId, name.arg("errors_count2"), "-", state.errors_count2, time); - emit valueChanged(uasId, name.arg("errors_count3"), "-", state.errors_count3, time); - emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time); - - // Process CPU load. - emit loadChanged(this, state.load / 10.0f); - emit valueChanged(uasId, name.arg("load"), "%", state.load / 10.0f, time); - - // control_sensors_enabled: - // relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control - emit attitudeControlEnabled(state.onboard_control_sensors_enabled & (1 << 11)); - emit positionYawControlEnabled(state.onboard_control_sensors_enabled & (1 << 12)); - emit positionZControlEnabled(state.onboard_control_sensors_enabled & (1 << 13)); - emit positionXYControlEnabled(state.onboard_control_sensors_enabled & (1 << 14)); - - // Trigger drop rate updates as needed. Here we convert the incoming - // drop_rate_comm value from 1/100 of a percent in a uint16 to a true - // percentage as a float. We also cap the incoming value at 100% as defined - // by the MAVLink specifications. - if (state.drop_rate_comm > 10000) - { - state.drop_rate_comm = 10000; - } - - emit dropRateChanged(this->getUASID(), state.drop_rate_comm / 100.0f); - emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm / 100.0f, time); - } - break; - - case MAVLINK_MSG_ID_ATTITUDE: - { - mavlink_attitude_t attitude; - mavlink_msg_attitude_decode(&message, &attitude); - quint64 time = getUnixReferenceTime(attitude.time_boot_ms); - - emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(attitude.roll), QGC::limitAngleToPMPIf(attitude.pitch), QGC::limitAngleToPMPIf(attitude.yaw), time); - - if (!wrongComponent) - { - lastAttitude = time; - setRoll(QGC::limitAngleToPMPIf(attitude.roll)); - setPitch(QGC::limitAngleToPMPIf(attitude.pitch)); - setYaw(QGC::limitAngleToPMPIf(attitude.yaw)); - - attitudeKnown = true; - emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); - emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); - } - } - break; - - case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: - { - mavlink_attitude_quaternion_t attitude; - mavlink_msg_attitude_quaternion_decode(&message, &attitude); - quint64 time = getUnixReferenceTime(attitude.time_boot_ms); - - double a = attitude.q1; - double b = attitude.q2; - double c = attitude.q3; - double d = attitude.q4; - - double aSq = a * a; - double bSq = b * b; - double cSq = c * c; - double dSq = d * d; - float dcm[3][3]; - dcm[0][0] = aSq + bSq - cSq - dSq; - dcm[0][1] = 2.0 * (b * c - a * d); - dcm[0][2] = 2.0 * (a * c + b * d); - dcm[1][0] = 2.0 * (b * c + a * d); - dcm[1][1] = aSq - bSq + cSq - dSq; - dcm[1][2] = 2.0 * (c * d - a * b); - dcm[2][0] = 2.0 * (b * d - a * c); - dcm[2][1] = 2.0 * (a * b + c * d); - dcm[2][2] = aSq - bSq - cSq + dSq; - - float phi, theta, psi; - theta = asin(-dcm[2][0]); - - if (fabs(theta - M_PI_2) < 1.0e-3f) - { - phi = 0.0f; - psi = (atan2(dcm[1][2] - dcm[0][1], - dcm[0][2] + dcm[1][1]) + phi); - - } - - else if (fabs(theta + M_PI_2) < 1.0e-3f) - { - phi = 0.0f; - psi = atan2f(dcm[1][2] - dcm[0][1], - dcm[0][2] + dcm[1][1] - phi); - - } - - else - { - phi = atan2f(dcm[2][1], dcm[2][2]); - psi = atan2f(dcm[1][0], dcm[0][0]); - } - - emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(phi), - QGC::limitAngleToPMPIf(theta), - QGC::limitAngleToPMPIf(psi), time); - - if (!wrongComponent) - { - lastAttitude = time; - setRoll(QGC::limitAngleToPMPIf(phi)); - setPitch(QGC::limitAngleToPMPIf(theta)); - setYaw(QGC::limitAngleToPMPIf(psi)); - - attitudeKnown = true; - emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); - emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); - } - } - break; - - case MAVLINK_MSG_ID_HIL_CONTROLS: - { - mavlink_hil_controls_t hil; - mavlink_msg_hil_controls_decode(&message, &hil); - emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode); - } - break; - - case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS: - { - mavlink_hil_actuator_controls_t hil; - mavlink_msg_hil_actuator_controls_decode(&message, &hil); - emit hilActuatorControlsChanged(hil.time_usec, hil.flags, - hil.controls[0], - hil.controls[1], - hil.controls[2], - hil.controls[3], - hil.controls[4], - hil.controls[5], - hil.controls[6], - hil.controls[7], - hil.controls[8], - hil.controls[9], - hil.controls[10], - hil.controls[11], - hil.controls[12], - hil.controls[13], - hil.controls[14], - hil.controls[15], - hil.mode); - } - break; - - case MAVLINK_MSG_ID_VFR_HUD: - { - mavlink_vfr_hud_t hud; - mavlink_msg_vfr_hud_decode(&message, &hud); - quint64 time = getUnixTime(); - // Display updated values - emit thrustChanged(this, hud.throttle / 100.0); - - if (!attitudeKnown) - { - setYaw(QGC::limitAngleToPMPId((((double)hud.heading) / 180.0)*M_PI)); - emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); - } - - setAltitudeAMSL(hud.alt); - setGroundSpeed(hud.groundspeed); - - if (!qIsNaN(hud.airspeed)) - setAirSpeed(hud.airspeed); - - speedZ = -hud.climb; - emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); - emit speedChanged(this, groundSpeed, airSpeed, time); - } - break; - - case MAVLINK_MSG_ID_LOCAL_POSITION_NED: - //std::cerr << std::endl; - //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; - { - mavlink_local_position_ned_t pos; - mavlink_msg_local_position_ned_decode(&message, &pos); - quint64 time = getUnixTime(pos.time_boot_ms); - - if (!wrongComponent) - { - speedX = pos.vx; - speedY = pos.vy; - speedZ = pos.vz; - - // Emit - emit velocityChanged_NED(this, speedX, speedY, speedZ, time); - } - } - break; - - case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE: - { - mavlink_global_vision_position_estimate_t pos; - mavlink_msg_global_vision_position_estimate_decode(&message, &pos); - quint64 time = getUnixTime(pos.usec); - emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time); - } - break; - - case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: - //std::cerr << std::endl; - //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; - { - mavlink_global_position_int_t pos; - mavlink_msg_global_position_int_decode(&message, &pos); - - quint64 time = getUnixTime(); - - setLatitude(pos.lat / (double)1E7); - setLongitude(pos.lon / (double)1E7); - setAltitudeRelative(pos.relative_alt / 1000.0); - - globalEstimatorActive = true; - - speedX = pos.vx / 100.0; - speedY = pos.vy / 100.0; - speedZ = pos.vz / 100.0; - - emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time); - emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); - // We had some frame mess here, global and local axes were mixed. - emit velocityChanged_NED(this, speedX, speedY, speedZ, time); - - setGroundSpeed(qSqrt(speedX * speedX + speedY * speedY)); - emit speedChanged(this, groundSpeed, airSpeed, time); - - isGlobalPositionKnown = true; - } - break; - - case MAVLINK_MSG_ID_GPS_RAW_INT: - { - mavlink_gps_raw_int_t pos; - mavlink_msg_gps_raw_int_decode(&message, &pos); - - quint64 time = getUnixTime(pos.time_usec); - - // TODO: track localization state not only for gps but also for other loc. sources - int loc_type = pos.fix_type; - - if (loc_type == 1) - { - loc_type = 0; - } - - setSatelliteCount(pos.satellites_visible); - - if (pos.fix_type > 2) - { - isGlobalPositionKnown = true; - - latitude_gps = pos.lat / (double)1E7; - longitude_gps = pos.lon / (double)1E7; - altitude_gps = pos.alt / 1000.0; - - // If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead. - if (!globalEstimatorActive) - { - setLatitude(latitude_gps); - setLongitude(longitude_gps); - emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time); - emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); - - float vel = pos.vel / 100.0f; - - // Smaller than threshold and not NaN - if ((vel < 1000000) && !qIsNaN(vel) && !qIsInf(vel)) - { - setGroundSpeed(vel); - emit speedChanged(this, groundSpeed, airSpeed, time); - } - - else - { - emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_NOTICE, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel)); - } - } - } - - double dtmp; - //-- Raw GPS data - dtmp = pos.eph == 0xFFFF ? 1e10f : pos.eph / 100.0; - - if (dtmp != satRawHDOP) - { - satRawHDOP = dtmp; - emit satRawHDOPChanged(satRawHDOP); - } - - dtmp = pos.epv == 0xFFFF ? 1e10f : pos.epv / 100.0; - - if (dtmp != satRawVDOP) - { - satRawVDOP = dtmp; - emit satRawVDOPChanged(satRawVDOP); - } - - dtmp = pos.cog == 0xFFFF ? 0.0 : pos.cog / 100.0; - - if (dtmp != satRawCOG) - { - satRawCOG = dtmp; - emit satRawCOGChanged(satRawCOG); - } - - // Emit this signal after the above signals. This way a trigger on gps lock signal which then asks for vehicle position - // gets a good position. - emit localizationChanged(this, loc_type); - } - break; - - case MAVLINK_MSG_ID_GPS_STATUS: - { - mavlink_gps_status_t pos; - mavlink_msg_gps_status_decode(&message, &pos); - - for (int i = 0; i < (int)pos.satellites_visible; i++) - { - emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast(pos.satellite_used[i])); - } - - setSatelliteCount(pos.satellites_visible); - } - break; - - case MAVLINK_MSG_ID_PARAM_VALUE: - { - mavlink_param_value_t rawValue; - mavlink_msg_param_value_decode(&message, &rawValue); - QByteArray bytes(rawValue.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - // Construct a string stopping at the first NUL (0) character, else copy the whole - // byte array (max MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN, so safe) - QString parameterName(bytes); - mavlink_param_union_t paramVal; - paramVal.param_float = rawValue.param_value; - paramVal.type = rawValue.param_type; - - processParamValueMsg(message, parameterName, rawValue, paramVal); - } - break; - - case MAVLINK_MSG_ID_ATTITUDE_TARGET: - { - mavlink_attitude_target_t out; - mavlink_msg_attitude_target_decode(&message, &out); - float roll, pitch, yaw; - mavlink_quaternion_to_euler(out.q, &roll, &pitch, &yaw); - quint64 time = getUnixTimeFromMs(out.time_boot_ms); - emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, out.thrust, time); - - // For plotting emit roll sp, pitch sp and yaw sp values - emit valueChanged(uasId, "roll sp", "rad", roll, time); - emit valueChanged(uasId, "pitch sp", "rad", pitch, time); - emit valueChanged(uasId, "yaw sp", "rad", yaw, time); - } - break; - - case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED: - { - if (multiComponentSourceDetected && wrongComponent) - { - break; - } - - mavlink_position_target_local_ned_t p; - mavlink_msg_position_target_local_ned_decode(&message, &p); - quint64 time = getUnixTimeFromMs(p.time_boot_ms); - emit positionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */, time); - } - break; - - case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: - { - mavlink_set_position_target_local_ned_t p; - mavlink_msg_set_position_target_local_ned_decode(&message, &p); - emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */); - } - break; - - case MAVLINK_MSG_ID_STATUSTEXT: - { - QByteArray b; - b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN + 1); - mavlink_msg_statustext_get_text(&message, b.data()); - - // Ensure NUL-termination - b[b.length() - 1] = '\0'; - QString text = QString(b); - int severity = mavlink_msg_statustext_get_severity(&message); - - // If the message is NOTIFY or higher severity, or starts with a '#', - // then read it aloud. - if (text.startsWith("#") || severity <= MAV_SEVERITY_NOTICE) - { - text.remove("#"); - emit textMessageReceived(uasId, message.compid, severity, text); - _say(text.toLower(), severity); - } - - else - { - emit textMessageReceived(uasId, message.compid, severity, text); - } - } - break; - - case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE: - { - mavlink_data_transmission_handshake_t p; - mavlink_msg_data_transmission_handshake_decode(&message, &p); - imageSize = p.size; - imagePackets = p.packets; - imagePayload = p.payload; - imageQuality = p.jpg_quality; - imageType = p.type; - imageWidth = p.width; - imageHeight = p.height; - imageStart = QGC::groundTimeMilliseconds(); - imagePacketsArrived = 0; - - } - break; - - case MAVLINK_MSG_ID_ENCAPSULATED_DATA: - { - mavlink_encapsulated_data_t img; - mavlink_msg_encapsulated_data_decode(&message, &img); - int seq = img.seqnr; - int pos = seq * imagePayload; - - // Check if we have a valid transaction - if (imagePackets == 0) - { - // NO VALID TRANSACTION - ABORT - // Restart statemachine - imagePacketsArrived = 0; - break; - } - - for (int i = 0; i < imagePayload; ++i) - { - if (pos <= imageSize) - { - imageRecBuffer[pos] = img.data[i]; - } - - ++pos; - } - - ++imagePacketsArrived; - - // emit signal if all packets arrived - if (imagePacketsArrived >= imagePackets) - { - // Restart statemachine - imagePackets = 0; - imagePacketsArrived = 0; - emit imageReady(this); - } - } - break; - - case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: - { - mavlink_nav_controller_output_t p; - mavlink_msg_nav_controller_output_decode(&message, &p); - setDistToWaypoint(p.wp_dist); - setBearingToWaypoint(p.nav_bearing); - emit navigationControllerErrorsChanged(this, p.alt_error, p.aspd_error, p.xtrack_error); - emit NavigationControllerDataChanged(this, p.nav_roll, p.nav_pitch, p.nav_bearing, p.target_bearing, p.wp_dist); - } - break; - - case MAVLINK_MSG_ID_LOG_ENTRY: - { - mavlink_log_entry_t log; - mavlink_msg_log_entry_decode(&message, &log); - emit logEntry(this, log.time_utc, log.size, log.id, log.num_logs, log.last_log_num); - } - break; - - case MAVLINK_MSG_ID_LOG_DATA: - { - mavlink_log_data_t log; - mavlink_msg_log_data_decode(&message, &log); - emit logData(this, log.ofs, log.id, log.count, log.data); - } - break; - - default: - break; - } - } -} - -void UAS::startCalibration(UASInterface::StartCalibrationType calType) -{ - if (!_vehicle) - { - return; - } - - int gyroCal = 0; - int magCal = 0; - int airspeedCal = 0; - int radioCal = 0; - int accelCal = 0; - int escCal = 0; - - switch (calType) - { - case StartCalibrationGyro: - gyroCal = 1; - break; - - case StartCalibrationMag: - magCal = 1; - break; - - case StartCalibrationAirspeed: - airspeedCal = 1; - break; - - case StartCalibrationRadio: - radioCal = 1; - break; - - case StartCalibrationCopyTrims: - radioCal = 2; - break; - - case StartCalibrationAccel: - accelCal = 1; - break; - - case StartCalibrationLevel: - accelCal = 2; - break; - - case StartCalibrationEsc: - escCal = 1; - break; - - case StartCalibrationUavcanEsc: - escCal = 2; - break; - - case StartCalibrationCompassMot: - airspeedCal = 1; // ArduPilot, bit of a hack - break; - } - - mavlink_message_t msg; - mavlink_msg_command_long_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - uasId, - _vehicle->defaultComponentId(), // target component - MAV_CMD_PREFLIGHT_CALIBRATION, // command id - 0, // 0=first transmission of command - gyroCal, // gyro cal - magCal, // mag cal - 0, // ground pressure - radioCal, // radio cal - accelCal, // accel cal - airspeedCal, // PX4: airspeed cal, ArduPilot: compass mot - escCal); // esc cal - _vehicle->sendMessageOnPriorityLink(msg); -} - -void UAS::stopCalibration(void) -{ - if (!_vehicle) - { - return; - } - - mavlink_message_t msg; - mavlink_msg_command_long_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - uasId, - _vehicle->defaultComponentId(), // target component - MAV_CMD_PREFLIGHT_CALIBRATION, // command id - 0, // 0=first transmission of command - 0, // gyro cal - 0, // mag cal - 0, // ground pressure - 0, // radio cal - 0, // accel cal - 0, // airspeed cal - 0); // unused - _vehicle->sendMessageOnPriorityLink(msg); -} - -void UAS::startBusConfig(UASInterface::StartBusConfigType calType) -{ - if (!_vehicle) - { - return; - } - - int actuatorCal = 0; - - switch (calType) - { - case StartBusConfigActuators: - actuatorCal = 1; - break; - - case EndBusConfigActuators: - actuatorCal = 0; - break; - } - - mavlink_message_t msg; - mavlink_msg_command_long_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - uasId, - _vehicle->defaultComponentId(), // target component - MAV_CMD_PREFLIGHT_UAVCAN, // command id - 0, // 0=first transmission of command - actuatorCal, // actuators - 0, - 0, - 0, - 0, - 0, - 0); - _vehicle->sendMessageOnPriorityLink(msg); -} - -void UAS::stopBusConfig(void) -{ - if (!_vehicle) - { - return; - } - - mavlink_message_t msg; - mavlink_msg_command_long_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &msg, - uasId, - _vehicle->defaultComponentId(), // target component - MAV_CMD_PREFLIGHT_UAVCAN, // command id - 0, // 0=first transmission of command - 0, - 0, - 0, - 0, - 0, - 0, - 0); - _vehicle->sendMessageOnPriorityLink(msg); -} - -/** -* Check if time is smaller than 40 years, assuming no system without Unix -* timestamp runs longer than 40 years continuously without reboot. In worst case -* this will add/subtract the communication delay between GCS and MAV, it will -* never alter the timestamp in a safety critical way. -*/ -quint64 UAS::getUnixReferenceTime(quint64 time) -{ - // Same as getUnixTime, but does not react to attitudeStamped mode - if (time == 0) - { - // qDebug() << "XNEW time:" < has to be - // a Unix epoch timestamp. Do nothing. - return time / 1000; - } -} - -/** -* @warning If attitudeStamped is enabled, this function will not actually return -* the precise time stamp of this measurement augmented to UNIX time, but will -* MOVE the timestamp IN TIME to match the last measured attitude. There is no -* reason why one would want this, except for system setups where the onboard -* clock is not present or broken and datasets should be collected that are still -* roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED RUINS THE -* SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL! -*/ -quint64 UAS::getUnixTimeFromMs(quint64 time) -{ - return getUnixTime(time * 1000); -} - -/** -* @warning If attitudeStamped is enabled, this function will not actually return -* the precise time stam of this measurement augmented to UNIX time, but will -* MOVE the timestamp IN TIME to match the last measured attitude. There is no -* reason why one would want this, except for system setups where the onboard -* clock is not present or broken and datasets should be collected that are -* still roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED -* RUINS THE SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL! -*/ -quint64 UAS::getUnixTime(quint64 time) -{ - quint64 ret = 0; - - if (attitudeStamped) - { - ret = lastAttitude; - } - - if (time == 0) - { - ret = QGC::groundTimeMilliseconds(); - } - - // Check if time is smaller than 40 years, - // assuming no system without Unix timestamp - // runs longer than 40 years continuously without - // reboot. In worst case this will add/subtract the - // communication delay between GCS and MAV, - // it will never alter the timestamp in a safety - // critical way. - // - // Calculation: - // 40 years - // 365 days - // 24 hours - // 60 minutes - // 60 seconds - // 1000 milliseconds - // 1000 microseconds -#ifndef _MSC_VER - - else if (time < 1261440000000000LLU) -#else - else if (time < 1261440000000000) -#endif - { - // qDebug() << "GEN time:" << time/1000 + onboardTimeOffset; - if (onboardTimeOffset == 0 || time < (lastNonNullTime - 100)) - { - lastNonNullTime = time; - onboardTimeOffset = QGC::groundTimeMilliseconds() - time / 1000; - } - - if (time > lastNonNullTime) lastNonNullTime = time; - - ret = time / 1000 + onboardTimeOffset; - } - - else - { - // Time is not zero and larger than 40 years -> has to be - // a Unix epoch timestamp. Do nothing. - ret = time / 1000; - } - - return ret; -} - -/** -* Get the status of the code and a description of the status. -* Status can be unitialized, booting up, calibrating sensors, active -* standby, cirtical, emergency, shutdown or unknown. -*/ -void UAS::getStatusForCode(int statusCode, QString &uasState, QString &stateDescription) -{ - switch (statusCode) - { - case MAV_STATE_UNINIT: - uasState = tr("UNINIT"); - stateDescription = tr("Unitialized, booting up."); - break; - - case MAV_STATE_BOOT: - uasState = tr("BOOT"); - stateDescription = tr("Booting system, please wait."); - break; - - case MAV_STATE_CALIBRATING: - uasState = tr("CALIBRATING"); - stateDescription = tr("Calibrating sensors, please wait."); - break; - - case MAV_STATE_ACTIVE: - uasState = tr("ACTIVE"); - stateDescription = tr("Active, normal operation."); - break; - - case MAV_STATE_STANDBY: - uasState = tr("STANDBY"); - stateDescription = tr("Standby mode, ready for launch."); - break; - - case MAV_STATE_CRITICAL: - uasState = tr("CRITICAL"); - stateDescription = tr("FAILURE: Continuing operation."); - break; - - case MAV_STATE_EMERGENCY: - uasState = tr("EMERGENCY"); - stateDescription = tr("EMERGENCY: Land Immediately!"); - break; - - //case MAV_STATE_HILSIM: - //uasState = tr("HIL SIM"); - //stateDescription = tr("HIL Simulation, Sensors read from SIM"); - //break; - - case MAV_STATE_POWEROFF: - uasState = tr("SHUTDOWN"); - stateDescription = tr("Powering off system."); - break; - - default: - uasState = tr("UNKNOWN"); - stateDescription = tr("Unknown system state"); - break; - } -} - -QImage UAS::getImage() -{ - -// qDebug() << "IMAGE TYPE:" << imageType; - - // RAW greyscale - if (imageType == MAVLINK_DATA_STREAM_IMG_RAW8U) - { - int imgColors = 255; - - // Construct PGM header - QString header("P5\n%1 %2\n%3\n"); - header = header.arg(imageWidth).arg(imageHeight).arg(imgColors); - - QByteArray tmpImage(header.toStdString().c_str(), header.length()); - tmpImage.append(imageRecBuffer); - - //qDebug() << "IMAGE SIZE:" << tmpImage.size() << "HEADER SIZE: (15):" << header.size() << "HEADER: " << header; - - if (imageRecBuffer.isNull()) - { - qDebug() << "could not convertToPGM()"; - return QImage(); - } - - if (!image.loadFromData(tmpImage, "PGM")) - { - qDebug() << __FILE__ << __LINE__ << "could not create extracted image"; - return QImage(); - } - - } - - // BMP with header - else if (imageType == MAVLINK_DATA_STREAM_IMG_BMP || - imageType == MAVLINK_DATA_STREAM_IMG_JPEG || - imageType == MAVLINK_DATA_STREAM_IMG_PGM || - imageType == MAVLINK_DATA_STREAM_IMG_PNG) - { - if (!image.loadFromData(imageRecBuffer)) - { - qDebug() << __FILE__ << __LINE__ << "Loading data from image buffer failed!"; - return QImage(); - } - } - - // Restart statemachine - imagePacketsArrived = 0; - imagePackets = 0; - imageRecBuffer.clear(); - return image; -} - -void UAS::requestImage() -{ - if (!_vehicle) - { - return; - } - - qDebug() << "trying to get an image from the uas..."; - - // check if there is already an image transmission going on - if (imagePacketsArrived == 0) - { - mavlink_message_t msg; - mavlink_msg_data_transmission_handshake_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, MAVLINK_DATA_STREAM_IMG_JPEG, 0, 0, 0, 0, 0, 50); - _vehicle->sendMessageOnPriorityLink(msg); - } -} - - -/* MANAGEMENT */ - -/** - * - * @return The uptime in milliseconds - * - */ -quint64 UAS::getUptime() const -{ - if (startTime == 0) - { - return 0; - } - - else - { - return QGC::groundTimeMilliseconds() - startTime; - } -} - -//TODO update this to use the parameter manager / param data model instead -void UAS::processParamValueMsg(mavlink_message_t &msg, const QString ¶mName, const mavlink_param_value_t &rawValue, mavlink_param_union_t ¶mUnion) -{ - int compId = msg.compid; - - QVariant paramValue; - - // Insert with correct type - - switch (rawValue.param_type) - { - case MAV_PARAM_TYPE_REAL32: - paramValue = QVariant(paramUnion.param_float); - break; - - case MAV_PARAM_TYPE_UINT8: - paramValue = QVariant(paramUnion.param_uint8); - break; - - case MAV_PARAM_TYPE_INT8: - paramValue = QVariant(paramUnion.param_int8); - break; - - case MAV_PARAM_TYPE_UINT16: - paramValue = QVariant(paramUnion.param_uint16); - break; - - case MAV_PARAM_TYPE_INT16: - paramValue = QVariant(paramUnion.param_int16); - break; - - case MAV_PARAM_TYPE_UINT32: - paramValue = QVariant(paramUnion.param_uint32); - break; - - case MAV_PARAM_TYPE_INT32: - paramValue = QVariant(paramUnion.param_int32); - break; - - //-- Note: These are not handled above: - // - // MAV_PARAM_TYPE_UINT64 - // MAV_PARAM_TYPE_INT64 - // MAV_PARAM_TYPE_REAL64 - // - // No space in message (the only storage allocation is a "float") and not present in mavlink_param_union_t - - default: - qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type; - } - - qCDebug(UASLog) << "Received PARAM_VALUE" << paramName << paramValue << rawValue.param_type; - - emit parameterUpdate(uasId, compId, paramName, rawValue.param_count, rawValue.param_index, rawValue.param_type, paramValue); -} - -void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) -{ - if (!_vehicle) - { - return; - } - - mavlink_message_t msg; - mavlink_command_long_t cmd; - cmd.command = (uint16_t)command; - cmd.confirmation = confirmation; - cmd.param1 = param1; - cmd.param2 = param2; - cmd.param3 = param3; - cmd.param4 = param4; - cmd.param5 = param5; - cmd.param6 = param6; - cmd.param7 = param7; - cmd.target_system = uasId; - cmd.target_component = component; - mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); - _vehicle->sendMessageOnPriorityLink(msg); -} - -/** -* Set the manual control commands. -* This can only be done if the system has manual inputs enabled and is armed. -*/ -void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode) -{ - if (!_vehicle) - { - return; - } - - // Store the previous manual commands - static float manualRollAngle = 0.0; - static float manualPitchAngle = 0.0; - static float manualYawAngle = 0.0; - static float manualThrust = 0.0; - static quint16 manualButtons = 0; - static quint8 countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission - - // Transmit the external setpoints only if they've changed OR if it's been a little bit since they were last transmit. To make sure there aren't issues with - // response rate, we make sure that a message is transmit when the commands have changed, then one more time, and then switch to the lower transmission rate - // if no command inputs have changed. - - // The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz. - bool sendCommand = false; - - if (countSinceLastTransmission++ >= 5) - { - sendCommand = true; - countSinceLastTransmission = 0; - } - - else if ((!qIsNaN(roll) && roll != manualRollAngle) || (!qIsNaN(pitch) && pitch != manualPitchAngle) || - (!qIsNaN(yaw) && yaw != manualYawAngle) || (!qIsNaN(thrust) && thrust != manualThrust) || - buttons != manualButtons) - { - sendCommand = true; - - // Ensure that another message will be sent the next time this function is called - countSinceLastTransmission = 10; - } - - // Now if we should trigger an update, let's do that - if (sendCommand) - { - // Save the new manual control inputs - manualRollAngle = roll; - manualPitchAngle = pitch; - manualYawAngle = yaw; - manualThrust = thrust; - manualButtons = buttons; - - mavlink_message_t message; - - if (joystickMode == Vehicle::JoystickModeAttitude) - { - // send an external attitude setpoint command (rate control disabled) - float attitudeQuaternion[4]; - mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion); - uint8_t typeMask = 0x7; // disable rate control - mavlink_msg_set_attitude_target_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &message, - QGC::groundTimeUsecs(), - this->uasId, - 0, - typeMask, - attitudeQuaternion, - 0, - 0, - 0, - thrust - ); - } - - else if (joystickMode == Vehicle::JoystickModePosition) - { - // Send the the local position setpoint (local pos sp external message) - static float px = 0; - static float py = 0; - static float pz = 0; - //XXX: find decent scaling - px -= pitch; - py += roll; - pz -= 2.0f * (thrust - 0.5); - uint16_t typeMask = (1 << 11) | (7 << 6) | (7 << 3); // select only POSITION control - mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &message, - QGC::groundTimeUsecs(), - this->uasId, - 0, - MAV_FRAME_LOCAL_NED, - typeMask, - px, - py, - pz, - 0, - 0, - 0, - 0, - 0, - 0, - yaw, - 0 - ); - } - - else if (joystickMode == Vehicle::JoystickModeForce) - { - // Send the the force setpoint (local pos sp external message) - float dcm[3][3]; - mavlink_euler_to_dcm(roll, pitch, yaw, dcm); - const float fx = -dcm[0][2] * thrust; - const float fy = -dcm[1][2] * thrust; - const float fz = -dcm[2][2] * thrust; - uint16_t typeMask = (3 << 10) | (7 << 3) | (7 << 0) | (1 << 9); // select only FORCE control (disable everything else) - mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &message, - QGC::groundTimeUsecs(), - this->uasId, - 0, - MAV_FRAME_LOCAL_NED, - typeMask, - 0, - 0, - 0, - 0, - 0, - 0, - fx, - fy, - fz, - 0, - 0 - ); - } - - else if (joystickMode == Vehicle::JoystickModeVelocity) - { - // Send the the local velocity setpoint (local pos sp external message) - static float vx = 0; - static float vy = 0; - static float vz = 0; - static float yawrate = 0; - //XXX: find decent scaling - vx -= pitch; - vy += roll; - vz -= 2.0f * (thrust - 0.5); - yawrate += yaw; //XXX: not sure what scale to apply here - uint16_t typeMask = (1 << 10) | (7 << 6) | (7 << 0); // select only VELOCITY control - mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &message, - QGC::groundTimeUsecs(), - this->uasId, - 0, - MAV_FRAME_LOCAL_NED, - typeMask, - 0, - 0, - 0, - vx, - vy, - vz, - 0, - 0, - 0, - 0, - yawrate - ); - } - - else if (joystickMode == Vehicle::JoystickModeRC) - { - - // Save the new manual control inputs - manualRollAngle = roll; - manualPitchAngle = pitch; - manualYawAngle = yaw; - manualThrust = thrust; - manualButtons = buttons; - - // Store scaling values for all 3 axes - const float axesScaling = 1.0 * 1000.0; - - // Calculate the new commands for roll, pitch, yaw, and thrust - const float newRollCommand = roll * axesScaling; - // negate pitch value because pitch is negative for pitching forward but mavlink message argument is positive for forward - const float newPitchCommand = -pitch * axesScaling; - const float newYawCommand = yaw * axesScaling; - const float newThrustCommand = thrust * axesScaling; - - //qDebug() << newRollCommand << newPitchCommand << newYawCommand << newThrustCommand; - - // Send the MANUAL_COMMAND message - mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons); - } - - _vehicle->sendMessageOnPriorityLink(message); - // Emit an update in control values to other UI elements, like the HSI display - emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); - } -} - -#ifndef __mobile__ -void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw) -{ - if (!_vehicle) - { - return; - } - - const uint8_t base_mode = _vehicle->baseMode(); - - // If system has manual inputs enabled and is armed - if (((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED)) - { - mavlink_message_t message; - float q[4]; - mavlink_euler_to_quaternion(roll, pitch, yaw, q); - - float yawrate = 0.0f; - - // Do not control rates and throttle - quint8 mask = (1 << 0) | (1 << 1) | (1 << 2); // ignore rates - mask |= (1 << 6); // ignore throttle - mavlink_msg_set_attitude_target_pack(mavlink->getSystemId(), mavlink->getComponentId(), - &message, QGC::groundTimeMilliseconds(), this->uasId, _vehicle->defaultComponentId(), - mask, q, 0, 0, 0, 0); - _vehicle->sendMessageOnPriorityLink(message); - quint16 position_mask = (1 << 3) | (1 << 4) | (1 << 5) | - (1 << 6) | (1 << 7) | (1 << 8); - mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), mavlink->getComponentId(), - &message, QGC::groundTimeMilliseconds(), this->uasId, _vehicle->defaultComponentId(), - MAV_FRAME_LOCAL_NED, position_mask, x, y, z, 0, 0, 0, 0, 0, 0, yaw, yawrate); - _vehicle->sendMessageOnPriorityLink(message); - qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGES: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw; - - //emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); - } - - else - { - qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first"; - } -} -#endif - -/** -* Order the robot to start receiver pairing -*/ -void UAS::pairRX(int rxType, int rxSubType) -{ - if (!_vehicle) - { - return; - } - - mavlink_message_t msg; - - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, _vehicle->defaultComponentId(), MAV_CMD_START_RX_PAIR, 0, rxType, rxSubType, 0, 0, 0, 0, 0); - _vehicle->sendMessageOnPriorityLink(msg); -} - -/** -* If enabled, connect the flight gear link. -*/ -#ifndef __mobile__ -void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject *configuration) -{ - Q_UNUSED(configuration); - - QGCFlightGearLink *link = dynamic_cast(simulation); - - if (!link) - { - // Delete wrong sim - if (simulation) - { - stopHil(); - delete simulation; - } - - simulation = new QGCFlightGearLink(_vehicle, options); - } - - float noise_scaler = 0.0001f; - xacc_var = noise_scaler * 0.2914f; - yacc_var = noise_scaler * 0.2914f; - zacc_var = noise_scaler * 0.9577f; - rollspeed_var = noise_scaler * 0.8126f; - pitchspeed_var = noise_scaler * 0.6145f; - yawspeed_var = noise_scaler * 0.5852f; - xmag_var = noise_scaler * 0.0786f; - ymag_var = noise_scaler * 0.0566f; - zmag_var = noise_scaler * 0.0333f; - abs_pressure_var = noise_scaler * 0.5604f; - diff_pressure_var = noise_scaler * 0.2604f; - pressure_alt_var = noise_scaler * 0.5604f; - temperature_var = noise_scaler * 0.7290f; - - // Connect Flight Gear Link - link = dynamic_cast(simulation); - link->setStartupArguments(options); - link->sensorHilEnabled(sensorHil); - - // FIXME: this signal is not on the base hil configuration widget, only on the FG widget - //QObject::connect(configuration, SIGNAL(barometerOffsetChanged(float)), link, SLOT(setBarometerOffset(float))); - if (enable) - { - startHil(); - } - - else - { - stopHil(); - } -} -#endif - -/** -* If enabled, connect the JSBSim link. -*/ -#ifndef __mobile__ -void UAS::enableHilJSBSim(bool enable, QString options) -{ - QGCJSBSimLink *link = dynamic_cast(simulation); - - if (!link) - { - // Delete wrong sim - if (simulation) - { - stopHil(); - delete simulation; - } - - simulation = new QGCJSBSimLink(_vehicle, options); - } - - // Connect Flight Gear Link - link = dynamic_cast(simulation); - link->setStartupArguments(options); - - if (enable) - { - startHil(); - } - - else - { - stopHil(); - } -} -#endif - -/** -* If enabled, connect the X-plane gear link. -*/ -#ifndef __mobile__ -void UAS::enableHilXPlane(bool enable) -{ - QGCXPlaneLink *link = dynamic_cast(simulation); - - if (!link) - { - if (simulation) - { - stopHil(); - delete simulation; - } - - simulation = new QGCXPlaneLink(_vehicle); - - float noise_scaler = 0.0001f; - xacc_var = noise_scaler * 0.2914f; - yacc_var = noise_scaler * 0.2914f; - zacc_var = noise_scaler * 0.9577f; - rollspeed_var = noise_scaler * 0.8126f; - pitchspeed_var = noise_scaler * 0.6145f; - yawspeed_var = noise_scaler * 0.5852f; - xmag_var = noise_scaler * 0.0786f; - ymag_var = noise_scaler * 0.0566f; - zmag_var = noise_scaler * 0.0333f; - abs_pressure_var = noise_scaler * 0.5604f; - diff_pressure_var = noise_scaler * 0.2604f; - pressure_alt_var = noise_scaler * 0.5604f; - temperature_var = noise_scaler * 0.7290f; - } - - // Connect X-Plane Link - if (enable) - { - startHil(); - } - - else - { - stopHil(); - } -} -#endif - -/** -* @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) -* @param roll Roll angle (rad) -* @param pitch Pitch angle (rad) -* @param yaw Yaw angle (rad) -* @param rollspeed Roll angular speed (rad/s) -* @param pitchspeed Pitch angular speed (rad/s) -* @param yawspeed Yaw angular speed (rad/s) -* @param lat Latitude, expressed as * 1E7 -* @param lon Longitude, expressed as * 1E7 -* @param alt Altitude in meters, expressed as * 1000 (millimeters) -* @param vx Ground X Speed (Latitude), expressed as m/s * 100 -* @param vy Ground Y Speed (Longitude), expressed as m/s * 100 -* @param vz Ground Z Speed (Altitude), expressed as m/s * 100 -* @param xacc X acceleration (mg) -* @param yacc Y acceleration (mg) -* @param zacc Z acceleration (mg) -*/ -#ifndef __mobile__ -void UAS::sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, - float pitchspeed, float yawspeed, double lat, double lon, double alt, - float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) -{ - Q_UNUSED(time_us); - Q_UNUSED(xacc); - Q_UNUSED(yacc); - Q_UNUSED(zacc); - - // Emit attitude for cross-check - emit valueChanged(uasId, "roll sim", "rad", roll, getUnixTime()); - emit valueChanged(uasId, "pitch sim", "rad", pitch, getUnixTime()); - emit valueChanged(uasId, "yaw sim", "rad", yaw, getUnixTime()); - - emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, getUnixTime()); - emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, getUnixTime()); - emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime()); - - emit valueChanged(uasId, "lat sim", "deg", lat * 1e7, getUnixTime()); - emit valueChanged(uasId, "lon sim", "deg", lon * 1e7, getUnixTime()); - emit valueChanged(uasId, "alt sim", "deg", alt * 1e3, getUnixTime()); - - emit valueChanged(uasId, "vx sim", "m/s", vx * 1e2, getUnixTime()); - emit valueChanged(uasId, "vy sim", "m/s", vy * 1e2, getUnixTime()); - emit valueChanged(uasId, "vz sim", "m/s", vz * 1e2, getUnixTime()); - - emit valueChanged(uasId, "IAS sim", "m/s", ind_airspeed, getUnixTime()); - emit valueChanged(uasId, "TAS sim", "m/s", true_airspeed, getUnixTime()); -} -#endif - -/** -* @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) -* @param roll Roll angle (rad) -* @param pitch Pitch angle (rad) -* @param yaw Yaw angle (rad) -* @param rollspeed Roll angular speed (rad/s) -* @param pitchspeed Pitch angular speed (rad/s) -* @param yawspeed Yaw angular speed (rad/s) -* @param lat Latitude, expressed as * 1E7 -* @param lon Longitude, expressed as * 1E7 -* @param alt Altitude in meters, expressed as * 1000 (millimeters) -* @param vx Ground X Speed (Latitude), expressed as m/s * 100 -* @param vy Ground Y Speed (Longitude), expressed as m/s * 100 -* @param vz Ground Z Speed (Altitude), expressed as m/s * 100 -* @param xacc X acceleration (mg) -* @param yacc Y acceleration (mg) -* @param zacc Z acceleration (mg) -*/ -#ifndef __mobile__ -void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, - float pitchspeed, float yawspeed, double lat, double lon, double alt, - float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) -{ - if (!_vehicle) - { - return; - } - - if (_vehicle->hilMode()) - { - float q[4]; - - double cosPhi_2 = cos(double(roll) / 2.0); - double sinPhi_2 = sin(double(roll) / 2.0); - double cosTheta_2 = cos(double(pitch) / 2.0); - double sinTheta_2 = sin(double(pitch) / 2.0); - double cosPsi_2 = cos(double(yaw) / 2.0); - double sinPsi_2 = sin(double(yaw) / 2.0); - q[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 + - sinPhi_2 * sinTheta_2 * sinPsi_2); - q[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 - - cosPhi_2 * sinTheta_2 * sinPsi_2); - q[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 + - sinPhi_2 * cosTheta_2 * sinPsi_2); - q[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 - - sinPhi_2 * sinTheta_2 * cosPsi_2); - - mavlink_message_t msg; - mavlink_msg_hil_state_quaternion_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, - time_us, q, rollspeed, pitchspeed, yawspeed, - lat * 1e7f, lon * 1e7f, alt * 1000, vx * 100, vy * 100, vz * 100, ind_airspeed * 100, true_airspeed * 100, xacc * 1000 / 9.81, yacc * 1000 / 9.81, zacc * 1000 / 9.81); - _vehicle->sendMessageOnPriorityLink(msg); - } - - else - { - // Attempt to set HIL mode - _vehicle->setHilMode(true); - qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; - } -} -#endif - -#ifndef __mobile__ -float UAS::addZeroMeanNoise(float truth_meas, float noise_var) -{ - /* Calculate normally distributed variable noise with mean = 0 and variance = noise_var. Calculated according to - Box-Muller transform */ - static const float epsilon = std::numeric_limits::min(); //used to ensure non-zero uniform numbers - static float z0; //calculated normal distribution random variables with mu = 0, var = 1; - float u1, u2; //random variables generated from c++ rand(); - - /*Generate random variables in range (0 1] */ - do - { - //TODO seed rand() with srand(time) but srand(time should be called once on startup) - //currently this will generate repeatable random noise - u1 = rand() * (1.0 / RAND_MAX); - u2 = rand() * (1.0 / RAND_MAX); - } - while (u1 <= epsilon); //Have a catch to ensure non-zero for log() - - z0 = sqrt(-2.0 * log(u1)) * cos(2.0f * M_PI * u2); //calculate normally distributed variable with mu = 0, var = 1 - - //TODO add bias term that changes randomly to simulate accelerometer and gyro bias the exf should handle these - //as well - float noise = z0 * sqrt(noise_var); //calculate normally distributed variable with mu = 0, std = var^2 - - //Finally guard against any case where the noise is not real - if (std::isfinite(noise)) - { - return truth_meas + noise; - } - - else - { - return truth_meas; - } -} -#endif - -/* -* @param abs_pressure Absolute Pressure (hPa) -* @param diff_pressure Differential Pressure (hPa) -*/ -#ifndef __mobile__ -void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, - float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed) -{ - if (!_vehicle) - { - return; - } - - if (_vehicle->hilMode()) - { - float xacc_corrupt = addZeroMeanNoise(xacc, xacc_var); - float yacc_corrupt = addZeroMeanNoise(yacc, yacc_var); - float zacc_corrupt = addZeroMeanNoise(zacc, zacc_var); - float rollspeed_corrupt = addZeroMeanNoise(rollspeed, rollspeed_var); - float pitchspeed_corrupt = addZeroMeanNoise(pitchspeed, pitchspeed_var); - float yawspeed_corrupt = addZeroMeanNoise(yawspeed, yawspeed_var); - float xmag_corrupt = addZeroMeanNoise(xmag, xmag_var); - float ymag_corrupt = addZeroMeanNoise(ymag, ymag_var); - float zmag_corrupt = addZeroMeanNoise(zmag, zmag_var); - float abs_pressure_corrupt = addZeroMeanNoise(abs_pressure, abs_pressure_var); - float diff_pressure_corrupt = addZeroMeanNoise(diff_pressure, diff_pressure_var); - float pressure_alt_corrupt = addZeroMeanNoise(pressure_alt, pressure_alt_var); - float temperature_corrupt = addZeroMeanNoise(temperature, temperature_var); - - mavlink_message_t msg; - mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, - time_us, xacc_corrupt, yacc_corrupt, zacc_corrupt, rollspeed_corrupt, pitchspeed_corrupt, - yawspeed_corrupt, xmag_corrupt, ymag_corrupt, zmag_corrupt, abs_pressure_corrupt, - diff_pressure_corrupt, pressure_alt_corrupt, temperature_corrupt, fields_changed); - _vehicle->sendMessageOnPriorityLink(msg); - lastSendTimeSensors = QGC::groundTimeMilliseconds(); - } - - else - { - // Attempt to set HIL mode - _vehicle->setHilMode(true); - qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; - } -} -#endif - -#ifndef __mobile__ -void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, - float flow_comp_m_y, quint8 quality, float ground_distance) -{ - if (!_vehicle) - { - return; - } - - // FIXME: This needs to be updated for new mavlink_msg_hil_optical_flow_pack api - - Q_UNUSED(time_us); - Q_UNUSED(flow_x); - Q_UNUSED(flow_y); - Q_UNUSED(flow_comp_m_x); - Q_UNUSED(flow_comp_m_y); - Q_UNUSED(quality); - Q_UNUSED(ground_distance); - - if (_vehicle->hilMode()) - { -#if 0 - mavlink_message_t msg; - mavlink_msg_hil_optical_flow_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, - time_us, 0, 0 /* hack */, flow_x, flow_y, 0.0f /* hack */, 0.0f /* hack */, 0.0f /* hack */, 0 /* hack */, quality, ground_distance); - - _vehicle->sendMessageOnPriorityLink(msg); - lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds(); -#endif - } - - else - { - // Attempt to set HIL mode - _vehicle->setHilMode(true); - qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; - } - -} -#endif - -#ifndef __mobile__ -void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites) -{ - if (!_vehicle) - { - return; - } - - // Only send at 10 Hz max rate - if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100) - return; - - if (_vehicle->hilMode()) - { - float course = cog; - - // map to 0..2pi - if (course < 0) - course += 2.0f * static_cast(M_PI); - - // scale from radians to degrees - course = (course / M_PI) * 180.0f; - - mavlink_message_t msg; - mavlink_msg_hil_gps_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, - time_us, fix_type, lat * 1e7, lon * 1e7, alt * 1e3, eph * 1e2, epv * 1e2, vel * 1e2, vn * 1e2, ve * 1e2, vd * 1e2, course * 1e2, satellites); - lastSendTimeGPS = QGC::groundTimeMilliseconds(); - _vehicle->sendMessageOnPriorityLink(msg); - } - - else - { - // Attempt to set HIL mode - _vehicle->setHilMode(true); - qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; - } -} -#endif - -/** -* Connect flight gear link. -**/ -#ifndef __mobile__ -void UAS::startHil() -{ - if (hilEnabled) return; - - hilEnabled = true; - sensorHil = false; - _vehicle->setHilMode(true); - qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; - // Connect HIL simulation link - simulation->connectSimulation(); -} -#endif - -/** -* disable flight gear link. -*/ -#ifndef __mobile__ -void UAS::stopHil() -{ - if (simulation && simulation->isConnected()) - { - simulation->disconnectSimulation(); - _vehicle->setHilMode(false); - qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to disable."; - } - - hilEnabled = false; - sensorHil = false; -} -#endif - -/** -* @rerturn the map of the components -*/ -QMap UAS::getComponents() -{ - return components; -} - -void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax) -{ - if (!_vehicle) - { - return; - } - - mavlink_message_t message; - - char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {}; - - // Copy string into buffer, ensuring not to exceed the buffer size - for (unsigned int i = 0; i < sizeof(param_id_cstr); i++) - { - if ((int)i < param_id.length()) - { - param_id_cstr[i] = param_id.toLatin1()[i]; - } - } - - mavlink_msg_param_map_rc_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &message, - this->uasId, - _vehicle->defaultComponentId(), - param_id_cstr, - -1, - param_rc_channel_index, - value0, - scale, - valueMin, - valueMax); - _vehicle->sendMessageOnPriorityLink(message); - //qDebug() << "Mavlink message sent"; -} - -void UAS::unsetRCToParameterMap() -{ - if (!_vehicle) - { - return; - } - - char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {}; - - for (int i = 0; i < 3; i++) - { - mavlink_message_t message; - mavlink_msg_param_map_rc_pack(mavlink->getSystemId(), - mavlink->getComponentId(), - &message, - this->uasId, - _vehicle->defaultComponentId(), - param_id_cstr, - -2, - i, - 0.0f, - 0.0f, - 0.0f, - 0.0f); - _vehicle->sendMessageOnPriorityLink(message); - } -} - -void UAS::_say(const QString &text, int severity) -{ - Q_UNUSED(severity); - qgcApp()->toolbox()->audioOutput()->say(text); -} - -void UAS::shutdownVehicle(void) -{ -#ifndef __mobile__ - stopHil(); - - if (simulation) - { - // wait for the simulator to exit - simulation->wait(); - simulation->disconnectSimulation(); - simulation->deleteLater(); - } - -#endif - _vehicle = NULL; -} +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +/** + * @file + * @brief Represents one unmanned aerial vehicle + * + * @author Lorenz Meier + * + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "UAS.h" +#include "LinkInterface.h" +#include "HomePositionManager.h" +#include "QGC.h" +#include "GAudioOutput.h" +#include "MAVLinkProtocol.h" +#include "QGCMAVLink.h" +#include "LinkManager.h" +#ifndef __ios__ +#include "SerialLink.h" +#endif +#include "FirmwarePluginManager.h" +#include "QGCLoggingCategory.h" +#include "Vehicle.h" +#include "Joystick.h" +#include "QGCApplication.h" + +QGC_LOGGING_CATEGORY(UASLog, "UASLog") + +/** +* Gets the settings from the previous UAS (name, airframe, autopilot, battery specs) +* by calling readSettings. This means the new UAS will have the same settings +* as the previous one created unless one calls deleteSettings in the code after +* creating the UAS. +*/ + +UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * firmwarePluginManager) : UASInterface(), + lipoFull(4.2f), + lipoEmpty(3.5f), + uasId(vehicle->id()), + unknownPackets(), + mavlink(protocol), + receiveDropRate(0), + sendDropRate(0), + + status(-1), + + startTime(QGC::groundTimeMilliseconds()), + onboardTimeOffset(0), + + controlRollManual(true), + controlPitchManual(true), + controlYawManual(true), + controlThrustManual(true), + manualRollAngle(0), + manualPitchAngle(0), + manualYawAngle(0), + manualThrust(0), + + isGlobalPositionKnown(false), + + latitude(0.0), + longitude(0.0), + altitudeAMSL(0.0), + altitudeAMSLFT(0.0), + altitudeRelative(0.0), + + satRawHDOP(1e10f), + satRawVDOP(1e10f), + satRawCOG(0.0), + + globalEstimatorActive(false), + + latitude_gps(0.0), + longitude_gps(0.0), + altitude_gps(0.0), + + speedX(0.0), + speedY(0.0), + speedZ(0.0), + + airSpeed(std::numeric_limits::quiet_NaN()), + groundSpeed(std::numeric_limits::quiet_NaN()), +#ifndef __mobile__ + fileManager(this, vehicle), +#endif + + attitudeKnown(false), + attitudeStamped(false), + lastAttitude(0), + + roll(0.0), + pitch(0.0), + yaw(0.0), + + imagePackets(0), // We must initialize to 0, otherwise extended data packets maybe incorrectly thought to be images + + blockHomePositionChanges(false), + receivedMode(false), + + // Note variances calculated from flight case from this log: http://dash.oznet.ch/view/MRjW8NUNYQSuSZkbn8dEjY + // TODO: calibrate stand-still pixhawk variances + xacc_var(0.6457f), + yacc_var(0.7048f), + zacc_var(0.97885f), + rollspeed_var(0.8126f), + pitchspeed_var(0.6145f), + yawspeed_var(0.5852f), + xmag_var(0.2393f), + ymag_var(0.2283f), + zmag_var(0.1665f), + abs_pressure_var(0.5802f), + diff_pressure_var(0.5802f), + pressure_alt_var(0.5802f), + temperature_var(0.7145f), + /* + xacc_var(0.0f), + yacc_var(0.0f), + zacc_var(0.0f), + rollspeed_var(0.0f), + pitchspeed_var(0.0f), + yawspeed_var(0.0f), + xmag_var(0.0f), + ymag_var(0.0f), + zmag_var(0.0f), + abs_pressure_var(0.0f), + diff_pressure_var(0.0f), + pressure_alt_var(0.0f), + temperature_var(0.0f), + */ + +#ifndef __mobile__ + simulation(0), +#endif + + // The protected members. + connectionLost(false), + lastVoltageWarning(0), + lastNonNullTime(0), + onboardTimeOffsetInvalidCount(0), + hilEnabled(false), + sensorHil(false), + lastSendTimeGPS(0), + lastSendTimeSensors(0), + lastSendTimeOpticalFlow(0), + _vehicle(vehicle), + _firmwarePluginManager(firmwarePluginManager) +{ + + for (unsigned int i = 0; i<255;++i) + { + componentID[i] = -1; + componentMulti[i] = false; + } + +#ifndef __mobile__ + connect(_vehicle, &Vehicle::mavlinkMessageReceived, &fileManager, &FileManager::receiveMessage); +#endif + + color = UASInterface::getNextColor(); +} + +/** +* @ return the id of the uas +*/ +int UAS::getUASID() const +{ + return uasId; +} + +void UAS::receiveMessage(mavlink_message_t message) +{ + if (!components.contains(message.compid)) + { + QString componentName; + + switch (message.compid) + { + case MAV_COMP_ID_ALL: + { + componentName = "ANONYMOUS"; + break; + } + case MAV_COMP_ID_IMU: + { + componentName = "IMU #1"; + break; + } + case MAV_COMP_ID_CAMERA: + { + componentName = "CAMERA"; + break; + } + case MAV_COMP_ID_MISSIONPLANNER: + { + componentName = "MISSIONPLANNER"; + break; + } + } + + components.insert(message.compid, componentName); + } + + // qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq; + + // Only accept messages from this system (condition 1) + // and only then if a) attitudeStamped is disabled OR b) attitudeStamped is enabled + // and we already got one attitude packet + if (message.sysid == uasId && (!attitudeStamped || (attitudeStamped && (lastAttitude != 0)) || message.msgid == MAVLINK_MSG_ID_ATTITUDE)) + { + QString uasState; + QString stateDescription; + + bool multiComponentSourceDetected = false; + bool wrongComponent = false; + + switch (message.compid) + { + case MAV_COMP_ID_IMU_2: + // Prefer IMU 2 over IMU 1 (FIXME) + componentID[message.msgid] = MAV_COMP_ID_IMU_2; + break; + default: + // Do nothing + break; + } + + // Store component ID + if (componentID[message.msgid] == -1) + { + // Prefer the first component + componentID[message.msgid] = message.compid; + } + else + { + // Got this message already + if (componentID[message.msgid] != message.compid) + { + componentMulti[message.msgid] = true; + wrongComponent = true; + } + } + + if (componentMulti[message.msgid] == true) multiComponentSourceDetected = true; + + + switch (message.msgid) + { + case MAVLINK_MSG_ID_HEARTBEAT: + { + if (multiComponentSourceDetected && wrongComponent) + { + break; + } + mavlink_heartbeat_t state; + mavlink_msg_heartbeat_decode(&message, &state); + + // Send the base_mode and system_status values to the plotter. This uses the ground time + // so the Ground Time checkbox must be ticked for these values to display + quint64 time = getUnixTime(); + QString name = QString("M%1:HEARTBEAT.%2").arg(message.sysid); + emit valueChanged(uasId, name.arg("base_mode"), "bits", state.base_mode, time); + emit valueChanged(uasId, name.arg("custom_mode"), "bits", state.custom_mode, time); + emit valueChanged(uasId, name.arg("system_status"), "-", state.system_status, time); + + if ((state.system_status != this->status) && state.system_status != MAV_STATE_UNINIT) + { + this->status = state.system_status; + getStatusForCode((int)state.system_status, uasState, stateDescription); + emit statusChanged(this, uasState, stateDescription); + emit statusChanged(this->status); + } + + // We got the mode + receivedMode = true; + } + + break; + + case MAVLINK_MSG_ID_SYS_STATUS: + { + if (multiComponentSourceDetected && wrongComponent) + { + break; + } + mavlink_sys_status_t state; + mavlink_msg_sys_status_decode(&message, &state); + + // Prepare for sending data to the realtime plotter, which is every field excluding onboard_control_sensors_present. + quint64 time = getUnixTime(); + QString name = QString("M%1:SYS_STATUS.%2").arg(message.sysid); + emit valueChanged(uasId, name.arg("sensors_enabled"), "bits", state.onboard_control_sensors_enabled, time); + emit valueChanged(uasId, name.arg("sensors_health"), "bits", state.onboard_control_sensors_health, time); + emit valueChanged(uasId, name.arg("errors_comm"), "-", state.errors_comm, time); + emit valueChanged(uasId, name.arg("errors_count1"), "-", state.errors_count1, time); + emit valueChanged(uasId, name.arg("errors_count2"), "-", state.errors_count2, time); + emit valueChanged(uasId, name.arg("errors_count3"), "-", state.errors_count3, time); + emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time); + + // Process CPU load. + emit loadChanged(this,state.load/10.0f); + emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time); + + // control_sensors_enabled: + // relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control + emit attitudeControlEnabled(state.onboard_control_sensors_enabled & (1 << 11)); + emit positionYawControlEnabled(state.onboard_control_sensors_enabled & (1 << 12)); + emit positionZControlEnabled(state.onboard_control_sensors_enabled & (1 << 13)); + emit positionXYControlEnabled(state.onboard_control_sensors_enabled & (1 << 14)); + + // Trigger drop rate updates as needed. Here we convert the incoming + // drop_rate_comm value from 1/100 of a percent in a uint16 to a true + // percentage as a float. We also cap the incoming value at 100% as defined + // by the MAVLink specifications. + if (state.drop_rate_comm > 10000) + { + state.drop_rate_comm = 10000; + } + emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f); + emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time); + } + break; + case MAVLINK_MSG_ID_ATTITUDE: + { + mavlink_attitude_t attitude; + mavlink_msg_attitude_decode(&message, &attitude); + quint64 time = getUnixReferenceTime(attitude.time_boot_ms); + + emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(attitude.roll), QGC::limitAngleToPMPIf(attitude.pitch), QGC::limitAngleToPMPIf(attitude.yaw), time); + + if (!wrongComponent) + { + lastAttitude = time; + setRoll(QGC::limitAngleToPMPIf(attitude.roll)); + setPitch(QGC::limitAngleToPMPIf(attitude.pitch)); + setYaw(QGC::limitAngleToPMPIf(attitude.yaw)); + + attitudeKnown = true; + emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); + emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); + } + } + break; + case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: + { + mavlink_attitude_quaternion_t attitude; + mavlink_msg_attitude_quaternion_decode(&message, &attitude); + quint64 time = getUnixReferenceTime(attitude.time_boot_ms); + + double a = attitude.q1; + double b = attitude.q2; + double c = attitude.q3; + double d = attitude.q4; + + double aSq = a * a; + double bSq = b * b; + double cSq = c * c; + double dSq = d * d; + float dcm[3][3]; + dcm[0][0] = aSq + bSq - cSq - dSq; + dcm[0][1] = 2.0 * (b * c - a * d); + dcm[0][2] = 2.0 * (a * c + b * d); + dcm[1][0] = 2.0 * (b * c + a * d); + dcm[1][1] = aSq - bSq + cSq - dSq; + dcm[1][2] = 2.0 * (c * d - a * b); + dcm[2][0] = 2.0 * (b * d - a * c); + dcm[2][1] = 2.0 * (a * b + c * d); + dcm[2][2] = aSq - bSq - cSq + dSq; + + float phi, theta, psi; + theta = asin(-dcm[2][0]); + + if (fabs(theta - M_PI_2) < 1.0e-3f) { + phi = 0.0f; + psi = (atan2(dcm[1][2] - dcm[0][1], + dcm[0][2] + dcm[1][1]) + phi); + + } else if (fabs(theta + M_PI_2) < 1.0e-3f) { + phi = 0.0f; + psi = atan2f(dcm[1][2] - dcm[0][1], + dcm[0][2] + dcm[1][1] - phi); + + } else { + phi = atan2f(dcm[2][1], dcm[2][2]); + psi = atan2f(dcm[1][0], dcm[0][0]); + } + + emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(phi), + QGC::limitAngleToPMPIf(theta), + QGC::limitAngleToPMPIf(psi), time); + + if (!wrongComponent) + { + lastAttitude = time; + setRoll(QGC::limitAngleToPMPIf(phi)); + setPitch(QGC::limitAngleToPMPIf(theta)); + setYaw(QGC::limitAngleToPMPIf(psi)); + + attitudeKnown = true; + emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); + emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); + } + } + break; + case MAVLINK_MSG_ID_HIL_CONTROLS: + { + mavlink_hil_controls_t hil; + mavlink_msg_hil_controls_decode(&message, &hil); + emit hilControlsChanged(hil.time_usec, hil.roll_ailerons, hil.pitch_elevator, hil.yaw_rudder, hil.throttle, hil.mode, hil.nav_mode); + } + break; + case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS: + { + mavlink_hil_actuator_controls_t hil; + mavlink_msg_hil_actuator_controls_decode(&message, &hil); + emit hilActuatorControlsChanged(hil.time_usec, hil.flags, + hil.controls[0], + hil.controls[1], + hil.controls[2], + hil.controls[3], + hil.controls[4], + hil.controls[5], + hil.controls[6], + hil.controls[7], + hil.controls[8], + hil.controls[9], + hil.controls[10], + hil.controls[11], + hil.controls[12], + hil.controls[13], + hil.controls[14], + hil.controls[15], + hil.mode); + } + break; + case MAVLINK_MSG_ID_VFR_HUD: + { + mavlink_vfr_hud_t hud; + mavlink_msg_vfr_hud_decode(&message, &hud); + quint64 time = getUnixTime(); + // Display updated values + emit thrustChanged(this, hud.throttle/100.0); + + if (!attitudeKnown) + { + setYaw(QGC::limitAngleToPMPId((((double)hud.heading)/180.0)*M_PI)); + emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time); + } + + setAltitudeAMSL(hud.alt); + setGroundSpeed(hud.groundspeed); + if (!qIsNaN(hud.airspeed)) + setAirSpeed(hud.airspeed); + speedZ = -hud.climb; + emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); + emit speedChanged(this, groundSpeed, airSpeed, time); + } + break; + case MAVLINK_MSG_ID_LOCAL_POSITION_NED: + //std::cerr << std::endl; + //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; + { + mavlink_local_position_ned_t pos; + mavlink_msg_local_position_ned_decode(&message, &pos); + quint64 time = getUnixTime(pos.time_boot_ms); + + if (!wrongComponent) + { + speedX = pos.vx; + speedY = pos.vy; + speedZ = pos.vz; + + // Emit + emit velocityChanged_NED(this, speedX, speedY, speedZ, time); + } + } + break; + case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE: + { + mavlink_global_vision_position_estimate_t pos; + mavlink_msg_global_vision_position_estimate_decode(&message, &pos); + quint64 time = getUnixTime(pos.usec); + emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time); + } + break; + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + //std::cerr << std::endl; + //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; + { + mavlink_global_position_int_t pos; + mavlink_msg_global_position_int_decode(&message, &pos); + + quint64 time = getUnixTime(); + + setLatitude(pos.lat/(double)1E7); + setLongitude(pos.lon/(double)1E7); + setAltitudeRelative(pos.relative_alt/1000.0); + + globalEstimatorActive = true; + + speedX = pos.vx/100.0; + speedY = pos.vy/100.0; + speedZ = pos.vz/100.0; + + emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time); + emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); + // We had some frame mess here, global and local axes were mixed. + emit velocityChanged_NED(this, speedX, speedY, speedZ, time); + + setGroundSpeed(qSqrt(speedX*speedX+speedY*speedY)); + emit speedChanged(this, groundSpeed, airSpeed, time); + + isGlobalPositionKnown = true; + } + break; + case MAVLINK_MSG_ID_GPS_RAW_INT: + { + mavlink_gps_raw_int_t pos; + mavlink_msg_gps_raw_int_decode(&message, &pos); + + quint64 time = getUnixTime(pos.time_usec); + + // TODO: track localization state not only for gps but also for other loc. sources + int loc_type = pos.fix_type; + if (loc_type == 1) + { + loc_type = 0; + } + setSatelliteCount(pos.satellites_visible); + + if (pos.fix_type > 2) + { + isGlobalPositionKnown = true; + + latitude_gps = pos.lat/(double)1E7; + longitude_gps = pos.lon/(double)1E7; + altitude_gps = pos.alt/1000.0; + + // If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead. + if (!globalEstimatorActive) { + setLatitude(latitude_gps); + setLongitude(longitude_gps); + emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time); + emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time); + + float vel = pos.vel/100.0f; + // Smaller than threshold and not NaN + if ((vel < 1000000) && !qIsNaN(vel) && !qIsInf(vel)) { + setGroundSpeed(vel); + emit speedChanged(this, groundSpeed, airSpeed, time); + } else { + emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_NOTICE, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel)); + } + } + } + + double dtmp; + //-- Raw GPS data + dtmp = pos.eph == 0xFFFF ? 1e10f : pos.eph / 100.0; + if(dtmp != satRawHDOP) + { + satRawHDOP = dtmp; + emit satRawHDOPChanged(satRawHDOP); + } + dtmp = pos.epv == 0xFFFF ? 1e10f : pos.epv / 100.0; + if(dtmp != satRawVDOP) + { + satRawVDOP = dtmp; + emit satRawVDOPChanged(satRawVDOP); + } + dtmp = pos.cog == 0xFFFF ? 0.0 : pos.cog / 100.0; + if(dtmp != satRawCOG) + { + satRawCOG = dtmp; + emit satRawCOGChanged(satRawCOG); + } + + // Emit this signal after the above signals. This way a trigger on gps lock signal which then asks for vehicle position + // gets a good position. + emit localizationChanged(this, loc_type); + } + break; + case MAVLINK_MSG_ID_GPS_STATUS: + { + mavlink_gps_status_t pos; + mavlink_msg_gps_status_decode(&message, &pos); + for(int i = 0; i < (int)pos.satellites_visible; i++) + { + emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast(pos.satellite_used[i])); + } + setSatelliteCount(pos.satellites_visible); + } + break; + + case MAVLINK_MSG_ID_PARAM_VALUE: + { + mavlink_param_value_t rawValue; + mavlink_msg_param_value_decode(&message, &rawValue); + QByteArray bytes(rawValue.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + // Construct a string stopping at the first NUL (0) character, else copy the whole + // byte array (max MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN, so safe) + QString parameterName(bytes); + mavlink_param_union_t paramVal; + paramVal.param_float = rawValue.param_value; + paramVal.type = rawValue.param_type; + + processParamValueMsg(message, parameterName,rawValue,paramVal); + } + break; + case MAVLINK_MSG_ID_ATTITUDE_TARGET: + { + mavlink_attitude_target_t out; + mavlink_msg_attitude_target_decode(&message, &out); + float roll, pitch, yaw; + mavlink_quaternion_to_euler(out.q, &roll, &pitch, &yaw); + quint64 time = getUnixTimeFromMs(out.time_boot_ms); + emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, out.thrust, time); + + // For plotting emit roll sp, pitch sp and yaw sp values + emit valueChanged(uasId, "roll sp", "rad", roll, time); + emit valueChanged(uasId, "pitch sp", "rad", pitch, time); + emit valueChanged(uasId, "yaw sp", "rad", yaw, time); + } + break; + + case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED: + { + if (multiComponentSourceDetected && wrongComponent) + { + break; + } + mavlink_position_target_local_ned_t p; + mavlink_msg_position_target_local_ned_decode(&message, &p); + quint64 time = getUnixTimeFromMs(p.time_boot_ms); + emit positionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */, time); + } + break; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + { + mavlink_set_position_target_local_ned_t p; + mavlink_msg_set_position_target_local_ned_decode(&message, &p); + emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */); + } + break; + case MAVLINK_MSG_ID_STATUSTEXT: + { + QByteArray b; + b.resize(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); + mavlink_msg_statustext_get_text(&message, b.data()); + + // Ensure NUL-termination + b[b.length()-1] = '\0'; + QString text = QString(b); + int severity = mavlink_msg_statustext_get_severity(&message); + + // If the message is NOTIFY or higher severity, or starts with a '#', + // then read it aloud. + if (text.startsWith("#") || severity <= MAV_SEVERITY_NOTICE) + { + text.remove("#"); + emit textMessageReceived(uasId, message.compid, severity, text); + _say(text.toLower(), severity); + } + else + { + emit textMessageReceived(uasId, message.compid, severity, text); + } + } + break; + + case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE: + { + mavlink_data_transmission_handshake_t p; + mavlink_msg_data_transmission_handshake_decode(&message, &p); + imageSize = p.size; + imagePackets = p.packets; + imagePayload = p.payload; + imageQuality = p.jpg_quality; + imageType = p.type; + imageWidth = p.width; + imageHeight = p.height; + imageStart = QGC::groundTimeMilliseconds(); + imagePacketsArrived = 0; + + } + break; + + case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + { + mavlink_encapsulated_data_t img; + mavlink_msg_encapsulated_data_decode(&message, &img); + int seq = img.seqnr; + int pos = seq * imagePayload; + + // Check if we have a valid transaction + if (imagePackets == 0) + { + // NO VALID TRANSACTION - ABORT + // Restart statemachine + imagePacketsArrived = 0; + break; + } + + for (int i = 0; i < imagePayload; ++i) + { + if (pos <= imageSize) { + imageRecBuffer[pos] = img.data[i]; + } + ++pos; + } + + ++imagePacketsArrived; + + // emit signal if all packets arrived + if (imagePacketsArrived >= imagePackets) + { + // Restart statemachine + imagePackets = 0; + imagePacketsArrived = 0; + emit imageReady(this); + } + } + break; + + case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: + { + mavlink_nav_controller_output_t p; + mavlink_msg_nav_controller_output_decode(&message,&p); + setDistToWaypoint(p.wp_dist); + setBearingToWaypoint(p.nav_bearing); + emit navigationControllerErrorsChanged(this, p.alt_error, p.aspd_error, p.xtrack_error); + emit NavigationControllerDataChanged(this, p.nav_roll, p.nav_pitch, p.nav_bearing, p.target_bearing, p.wp_dist); + } + break; + + case MAVLINK_MSG_ID_LOG_ENTRY: + { + mavlink_log_entry_t log; + mavlink_msg_log_entry_decode(&message, &log); + emit logEntry(this, log.time_utc, log.size, log.id, log.num_logs, log.last_log_num); + } + break; + + case MAVLINK_MSG_ID_LOG_DATA: + { + mavlink_log_data_t log; + mavlink_msg_log_data_decode(&message, &log); + emit logData(this, log.ofs, log.id, log.count, log.data); + } + break; + + default: + break; + } + } +} + +void UAS::startCalibration(UASInterface::StartCalibrationType calType) +{ + if (!_vehicle) { + return; + } + + int gyroCal = 0; + int magCal = 0; + int airspeedCal = 0; + int radioCal = 0; + int accelCal = 0; + int escCal = 0; + + switch (calType) { + case StartCalibrationGyro: + gyroCal = 1; + break; + case StartCalibrationMag: + magCal = 1; + break; + case StartCalibrationAirspeed: + airspeedCal = 1; + break; + case StartCalibrationRadio: + radioCal = 1; + break; + case StartCalibrationCopyTrims: + radioCal = 2; + break; + case StartCalibrationAccel: + accelCal = 1; + break; + case StartCalibrationLevel: + accelCal = 2; + break; + case StartCalibrationEsc: + escCal = 1; + break; + case StartCalibrationUavcanEsc: + escCal = 2; + break; + case StartCalibrationCompassMot: + airspeedCal = 1; // ArduPilot, bit of a hack + break; + } + + mavlink_message_t msg; + mavlink_msg_command_long_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + uasId, + _vehicle->defaultComponentId(), // target component + MAV_CMD_PREFLIGHT_CALIBRATION, // command id + 0, // 0=first transmission of command + gyroCal, // gyro cal + magCal, // mag cal + 0, // ground pressure + radioCal, // radio cal + accelCal, // accel cal + airspeedCal, // PX4: airspeed cal, ArduPilot: compass mot + escCal); // esc cal + _vehicle->sendMessageOnPriorityLink(msg); +} + +void UAS::stopCalibration(void) +{ + if (!_vehicle) { + return; + } + + mavlink_message_t msg; + mavlink_msg_command_long_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + uasId, + _vehicle->defaultComponentId(), // target component + MAV_CMD_PREFLIGHT_CALIBRATION, // command id + 0, // 0=first transmission of command + 0, // gyro cal + 0, // mag cal + 0, // ground pressure + 0, // radio cal + 0, // accel cal + 0, // airspeed cal + 0); // unused + _vehicle->sendMessageOnPriorityLink(msg); +} + +void UAS::startBusConfig(UASInterface::StartBusConfigType calType) +{ + if (!_vehicle) { + return; + } + + int actuatorCal = 0; + + switch (calType) { + case StartBusConfigActuators: + actuatorCal = 1; + break; + case EndBusConfigActuators: + actuatorCal = 0; + break; + } + + mavlink_message_t msg; + mavlink_msg_command_long_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + uasId, + _vehicle->defaultComponentId(), // target component + MAV_CMD_PREFLIGHT_UAVCAN, // command id + 0, // 0=first transmission of command + actuatorCal, // actuators + 0, + 0, + 0, + 0, + 0, + 0); + _vehicle->sendMessageOnPriorityLink(msg); +} + +void UAS::stopBusConfig(void) +{ + if (!_vehicle) { + return; + } + + mavlink_message_t msg; + mavlink_msg_command_long_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &msg, + uasId, + _vehicle->defaultComponentId(), // target component + MAV_CMD_PREFLIGHT_UAVCAN, // command id + 0, // 0=first transmission of command + 0, + 0, + 0, + 0, + 0, + 0, + 0); + _vehicle->sendMessageOnPriorityLink(msg); +} + +/** +* Check if time is smaller than 40 years, assuming no system without Unix +* timestamp runs longer than 40 years continuously without reboot. In worst case +* this will add/subtract the communication delay between GCS and MAV, it will +* never alter the timestamp in a safety critical way. +*/ +quint64 UAS::getUnixReferenceTime(quint64 time) +{ + // Same as getUnixTime, but does not react to attitudeStamped mode + if (time == 0) + { + // qDebug() << "XNEW time:" < has to be + // a Unix epoch timestamp. Do nothing. + return time/1000; + } +} + +/** +* @warning If attitudeStamped is enabled, this function will not actually return +* the precise time stamp of this measurement augmented to UNIX time, but will +* MOVE the timestamp IN TIME to match the last measured attitude. There is no +* reason why one would want this, except for system setups where the onboard +* clock is not present or broken and datasets should be collected that are still +* roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED RUINS THE +* SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL! +*/ +quint64 UAS::getUnixTimeFromMs(quint64 time) +{ + return getUnixTime(time*1000); +} + +/** +* @warning If attitudeStamped is enabled, this function will not actually return +* the precise time stam of this measurement augmented to UNIX time, but will +* MOVE the timestamp IN TIME to match the last measured attitude. There is no +* reason why one would want this, except for system setups where the onboard +* clock is not present or broken and datasets should be collected that are +* still roughly synchronized. PLEASE NOTE THAT ENABLING ATTITUDE STAMPED +* RUINS THE SCIENTIFIC NATURE OF THE CORRECT LOGGING FUNCTIONS OF QGROUNDCONTROL! +*/ +quint64 UAS::getUnixTime(quint64 time) +{ + quint64 ret = 0; + if (attitudeStamped) + { + ret = lastAttitude; + } + + if (time == 0) + { + ret = QGC::groundTimeMilliseconds(); + } + // Check if time is smaller than 40 years, + // assuming no system without Unix timestamp + // runs longer than 40 years continuously without + // reboot. In worst case this will add/subtract the + // communication delay between GCS and MAV, + // it will never alter the timestamp in a safety + // critical way. + // + // Calculation: + // 40 years + // 365 days + // 24 hours + // 60 minutes + // 60 seconds + // 1000 milliseconds + // 1000 microseconds +#ifndef _MSC_VER + else if (time < 1261440000000000LLU) +#else + else if (time < 1261440000000000) +#endif + { + // qDebug() << "GEN time:" << time/1000 + onboardTimeOffset; + if (onboardTimeOffset == 0 || time < (lastNonNullTime - 100)) + { + lastNonNullTime = time; + onboardTimeOffset = QGC::groundTimeMilliseconds() - time/1000; + } + if (time > lastNonNullTime) lastNonNullTime = time; + + ret = time/1000 + onboardTimeOffset; + } + else + { + // Time is not zero and larger than 40 years -> has to be + // a Unix epoch timestamp. Do nothing. + ret = time/1000; + } + + return ret; +} + +/** +* Get the status of the code and a description of the status. +* Status can be unitialized, booting up, calibrating sensors, active +* standby, cirtical, emergency, shutdown or unknown. +*/ +void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription) +{ + switch (statusCode) + { + case MAV_STATE_UNINIT: + uasState = tr("UNINIT"); + stateDescription = tr("Unitialized, booting up."); + break; + case MAV_STATE_BOOT: + uasState = tr("BOOT"); + stateDescription = tr("Booting system, please wait."); + break; + case MAV_STATE_CALIBRATING: + uasState = tr("CALIBRATING"); + stateDescription = tr("Calibrating sensors, please wait."); + break; + case MAV_STATE_ACTIVE: + uasState = tr("ACTIVE"); + stateDescription = tr("Active, normal operation."); + break; + case MAV_STATE_STANDBY: + uasState = tr("STANDBY"); + stateDescription = tr("Standby mode, ready for launch."); + break; + case MAV_STATE_CRITICAL: + uasState = tr("CRITICAL"); + stateDescription = tr("FAILURE: Continuing operation."); + break; + case MAV_STATE_EMERGENCY: + uasState = tr("EMERGENCY"); + stateDescription = tr("EMERGENCY: Land Immediately!"); + break; + //case MAV_STATE_HILSIM: + //uasState = tr("HIL SIM"); + //stateDescription = tr("HIL Simulation, Sensors read from SIM"); + //break; + + case MAV_STATE_POWEROFF: + uasState = tr("SHUTDOWN"); + stateDescription = tr("Powering off system."); + break; + + default: + uasState = tr("UNKNOWN"); + stateDescription = tr("Unknown system state"); + break; + } +} + +QImage UAS::getImage() +{ + +// qDebug() << "IMAGE TYPE:" << imageType; + + // RAW greyscale + if (imageType == MAVLINK_DATA_STREAM_IMG_RAW8U) + { + int imgColors = 255; + + // Construct PGM header + QString header("P5\n%1 %2\n%3\n"); + header = header.arg(imageWidth).arg(imageHeight).arg(imgColors); + + QByteArray tmpImage(header.toStdString().c_str(), header.length()); + tmpImage.append(imageRecBuffer); + + //qDebug() << "IMAGE SIZE:" << tmpImage.size() << "HEADER SIZE: (15):" << header.size() << "HEADER: " << header; + + if (imageRecBuffer.isNull()) + { + qDebug()<< "could not convertToPGM()"; + return QImage(); + } + + if (!image.loadFromData(tmpImage, "PGM")) + { + qDebug()<< __FILE__ << __LINE__ << "could not create extracted image"; + return QImage(); + } + + } + // BMP with header + else if (imageType == MAVLINK_DATA_STREAM_IMG_BMP || + imageType == MAVLINK_DATA_STREAM_IMG_JPEG || + imageType == MAVLINK_DATA_STREAM_IMG_PGM || + imageType == MAVLINK_DATA_STREAM_IMG_PNG) + { + if (!image.loadFromData(imageRecBuffer)) + { + qDebug() << __FILE__ << __LINE__ << "Loading data from image buffer failed!"; + return QImage(); + } + } + + // Restart statemachine + imagePacketsArrived = 0; + imagePackets = 0; + imageRecBuffer.clear(); + return image; +} + +void UAS::requestImage() +{ + if (!_vehicle) { + return; + } + + qDebug() << "trying to get an image from the uas..."; + + // check if there is already an image transmission going on + if (imagePacketsArrived == 0) + { + mavlink_message_t msg; + mavlink_msg_data_transmission_handshake_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, MAVLINK_DATA_STREAM_IMG_JPEG, 0, 0, 0, 0, 0, 50); + _vehicle->sendMessageOnPriorityLink(msg); + } +} + + +/* MANAGEMENT */ + +/** + * + * @return The uptime in milliseconds + * + */ +quint64 UAS::getUptime() const +{ + if(startTime == 0) + { + return 0; + } + else + { + return QGC::groundTimeMilliseconds() - startTime; + } +} + +//TODO update this to use the parameter manager / param data model instead +void UAS::processParamValueMsg(mavlink_message_t& msg, const QString& paramName, const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramUnion) +{ + int compId = msg.compid; + + QVariant paramValue; + + // Insert with correct type + + switch (rawValue.param_type) { + case MAV_PARAM_TYPE_REAL32: + paramValue = QVariant(paramUnion.param_float); + break; + + case MAV_PARAM_TYPE_UINT8: + paramValue = QVariant(paramUnion.param_uint8); + break; + + case MAV_PARAM_TYPE_INT8: + paramValue = QVariant(paramUnion.param_int8); + break; + + case MAV_PARAM_TYPE_UINT16: + paramValue = QVariant(paramUnion.param_uint16); + break; + + case MAV_PARAM_TYPE_INT16: + paramValue = QVariant(paramUnion.param_int16); + break; + + case MAV_PARAM_TYPE_UINT32: + paramValue = QVariant(paramUnion.param_uint32); + break; + + case MAV_PARAM_TYPE_INT32: + paramValue = QVariant(paramUnion.param_int32); + break; + + //-- Note: These are not handled above: + // + // MAV_PARAM_TYPE_UINT64 + // MAV_PARAM_TYPE_INT64 + // MAV_PARAM_TYPE_REAL64 + // + // No space in message (the only storage allocation is a "float") and not present in mavlink_param_union_t + + default: + qCritical() << "INVALID DATA TYPE USED AS PARAMETER VALUE: " << rawValue.param_type; + } + + qCDebug(UASLog) << "Received PARAM_VALUE" << paramName << paramValue << rawValue.param_type; + + emit parameterUpdate(uasId, compId, paramName, rawValue.param_count, rawValue.param_index, rawValue.param_type, paramValue); +} + +void UAS::executeCommand(MAV_CMD command, int confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7, int component) +{ + if (!_vehicle) { + return; + } + + mavlink_message_t msg; + mavlink_command_long_t cmd; + cmd.command = (uint16_t)command; + cmd.confirmation = confirmation; + cmd.param1 = param1; + cmd.param2 = param2; + cmd.param3 = param3; + cmd.param4 = param4; + cmd.param5 = param5; + cmd.param6 = param6; + cmd.param7 = param7; + cmd.target_system = uasId; + cmd.target_component = component; + mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd); + _vehicle->sendMessageOnPriorityLink(msg); +} + +/** +* Set the manual control commands. +* This can only be done if the system has manual inputs enabled and is armed. +*/ +void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode) +{ + if (!_vehicle) { + return; + } + + // Store the previous manual commands + static float manualRollAngle = 0.0; + static float manualPitchAngle = 0.0; + static float manualYawAngle = 0.0; + static float manualThrust = 0.0; + static quint16 manualButtons = 0; + static quint8 countSinceLastTransmission = 0; // Track how many calls to this function have occurred since the last MAVLink transmission + + // Transmit the external setpoints only if they've changed OR if it's been a little bit since they were last transmit. To make sure there aren't issues with + // response rate, we make sure that a message is transmit when the commands have changed, then one more time, and then switch to the lower transmission rate + // if no command inputs have changed. + + // The default transmission rate is 25Hz, but when no inputs have changed it drops down to 5Hz. + bool sendCommand = false; + if (countSinceLastTransmission++ >= 5) { + sendCommand = true; + countSinceLastTransmission = 0; + } else if ((!qIsNaN(roll) && roll != manualRollAngle) || (!qIsNaN(pitch) && pitch != manualPitchAngle) || + (!qIsNaN(yaw) && yaw != manualYawAngle) || (!qIsNaN(thrust) && thrust != manualThrust) || + buttons != manualButtons) { + sendCommand = true; + + // Ensure that another message will be sent the next time this function is called + countSinceLastTransmission = 10; + } + + // Now if we should trigger an update, let's do that + if (sendCommand) { + // Save the new manual control inputs + manualRollAngle = roll; + manualPitchAngle = pitch; + manualYawAngle = yaw; + manualThrust = thrust; + manualButtons = buttons; + + mavlink_message_t message; + + if (joystickMode == Vehicle::JoystickModeAttitude) { + // send an external attitude setpoint command (rate control disabled) + float attitudeQuaternion[4]; + mavlink_euler_to_quaternion(roll, pitch, yaw, attitudeQuaternion); + uint8_t typeMask = 0x7; // disable rate control + mavlink_msg_set_attitude_target_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &message, + QGC::groundTimeUsecs(), + this->uasId, + 0, + typeMask, + attitudeQuaternion, + 0, + 0, + 0, + thrust + ); + } else if (joystickMode == Vehicle::JoystickModePosition) { + // Send the the local position setpoint (local pos sp external message) + static float px = 0; + static float py = 0; + static float pz = 0; + //XXX: find decent scaling + px -= pitch; + py += roll; + pz -= 2.0f*(thrust-0.5); + uint16_t typeMask = (1<<11)|(7<<6)|(7<<3); // select only POSITION control + mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &message, + QGC::groundTimeUsecs(), + this->uasId, + 0, + MAV_FRAME_LOCAL_NED, + typeMask, + px, + py, + pz, + 0, + 0, + 0, + 0, + 0, + 0, + yaw, + 0 + ); + } else if (joystickMode == Vehicle::JoystickModeForce) { + // Send the the force setpoint (local pos sp external message) + float dcm[3][3]; + mavlink_euler_to_dcm(roll, pitch, yaw, dcm); + const float fx = -dcm[0][2] * thrust; + const float fy = -dcm[1][2] * thrust; + const float fz = -dcm[2][2] * thrust; + uint16_t typeMask = (3<<10)|(7<<3)|(7<<0)|(1<<9); // select only FORCE control (disable everything else) + mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &message, + QGC::groundTimeUsecs(), + this->uasId, + 0, + MAV_FRAME_LOCAL_NED, + typeMask, + 0, + 0, + 0, + 0, + 0, + 0, + fx, + fy, + fz, + 0, + 0 + ); + } else if (joystickMode == Vehicle::JoystickModeVelocity) { + // Send the the local velocity setpoint (local pos sp external message) + static float vx = 0; + static float vy = 0; + static float vz = 0; + static float yawrate = 0; + //XXX: find decent scaling + vx -= pitch; + vy += roll; + vz -= 2.0f*(thrust-0.5); + yawrate += yaw; //XXX: not sure what scale to apply here + uint16_t typeMask = (1<<10)|(7<<6)|(7<<0); // select only VELOCITY control + mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &message, + QGC::groundTimeUsecs(), + this->uasId, + 0, + MAV_FRAME_LOCAL_NED, + typeMask, + 0, + 0, + 0, + vx, + vy, + vz, + 0, + 0, + 0, + 0, + yawrate + ); + } else if (joystickMode == Vehicle::JoystickModeRC) { + + // Save the new manual control inputs + manualRollAngle = roll; + manualPitchAngle = pitch; + manualYawAngle = yaw; + manualThrust = thrust; + manualButtons = buttons; + + // Store scaling values for all 3 axes + const float axesScaling = 1.0 * 1000.0; + + // Calculate the new commands for roll, pitch, yaw, and thrust + const float newRollCommand = roll * axesScaling; + // negate pitch value because pitch is negative for pitching forward but mavlink message argument is positive for forward + const float newPitchCommand = -pitch * axesScaling; + const float newYawCommand = yaw * axesScaling; + const float newThrustCommand = thrust * axesScaling; + + //qDebug() << newRollCommand << newPitchCommand << newYawCommand << newThrustCommand; + + // Send the MANUAL_COMMAND message + mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &message, this->uasId, newPitchCommand, newRollCommand, newThrustCommand, newYawCommand, buttons); + } + + _vehicle->sendMessageOnPriorityLink(message); + // Emit an update in control values to other UI elements, like the HSI display + emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); + } +} + +#ifndef __mobile__ +void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw) +{ + if (!_vehicle) { + return; + } + const uint8_t base_mode = _vehicle->baseMode(); + + // If system has manual inputs enabled and is armed + if(((base_mode & MAV_MODE_FLAG_DECODE_POSITION_MANUAL) && (base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY)) || (base_mode & MAV_MODE_FLAG_HIL_ENABLED)) + { + mavlink_message_t message; + float q[4]; + mavlink_euler_to_quaternion(roll, pitch, yaw, q); + + float yawrate = 0.0f; + + // Do not control rates and throttle + quint8 mask = (1 << 0) | (1 << 1) | (1 << 2); // ignore rates + mask |= (1 << 6); // ignore throttle + mavlink_msg_set_attitude_target_pack(mavlink->getSystemId(), mavlink->getComponentId(), + &message, QGC::groundTimeMilliseconds(), this->uasId, _vehicle->defaultComponentId(), + mask, q, 0, 0, 0, 0); + _vehicle->sendMessageOnPriorityLink(message); + quint16 position_mask = (1 << 3) | (1 << 4) | (1 << 5) | + (1 << 6) | (1 << 7) | (1 << 8); + mavlink_msg_set_position_target_local_ned_pack(mavlink->getSystemId(), mavlink->getComponentId(), + &message, QGC::groundTimeMilliseconds(), this->uasId, _vehicle->defaultComponentId(), + MAV_FRAME_LOCAL_NED, position_mask, x, y, z, 0, 0, 0, 0, 0, 0, yaw, yawrate); + _vehicle->sendMessageOnPriorityLink(message); + qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGES: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw; + + //emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds()); + } + else + { + qDebug() << "3DMOUSE/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send 3DMouse commands first"; + } +} +#endif + +/** +* Order the robot to start receiver pairing +*/ +void UAS::pairRX(int rxType, int rxSubType) +{ + if (!_vehicle) { + return; + } + + mavlink_message_t msg; + + mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, _vehicle->defaultComponentId(), MAV_CMD_START_RX_PAIR, 0, rxType, rxSubType, 0, 0, 0, 0, 0); + _vehicle->sendMessageOnPriorityLink(msg); +} + +/** +* If enabled, connect the flight gear link. +*/ +#ifndef __mobile__ +void UAS::enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject * configuration) +{ + Q_UNUSED(configuration); + + QGCFlightGearLink* link = dynamic_cast(simulation); + if (!link) { + // Delete wrong sim + if (simulation) { + stopHil(); + delete simulation; + } + simulation = new QGCFlightGearLink(_vehicle, options); + } + + float noise_scaler = 0.0001f; + xacc_var = noise_scaler * 0.2914f; + yacc_var = noise_scaler * 0.2914f; + zacc_var = noise_scaler * 0.9577f; + rollspeed_var = noise_scaler * 0.8126f; + pitchspeed_var = noise_scaler * 0.6145f; + yawspeed_var = noise_scaler * 0.5852f; + xmag_var = noise_scaler * 0.0786f; + ymag_var = noise_scaler * 0.0566f; + zmag_var = noise_scaler * 0.0333f; + abs_pressure_var = noise_scaler * 0.5604f; + diff_pressure_var = noise_scaler * 0.2604f; + pressure_alt_var = noise_scaler * 0.5604f; + temperature_var = noise_scaler * 0.7290f; + + // Connect Flight Gear Link + link = dynamic_cast(simulation); + link->setStartupArguments(options); + link->sensorHilEnabled(sensorHil); + // FIXME: this signal is not on the base hil configuration widget, only on the FG widget + //QObject::connect(configuration, SIGNAL(barometerOffsetChanged(float)), link, SLOT(setBarometerOffset(float))); + if (enable) + { + startHil(); + } + else + { + stopHil(); + } +} +#endif + +/** +* If enabled, connect the JSBSim link. +*/ +#ifndef __mobile__ +void UAS::enableHilJSBSim(bool enable, QString options) +{ + QGCJSBSimLink* link = dynamic_cast(simulation); + if (!link) { + // Delete wrong sim + if (simulation) { + stopHil(); + delete simulation; + } + simulation = new QGCJSBSimLink(_vehicle, options); + } + // Connect Flight Gear Link + link = dynamic_cast(simulation); + link->setStartupArguments(options); + if (enable) + { + startHil(); + } + else + { + stopHil(); + } +} +#endif + +/** +* If enabled, connect the X-plane gear link. +*/ +#ifndef __mobile__ +void UAS::enableHilXPlane(bool enable) +{ + QGCXPlaneLink* link = dynamic_cast(simulation); + if (!link) { + if (simulation) { + stopHil(); + delete simulation; + } + simulation = new QGCXPlaneLink(_vehicle); + + float noise_scaler = 0.0001f; + xacc_var = noise_scaler * 0.2914f; + yacc_var = noise_scaler * 0.2914f; + zacc_var = noise_scaler * 0.9577f; + rollspeed_var = noise_scaler * 0.8126f; + pitchspeed_var = noise_scaler * 0.6145f; + yawspeed_var = noise_scaler * 0.5852f; + xmag_var = noise_scaler * 0.0786f; + ymag_var = noise_scaler * 0.0566f; + zmag_var = noise_scaler * 0.0333f; + abs_pressure_var = noise_scaler * 0.5604f; + diff_pressure_var = noise_scaler * 0.2604f; + pressure_alt_var = noise_scaler * 0.5604f; + temperature_var = noise_scaler * 0.7290f; + } + // Connect X-Plane Link + if (enable) + { + startHil(); + } + else + { + stopHil(); + } +} +#endif + +/** +* @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) +* @param roll Roll angle (rad) +* @param pitch Pitch angle (rad) +* @param yaw Yaw angle (rad) +* @param rollspeed Roll angular speed (rad/s) +* @param pitchspeed Pitch angular speed (rad/s) +* @param yawspeed Yaw angular speed (rad/s) +* @param lat Latitude, expressed as * 1E7 +* @param lon Longitude, expressed as * 1E7 +* @param alt Altitude in meters, expressed as * 1000 (millimeters) +* @param vx Ground X Speed (Latitude), expressed as m/s * 100 +* @param vy Ground Y Speed (Longitude), expressed as m/s * 100 +* @param vz Ground Z Speed (Altitude), expressed as m/s * 100 +* @param xacc X acceleration (mg) +* @param yacc Y acceleration (mg) +* @param zacc Z acceleration (mg) +*/ +#ifndef __mobile__ +void UAS::sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, + float pitchspeed, float yawspeed, double lat, double lon, double alt, + float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) +{ + Q_UNUSED(time_us); + Q_UNUSED(xacc); + Q_UNUSED(yacc); + Q_UNUSED(zacc); + + // Emit attitude for cross-check + emit valueChanged(uasId, "roll sim", "rad", roll, getUnixTime()); + emit valueChanged(uasId, "pitch sim", "rad", pitch, getUnixTime()); + emit valueChanged(uasId, "yaw sim", "rad", yaw, getUnixTime()); + + emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, getUnixTime()); + emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, getUnixTime()); + emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, getUnixTime()); + + emit valueChanged(uasId, "lat sim", "deg", lat*1e7, getUnixTime()); + emit valueChanged(uasId, "lon sim", "deg", lon*1e7, getUnixTime()); + emit valueChanged(uasId, "alt sim", "deg", alt*1e3, getUnixTime()); + + emit valueChanged(uasId, "vx sim", "m/s", vx*1e2, getUnixTime()); + emit valueChanged(uasId, "vy sim", "m/s", vy*1e2, getUnixTime()); + emit valueChanged(uasId, "vz sim", "m/s", vz*1e2, getUnixTime()); + + emit valueChanged(uasId, "IAS sim", "m/s", ind_airspeed, getUnixTime()); + emit valueChanged(uasId, "TAS sim", "m/s", true_airspeed, getUnixTime()); +} +#endif + +/** +* @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) +* @param roll Roll angle (rad) +* @param pitch Pitch angle (rad) +* @param yaw Yaw angle (rad) +* @param rollspeed Roll angular speed (rad/s) +* @param pitchspeed Pitch angular speed (rad/s) +* @param yawspeed Yaw angular speed (rad/s) +* @param lat Latitude, expressed as * 1E7 +* @param lon Longitude, expressed as * 1E7 +* @param alt Altitude in meters, expressed as * 1000 (millimeters) +* @param vx Ground X Speed (Latitude), expressed as m/s * 100 +* @param vy Ground Y Speed (Longitude), expressed as m/s * 100 +* @param vz Ground Z Speed (Altitude), expressed as m/s * 100 +* @param xacc X acceleration (mg) +* @param yacc Y acceleration (mg) +* @param zacc Z acceleration (mg) +*/ +#ifndef __mobile__ +void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, + float pitchspeed, float yawspeed, double lat, double lon, double alt, + float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc) +{ + if (!_vehicle) { + return; + } + + if (_vehicle->hilMode()) + { + float q[4]; + + double cosPhi_2 = cos(double(roll) / 2.0); + double sinPhi_2 = sin(double(roll) / 2.0); + double cosTheta_2 = cos(double(pitch) / 2.0); + double sinTheta_2 = sin(double(pitch) / 2.0); + double cosPsi_2 = cos(double(yaw) / 2.0); + double sinPsi_2 = sin(double(yaw) / 2.0); + q[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 + + sinPhi_2 * sinTheta_2 * sinPsi_2); + q[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 - + cosPhi_2 * sinTheta_2 * sinPsi_2); + q[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 + + sinPhi_2 * cosTheta_2 * sinPsi_2); + q[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 - + sinPhi_2 * sinTheta_2 * cosPsi_2); + + mavlink_message_t msg; + mavlink_msg_hil_state_quaternion_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, + time_us, q, rollspeed, pitchspeed, yawspeed, + lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, ind_airspeed*100, true_airspeed*100, xacc*1000/9.81, yacc*1000/9.81, zacc*1000/9.81); + _vehicle->sendMessageOnPriorityLink(msg); + } + else + { + // Attempt to set HIL mode + _vehicle->setHilMode(true); + qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; + } +} +#endif + +#ifndef __mobile__ +float UAS::addZeroMeanNoise(float truth_meas, float noise_var) +{ + /* Calculate normally distributed variable noise with mean = 0 and variance = noise_var. Calculated according to + Box-Muller transform */ + static const float epsilon = std::numeric_limits::min(); //used to ensure non-zero uniform numbers + static float z0; //calculated normal distribution random variables with mu = 0, var = 1; + float u1, u2; //random variables generated from c++ rand(); + + /*Generate random variables in range (0 1] */ + do + { + //TODO seed rand() with srand(time) but srand(time should be called once on startup) + //currently this will generate repeatable random noise + u1 = rand() * (1.0 / RAND_MAX); + u2 = rand() * (1.0 / RAND_MAX); + } + while ( u1 <= epsilon ); //Have a catch to ensure non-zero for log() + + z0 = sqrt(-2.0 * log(u1)) * cos(2.0f * M_PI * u2); //calculate normally distributed variable with mu = 0, var = 1 + + //TODO add bias term that changes randomly to simulate accelerometer and gyro bias the exf should handle these + //as well + float noise = z0 * sqrt(noise_var); //calculate normally distributed variable with mu = 0, std = var^2 + + //Finally guard against any case where the noise is not real + if(std::isfinite(noise)) { + return truth_meas + noise; + } else { + return truth_meas; + } +} +#endif + +/* +* @param abs_pressure Absolute Pressure (hPa) +* @param diff_pressure Differential Pressure (hPa) +*/ +#ifndef __mobile__ +void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, + float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed) +{ + if (!_vehicle) { + return; + } + + if (_vehicle->hilMode()) + { + float xacc_corrupt = addZeroMeanNoise(xacc, xacc_var); + float yacc_corrupt = addZeroMeanNoise(yacc, yacc_var); + float zacc_corrupt = addZeroMeanNoise(zacc, zacc_var); + float rollspeed_corrupt = addZeroMeanNoise(rollspeed,rollspeed_var); + float pitchspeed_corrupt = addZeroMeanNoise(pitchspeed,pitchspeed_var); + float yawspeed_corrupt = addZeroMeanNoise(yawspeed,yawspeed_var); + float xmag_corrupt = addZeroMeanNoise(xmag, xmag_var); + float ymag_corrupt = addZeroMeanNoise(ymag, ymag_var); + float zmag_corrupt = addZeroMeanNoise(zmag, zmag_var); + float abs_pressure_corrupt = addZeroMeanNoise(abs_pressure,abs_pressure_var); + float diff_pressure_corrupt = addZeroMeanNoise(diff_pressure, diff_pressure_var); + float pressure_alt_corrupt = addZeroMeanNoise(pressure_alt, pressure_alt_var); + float temperature_corrupt = addZeroMeanNoise(temperature,temperature_var); + + mavlink_message_t msg; + mavlink_msg_hil_sensor_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, + time_us, xacc_corrupt, yacc_corrupt, zacc_corrupt, rollspeed_corrupt, pitchspeed_corrupt, + yawspeed_corrupt, xmag_corrupt, ymag_corrupt, zmag_corrupt, abs_pressure_corrupt, + diff_pressure_corrupt, pressure_alt_corrupt, temperature_corrupt, fields_changed); + _vehicle->sendMessageOnPriorityLink(msg); + lastSendTimeSensors = QGC::groundTimeMilliseconds(); + } + else + { + // Attempt to set HIL mode + _vehicle->setHilMode(true); + qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; + } +} +#endif + +#ifndef __mobile__ +void UAS::sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, + float flow_comp_m_y, quint8 quality, float ground_distance) +{ + if (!_vehicle) { + return; + } + + // FIXME: This needs to be updated for new mavlink_msg_hil_optical_flow_pack api + + Q_UNUSED(time_us); + Q_UNUSED(flow_x); + Q_UNUSED(flow_y); + Q_UNUSED(flow_comp_m_x); + Q_UNUSED(flow_comp_m_y); + Q_UNUSED(quality); + Q_UNUSED(ground_distance); + + if (_vehicle->hilMode()) + { +#if 0 + mavlink_message_t msg; + mavlink_msg_hil_optical_flow_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, + time_us, 0, 0 /* hack */, flow_x, flow_y, 0.0f /* hack */, 0.0f /* hack */, 0.0f /* hack */, 0 /* hack */, quality, ground_distance); + + _vehicle->sendMessageOnPriorityLink(msg); + lastSendTimeOpticalFlow = QGC::groundTimeMilliseconds(); +#endif + } + else + { + // Attempt to set HIL mode + _vehicle->setHilMode(true); + qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; + } + +} +#endif + +#ifndef __mobile__ +void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float vn, float ve, float vd, float cog, int satellites) +{ + if (!_vehicle) { + return; + } + + // Only send at 10 Hz max rate + if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100) + return; + + if (_vehicle->hilMode()) + { + float course = cog; + // map to 0..2pi + if (course < 0) + course += 2.0f * static_cast(M_PI); + // scale from radians to degrees + course = (course / M_PI) * 180.0f; + + mavlink_message_t msg; + mavlink_msg_hil_gps_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, + time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, vn*1e2, ve*1e2, vd*1e2, course*1e2, satellites); + lastSendTimeGPS = QGC::groundTimeMilliseconds(); + _vehicle->sendMessageOnPriorityLink(msg); + } + else + { + // Attempt to set HIL mode + _vehicle->setHilMode(true); + qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; + } +} +#endif + +/** +* Connect flight gear link. +**/ +#ifndef __mobile__ +void UAS::startHil() +{ + if (hilEnabled) return; + hilEnabled = true; + sensorHil = false; + _vehicle->setHilMode(true); + qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to enable."; + // Connect HIL simulation link + simulation->connectSimulation(); +} +#endif + +/** +* disable flight gear link. +*/ +#ifndef __mobile__ +void UAS::stopHil() +{ + if (simulation && simulation->isConnected()) { + simulation->disconnectSimulation(); + _vehicle->setHilMode(false); + qDebug() << __FILE__ << __LINE__ << "HIL is onboard not enabled, trying to disable."; + } + hilEnabled = false; + sensorHil = false; +} +#endif + +/** +* @rerturn the map of the components +*/ +QMap UAS::getComponents() +{ + return components; +} + +void UAS::sendMapRCToParam(QString param_id, float scale, float value0, quint8 param_rc_channel_index, float valueMin, float valueMax) +{ + if (!_vehicle) { + return; + } + + mavlink_message_t message; + + char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {}; + // Copy string into buffer, ensuring not to exceed the buffer size + for (unsigned int i = 0; i < sizeof(param_id_cstr); i++) + { + if ((int)i < param_id.length()) + { + param_id_cstr[i] = param_id.toLatin1()[i]; + } + } + + mavlink_msg_param_map_rc_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &message, + this->uasId, + _vehicle->defaultComponentId(), + param_id_cstr, + -1, + param_rc_channel_index, + value0, + scale, + valueMin, + valueMax); + _vehicle->sendMessageOnPriorityLink(message); + //qDebug() << "Mavlink message sent"; +} + +void UAS::unsetRCToParameterMap() +{ + if (!_vehicle) { + return; + } + + char param_id_cstr[MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = {}; + + for (int i = 0; i < 3; i++) { + mavlink_message_t message; + mavlink_msg_param_map_rc_pack(mavlink->getSystemId(), + mavlink->getComponentId(), + &message, + this->uasId, + _vehicle->defaultComponentId(), + param_id_cstr, + -2, + i, + 0.0f, + 0.0f, + 0.0f, + 0.0f); + _vehicle->sendMessageOnPriorityLink(message); + } +} + +void UAS::_say(const QString& text, int severity) +{ + Q_UNUSED(severity); + qgcApp()->toolbox()->audioOutput()->say(text); +} + +void UAS::shutdownVehicle(void) +{ +#ifndef __mobile__ + stopHil(); + if (simulation) { + // wait for the simulator to exit + simulation->wait(); + simulation->disconnectSimulation(); + simulation->deleteLater(); + } +#endif + _vehicle = NULL; +} diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 9f5eb14505e57094d2e088016ec5d3fa2b9dfb90..b77a1b1ba2c2fd4510e708b04f59effb96beb50b 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -1,627 +1,613 @@ -/**************************************************************************** - * - * (c) 2009-2016 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -/** - * @file - * @brief Definition of Unmanned Aerial Vehicle object - * - * @author Lorenz Meier - * - */ - -#ifndef _UAS_H_ -#define _UAS_H_ - -#include "UASInterface.h" -#include -#include -#include "QGCMAVLink.h" -#include "Vehicle.h" -#include "FirmwarePluginManager.h" - -#ifndef __mobile__ -#include "FileManager.h" -#include "QGCHilLink.h" -#include "QGCFlightGearLink.h" -#include "QGCJSBSimLink.h" -#include "QGCXPlaneLink.h" -#endif - -Q_DECLARE_LOGGING_CATEGORY(UASLog) - -class Vehicle; - -/** - * @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() - * 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 - * no knowledge of the communication infrastructure is needed. - */ -class UAS : public UASInterface -{ - Q_OBJECT -public: - UAS(MAVLinkProtocol *protocol, Vehicle *vehicle, FirmwarePluginManager *firmwarePluginManager); - - float lipoFull; ///< 100% charged voltage - float lipoEmpty; ///< Discharged voltage - - /* MANAGEMENT */ - - /** @brief Get the unique system id */ - int getUASID() const; - /** @brief Get the components */ - QMap getComponents(); - - /** @brief The time interval the robot is switched on */ - quint64 getUptime() const; - - Q_PROPERTY(double latitude READ getLatitude WRITE setLatitude NOTIFY latitudeChanged) - Q_PROPERTY(double longitude READ getLongitude WRITE setLongitude NOTIFY longitudeChanged) - Q_PROPERTY(double satelliteCount READ getSatelliteCount WRITE setSatelliteCount NOTIFY satelliteCountChanged) - Q_PROPERTY(bool isGlobalPositionKnown READ globalPositionKnown) - Q_PROPERTY(double roll READ getRoll WRITE setRoll NOTIFY rollChanged) - Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged) - Q_PROPERTY(double yaw READ getYaw WRITE setYaw NOTIFY yawChanged) - Q_PROPERTY(double distToWaypoint READ getDistToWaypoint WRITE setDistToWaypoint NOTIFY distToWaypointChanged) - Q_PROPERTY(double airSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY airSpeedChanged) - Q_PROPERTY(double groundSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY groundSpeedChanged) - Q_PROPERTY(double bearingToWaypoint READ getBearingToWaypoint WRITE setBearingToWaypoint NOTIFY bearingToWaypointChanged) - Q_PROPERTY(double altitudeAMSL READ getAltitudeAMSL WRITE setAltitudeAMSL NOTIFY altitudeAMSLChanged) - Q_PROPERTY(double altitudeAMSLFT READ getAltitudeAMSLFT NOTIFY altitudeAMSLFTChanged) - Q_PROPERTY(double altitudeRelative READ getAltitudeRelative WRITE setAltitudeRelative NOTIFY altitudeRelativeChanged) - Q_PROPERTY(double satRawHDOP READ getSatRawHDOP NOTIFY satRawHDOPChanged) - Q_PROPERTY(double satRawVDOP READ getSatRawVDOP NOTIFY satRawVDOPChanged) - Q_PROPERTY(double satRawCOG READ getSatRawCOG NOTIFY satRawCOGChanged) - - /// Vehicle is about to go away - void shutdownVehicle(void); - - void setGroundSpeed(double val) - { - groundSpeed = val; - emit groundSpeedChanged(val, "groundSpeed"); - emit valueChanged(this->uasId, "groundSpeed", "m/s", QVariant(val), getUnixTime()); - } - double getGroundSpeed() const - { - return groundSpeed; - } - - void setAirSpeed(double val) - { - airSpeed = val; - emit airSpeedChanged(val, "airSpeed"); - emit valueChanged(this->uasId, "airSpeed", "m/s", QVariant(val), getUnixTime()); - } - - double getAirSpeed() const - { - return airSpeed; - } - - void setLocalX(double val) - { - localX = val; - emit localXChanged(val, "localX"); - emit valueChanged(this->uasId, "localX", "m", QVariant(val), getUnixTime()); - } - - double getLocalX() const - { - return localX; - } - - void setLocalY(double val) - { - localY = val; - emit localYChanged(val, "localY"); - emit valueChanged(this->uasId, "localY", "m", QVariant(val), getUnixTime()); - } - double getLocalY() const - { - return localY; - } - - void setLocalZ(double val) - { - localZ = val; - emit localZChanged(val, "localZ"); - emit valueChanged(this->uasId, "localZ", "m", QVariant(val), getUnixTime()); - } - double getLocalZ() const - { - return localZ; - } - - void setLatitude(double val) - { - latitude = val; - emit latitudeChanged(val, "latitude"); - emit valueChanged(this->uasId, "latitude", "deg", QVariant(val), getUnixTime()); - } - - double getLatitude() const - { - return latitude; - } - - void setLongitude(double val) - { - longitude = val; - emit longitudeChanged(val, "longitude"); - emit valueChanged(this->uasId, "longitude", "deg", QVariant(val), getUnixTime()); - } - - double getLongitude() const - { - return longitude; - } - - void setAltitudeAMSL(double val) - { - altitudeAMSL = val; - emit altitudeAMSLChanged(val, "altitudeAMSL"); - emit valueChanged(this->uasId, "altitudeAMSL", "m", QVariant(altitudeAMSL), getUnixTime()); - altitudeAMSLFT = 3.28084 * altitudeAMSL; - emit altitudeAMSLFTChanged(val, "altitudeAMSLFT"); - emit valueChanged(this->uasId, "altitudeAMSLFT", "m", QVariant(altitudeAMSLFT), getUnixTime()); - } - - double getAltitudeAMSL() const - { - return altitudeAMSL; - } - - double getAltitudeAMSLFT() const - { - return altitudeAMSLFT; - } - - void setAltitudeRelative(double val) - { - altitudeRelative = val; - emit altitudeRelativeChanged(val, "altitudeRelative"); - emit valueChanged(this->uasId, "altitudeRelative", "m", QVariant(val), getUnixTime()); - } - - double getAltitudeRelative() const - { - return altitudeRelative; - } - - double getSatRawHDOP() const - { - return satRawHDOP; - } - - double getSatRawVDOP() const - { - return satRawVDOP; - } - - double getSatRawCOG() const - { - return satRawCOG; - } - - void setSatelliteCount(double val) - { - satelliteCount = val; - emit satelliteCountChanged(val, "satelliteCount"); - emit valueChanged(this->uasId, "satelliteCount", "", QVariant(val), getUnixTime()); - } - - double getSatelliteCount() const - { - return satelliteCount; - } - - virtual bool globalPositionKnown() const - { - return isGlobalPositionKnown; - } - - void setDistToWaypoint(double val) - { - distToWaypoint = val; - emit distToWaypointChanged(val, "distToWaypoint"); - emit valueChanged(this->uasId, "distToWaypoint", "m", QVariant(val), getUnixTime()); - } - - double getDistToWaypoint() const - { - return distToWaypoint; - } - - void setBearingToWaypoint(double val) - { - bearingToWaypoint = val; - emit bearingToWaypointChanged(val, "bearingToWaypoint"); - emit valueChanged(this->uasId, "bearingToWaypoint", "deg", QVariant(val), getUnixTime()); - } - - double getBearingToWaypoint() const - { - return bearingToWaypoint; - } - - - void setRoll(double val) - { - roll = val; - emit rollChanged(val, "roll"); - } - - double getRoll() const - { - return roll; - } - - void setPitch(double val) - { - pitch = val; - emit pitchChanged(val, "pitch"); - } - - double getPitch() const - { - return pitch; - } - - void setYaw(double val) - { - yaw = val; - emit yawChanged(val, "yaw"); - } - - double getYaw() const - { - return yaw; - } - - // Setters for HIL noise variance - void setXaccVar(float var) - { - xacc_var = var; - } - - void setYaccVar(float var) - { - yacc_var = var; - } - - void setZaccVar(float var) - { - zacc_var = var; - } - - void setRollSpeedVar(float var) - { - rollspeed_var = var; - } - - void setPitchSpeedVar(float var) - { - pitchspeed_var = var; - } - - void setYawSpeedVar(float var) - { - pitchspeed_var = var; - } - - void setXmagVar(float var) - { - xmag_var = var; - } - - void setYmagVar(float var) - { - ymag_var = var; - } - - void setZmagVar(float var) - { - zmag_var = var; - } - - void setAbsPressureVar(float var) - { - abs_pressure_var = var; - } - - void setDiffPressureVar(float var) - { - diff_pressure_var = var; - } - - void setPressureAltVar(float var) - { - pressure_alt_var = var; - } - - void setTemperatureVar(float var) - { - temperature_var = var; - } - -#ifndef __mobile__ - friend class FileManager; -#endif - -protected: //COMMENTS FOR TEST UNIT - /// LINK ID AND STATUS - int uasId; ///< Unique system ID - QMap components;///< IDs and names of all detected onboard components - - QList unknownPackets; ///< Packet IDs which are unknown and have been received - 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 - - /// BASIC UAS TYPE, NAME AND STATE - int status; ///< The current status of the MAV - - /// TIMEKEEPING - quint64 startTime; ///< The time the UAS was switched on - quint64 onboardTimeOffset; - - /// MANUAL CONTROL - bool controlRollManual; ///< status flag, true if roll is controlled manually - bool controlPitchManual; ///< status flag, true if pitch is controlled manually - bool controlYawManual; ///< status flag, true if yaw is controlled manually - bool controlThrustManual; ///< status flag, true if thrust is controlled manually - - double manualRollAngle; ///< Roll angle set by human pilot (radians) - double manualPitchAngle; ///< Pitch angle set by human pilot (radians) - double manualYawAngle; ///< Yaw angle set by human pilot (radians) - double manualThrust; ///< Thrust set by human pilot (radians) - - /// POSITION - bool isGlobalPositionKnown; ///< If the global position has been received for this MAV - - double localX; - double localY; - double localZ; - - double latitude; ///< Global latitude as estimated by position estimator - double longitude; ///< Global longitude as estimated by position estimator - double altitudeAMSL; ///< Global altitude as estimated by position estimator, AMSL - double altitudeAMSLFT; ///< Global altitude as estimated by position estimator, AMSL - double altitudeRelative; ///< Altitude above home as estimated by position estimator - - double satRawHDOP; - double satRawVDOP; - double satRawCOG; - - double satelliteCount; ///< Number of satellites visible to raw GPS - 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 longitude_gps; ///< Global longitude as estimated by raw GPS - double altitude_gps; ///< Global altitude as estimated by raw GPS - double speedX; ///< True speed in X axis - double speedY; ///< True speed in Y axis - double speedZ; ///< True speed in Z axis - - /// WAYPOINT NAVIGATION - double distToWaypoint; ///< Distance to next waypoint - double airSpeed; ///< Airspeed - double groundSpeed; ///< Groundspeed - double bearingToWaypoint; ///< Bearing to next waypoint -#ifndef __mobile__ - FileManager fileManager; -#endif - - /// ATTITUDE - bool attitudeKnown; ///< True if attitude was received, false else - 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 - double roll; - double pitch; - double yaw; - - // dongfang: This looks like a candidate for being moved off to a separate class. - /// IMAGING - int imageSize; ///< Image size being transmitted (bytes) - int imagePackets; ///< Number of data packets being sent for this image - int imagePacketsArrived; ///< Number of data packets received - int imagePayload; ///< Payload size per transmitted packet (bytes). Standard is 254, and decreases when image resolution increases. - int imageQuality; ///< Quality of the transmitted image (percentage) - int imageType; ///< Type of the transmitted image (BMP, PNG, JPEG, RAW 8 bit, RAW 32 bit) - int imageWidth; ///< Width of the image stream - int imageHeight; ///< Width of the image stream - QByteArray imageRecBuffer; ///< Buffer for the incoming bytestream - QImage image; ///< Image data of last completely transmitted image - quint64 imageStart; - bool blockHomePositionChanges; ///< Block changes to the home position - bool receivedMode; ///< True if mode was retrieved from current conenction to UAS - - /// SIMULATION NOISE - float xacc_var; ///< variance of x acclerometer noise for HIL sim (mg) - float yacc_var; ///< variance of y acclerometer noise for HIL sim (mg) - float zacc_var; ///< variance of z acclerometer noise for HIL sim (mg) - float rollspeed_var; ///< variance of x gyroscope noise for HIL sim (rad/s) - float pitchspeed_var; ///< variance of y gyroscope noise for HIL sim (rad/s) - float yawspeed_var; ///< variance of z gyroscope noise for HIL sim (rad/s) - float xmag_var; ///< variance of x magnatometer noise for HIL sim (???) - float ymag_var; ///< variance of y magnatometer noise for HIL sim (???) - float zmag_var; ///< variance of z magnatometer noise for HIL sim (???) - 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) - float pressure_alt_var; ///< variance of altitude pressure noise for HIL sim (hPa) - float temperature_var; ///< variance of temperature noise for HIL sim (C) - - /// SIMULATION -#ifndef __mobile__ - QGCHilLink *simulation; ///< Hardware in the loop simulation link -#endif - -public: - /** @brief Get the human-readable status message for this code */ - void getStatusForCode(int statusCode, QString &uasState, QString &stateDescription); - -#ifndef __mobile__ - virtual FileManager *getFileManager() { return &fileManager; } -#endif - - /** @brief Get the HIL simulation */ -#ifndef __mobile__ - QGCHilLink *getHILSimulation() const - { - return simulation; - } -#endif - - QImage getImage(); - void requestImage(); - -public slots: - /** @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 Order the robot to pair its receiver **/ - void pairRX(int rxType, int rxSubType); - - /** @brief Enable / disable HIL */ -#ifndef __mobile__ - void enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject *configuration); - void enableHilJSBSim(bool enable, QString options); - void enableHilXPlane(bool enable); - - /** @brief Send the full HIL state to the MAV */ - void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate, - 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); - - void sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate, - 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 RAW sensors for sensor HIL */ - void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, - float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed); - - /** @brief Send Optical Flow sensor message for HIL, (arguments and units accoding to mavlink documentation*/ - void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, - float flow_comp_m_y, quint8 quality, float ground_distance); - - float addZeroMeanNoise(float truth_meas, float noise_var); - - /** - * @param time_us - * @param lat - * @param lon - * @param alt - * @param fix_type - * @param eph - * @param epv - * @param vel - * @param cog course over ground, in radians, -pi..pi - * @param satellites - */ - 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); - - - /** @brief Places the UAV in Hardware-in-the-Loop simulation status **/ - void startHil(); - - /** @brief Stops the UAV's Hardware-in-the-Loop simulation status **/ - void stopHil(); -#endif - - /** @brief Set the values for the manual control of the vehicle */ - void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode); - - /** @brief Set the values for the 6dof manual control of the vehicle */ -#ifndef __mobile__ - void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw); -#endif - - /** @brief Receive a message from one of the communication links. */ - virtual void receiveMessage(mavlink_message_t message); - - void startCalibration(StartCalibrationType calType); - void stopCalibration(void); - - void startBusConfig(StartBusConfigType calType); - void stopBusConfig(void); - - /** @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); - - /** @brief Send command to disable all bindings/maps between RC and parameters */ - void unsetRCToParameterMap(); -signals: - void loadChanged(UASInterface *uas, double load); - void imageStarted(quint64 timestamp); - /** @brief A new camera image has arrived */ - void imageReady(UASInterface *uas); - /** @brief HIL controls have changed */ - void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode); - /** @brief HIL actuator controls (replaces HIL controls) */ - void hilActuatorControlsChanged(quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode); - - void localXChanged(double val, QString name); - void localYChanged(double val, QString name); - void localZChanged(double val, QString name); - void longitudeChanged(double val, QString name); - void latitudeChanged(double val, QString name); - void altitudeAMSLChanged(double val, QString name); - void altitudeAMSLFTChanged(double val, QString name); - void altitudeRelativeChanged(double val, QString name); - - void satRawHDOPChanged(double value); - void satRawVDOPChanged(double value); - void satRawCOGChanged(double value); - - void rollChanged(double val, QString name); - void pitchChanged(double val, QString name); - void yawChanged(double val, QString name); - void satelliteCountChanged(double val, QString name); - void distToWaypointChanged(double val, QString name); - void groundSpeedChanged(double val, QString name); - void airSpeedChanged(double val, QString name); - void bearingToWaypointChanged(double val, QString name); -protected: - /** @brief Get the UNIX timestamp in milliseconds, enter microseconds */ - quint64 getUnixTime(quint64 time = 0); - /** @brief Get the UNIX timestamp in milliseconds, enter milliseconds */ - quint64 getUnixTimeFromMs(quint64 time); - /** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */ - quint64 getUnixReferenceTime(quint64 time); - - virtual void processParamValueMsg(mavlink_message_t &msg, const QString ¶mName, const mavlink_param_value_t &rawValue, mavlink_param_union_t ¶mValue); - - int componentID[256]; - bool componentMulti[256]; - bool connectionLost; ///< Flag indicates a timed out connection - quint64 connectionLossTime; ///< Time the connection was interrupted - quint64 lastVoltageWarning; ///< Time at which the last voltage warning occurred - 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 - 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_ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +/** + * @file + * @brief Definition of Unmanned Aerial Vehicle object + * + * @author Lorenz Meier + * + */ + +#ifndef _UAS_H_ +#define _UAS_H_ + +#include "UASInterface.h" +#include +#include +#include "QGCMAVLink.h" +#include "Vehicle.h" +#include "FirmwarePluginManager.h" + +#ifndef __mobile__ +#include "FileManager.h" +#include "QGCHilLink.h" +#include "QGCFlightGearLink.h" +#include "QGCJSBSimLink.h" +#include "QGCXPlaneLink.h" +#endif + +Q_DECLARE_LOGGING_CATEGORY(UASLog) + +class Vehicle; + +/** + * @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() + * 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 + * no knowledge of the communication infrastructure is needed. + */ +class UAS : public UASInterface +{ + Q_OBJECT +public: + UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * firmwarePluginManager); + + float lipoFull; ///< 100% charged voltage + float lipoEmpty; ///< Discharged voltage + + /* MANAGEMENT */ + + /** @brief Get the unique system id */ + int getUASID() const; + /** @brief Get the components */ + QMap getComponents(); + + /** @brief The time interval the robot is switched on */ + quint64 getUptime() const; + + Q_PROPERTY(double latitude READ getLatitude WRITE setLatitude NOTIFY latitudeChanged) + Q_PROPERTY(double longitude READ getLongitude WRITE setLongitude NOTIFY longitudeChanged) + Q_PROPERTY(double satelliteCount READ getSatelliteCount WRITE setSatelliteCount NOTIFY satelliteCountChanged) + Q_PROPERTY(bool isGlobalPositionKnown READ globalPositionKnown) + Q_PROPERTY(double roll READ getRoll WRITE setRoll NOTIFY rollChanged) + Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged) + Q_PROPERTY(double yaw READ getYaw WRITE setYaw NOTIFY yawChanged) + Q_PROPERTY(double distToWaypoint READ getDistToWaypoint WRITE setDistToWaypoint NOTIFY distToWaypointChanged) + Q_PROPERTY(double airSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY airSpeedChanged) + Q_PROPERTY(double groundSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY groundSpeedChanged) + Q_PROPERTY(double bearingToWaypoint READ getBearingToWaypoint WRITE setBearingToWaypoint NOTIFY bearingToWaypointChanged) + Q_PROPERTY(double altitudeAMSL READ getAltitudeAMSL WRITE setAltitudeAMSL NOTIFY altitudeAMSLChanged) + Q_PROPERTY(double altitudeAMSLFT READ getAltitudeAMSLFT NOTIFY altitudeAMSLFTChanged) + Q_PROPERTY(double altitudeRelative READ getAltitudeRelative WRITE setAltitudeRelative NOTIFY altitudeRelativeChanged) + Q_PROPERTY(double satRawHDOP READ getSatRawHDOP NOTIFY satRawHDOPChanged) + Q_PROPERTY(double satRawVDOP READ getSatRawVDOP NOTIFY satRawVDOPChanged) + Q_PROPERTY(double satRawCOG READ getSatRawCOG NOTIFY satRawCOGChanged) + + /// Vehicle is about to go away + void shutdownVehicle(void); + + void setGroundSpeed(double val) + { + groundSpeed = val; + emit groundSpeedChanged(val,"groundSpeed"); + emit valueChanged(this->uasId,"groundSpeed","m/s",QVariant(val),getUnixTime()); + } + double getGroundSpeed() const + { + return groundSpeed; + } + + void setAirSpeed(double val) + { + airSpeed = val; + emit airSpeedChanged(val,"airSpeed"); + emit valueChanged(this->uasId,"airSpeed","m/s",QVariant(val),getUnixTime()); + } + + double getAirSpeed() const + { + return airSpeed; + } + + void setLocalX(double val) + { + localX = val; + emit localXChanged(val,"localX"); + emit valueChanged(this->uasId,"localX","m",QVariant(val),getUnixTime()); + } + + double getLocalX() const + { + return localX; + } + + void setLocalY(double val) + { + localY = val; + emit localYChanged(val,"localY"); + emit valueChanged(this->uasId,"localY","m",QVariant(val),getUnixTime()); + } + double getLocalY() const + { + return localY; + } + + void setLocalZ(double val) + { + localZ = val; + emit localZChanged(val,"localZ"); + emit valueChanged(this->uasId,"localZ","m",QVariant(val),getUnixTime()); + } + double getLocalZ() const + { + return localZ; + } + + void setLatitude(double val) + { + latitude = val; + emit latitudeChanged(val,"latitude"); + emit valueChanged(this->uasId,"latitude","deg",QVariant(val),getUnixTime()); + } + + double getLatitude() const + { + return latitude; + } + + void setLongitude(double val) + { + longitude = val; + emit longitudeChanged(val,"longitude"); + emit valueChanged(this->uasId,"longitude","deg",QVariant(val),getUnixTime()); + } + + double getLongitude() const + { + return longitude; + } + + void setAltitudeAMSL(double val) + { + altitudeAMSL = val; + emit altitudeAMSLChanged(val, "altitudeAMSL"); + emit valueChanged(this->uasId,"altitudeAMSL","m",QVariant(altitudeAMSL),getUnixTime()); + altitudeAMSLFT = 3.28084 * altitudeAMSL; + emit altitudeAMSLFTChanged(val, "altitudeAMSLFT"); + emit valueChanged(this->uasId,"altitudeAMSLFT","m",QVariant(altitudeAMSLFT),getUnixTime()); + } + + double getAltitudeAMSL() const + { + return altitudeAMSL; + } + + double getAltitudeAMSLFT() const + { + return altitudeAMSLFT; + } + + void setAltitudeRelative(double val) + { + altitudeRelative = val; + emit altitudeRelativeChanged(val, "altitudeRelative"); + emit valueChanged(this->uasId,"altitudeRelative","m",QVariant(val),getUnixTime()); + } + + double getAltitudeRelative() const + { + return altitudeRelative; + } + + double getSatRawHDOP() const + { + return satRawHDOP; + } + + double getSatRawVDOP() const + { + return satRawVDOP; + } + + double getSatRawCOG() const + { + return satRawCOG; + } + + void setSatelliteCount(double val) + { + satelliteCount = val; + emit satelliteCountChanged(val,"satelliteCount"); + emit valueChanged(this->uasId,"satelliteCount","",QVariant(val),getUnixTime()); + } + + double getSatelliteCount() const + { + return satelliteCount; + } + + virtual bool globalPositionKnown() const + { + return isGlobalPositionKnown; + } + + void setDistToWaypoint(double val) + { + distToWaypoint = val; + emit distToWaypointChanged(val,"distToWaypoint"); + emit valueChanged(this->uasId,"distToWaypoint","m",QVariant(val),getUnixTime()); + } + + double getDistToWaypoint() const + { + return distToWaypoint; + } + + void setBearingToWaypoint(double val) + { + bearingToWaypoint = val; + emit bearingToWaypointChanged(val,"bearingToWaypoint"); + emit valueChanged(this->uasId,"bearingToWaypoint","deg",QVariant(val),getUnixTime()); + } + + double getBearingToWaypoint() const + { + return bearingToWaypoint; + } + + + void setRoll(double val) + { + roll = val; + emit rollChanged(val,"roll"); + } + + double getRoll() const + { + return roll; + } + + void setPitch(double val) + { + pitch = val; + emit pitchChanged(val,"pitch"); + } + + double getPitch() const + { + return pitch; + } + + void setYaw(double val) + { + yaw = val; + emit yawChanged(val,"yaw"); + } + + double getYaw() const + { + return yaw; + } + + // Setters for HIL noise variance + void setXaccVar(float var){ + xacc_var = var; + } + + void setYaccVar(float var){ + yacc_var = var; + } + + void setZaccVar(float var){ + zacc_var = var; + } + + void setRollSpeedVar(float var){ + rollspeed_var = var; + } + + void setPitchSpeedVar(float var){ + pitchspeed_var = var; + } + + void setYawSpeedVar(float var){ + pitchspeed_var = var; + } + + void setXmagVar(float var){ + xmag_var = var; + } + + void setYmagVar(float var){ + ymag_var = var; + } + + void setZmagVar(float var){ + zmag_var = var; + } + + void setAbsPressureVar(float var){ + abs_pressure_var = var; + } + + void setDiffPressureVar(float var){ + diff_pressure_var = var; + } + + void setPressureAltVar(float var){ + pressure_alt_var = var; + } + + void setTemperatureVar(float var){ + temperature_var = var; + } + +#ifndef __mobile__ + friend class FileManager; +#endif + +protected: //COMMENTS FOR TEST UNIT + /// LINK ID AND STATUS + int uasId; ///< Unique system ID + QMap components;///< IDs and names of all detected onboard components + + QList unknownPackets; ///< Packet IDs which are unknown and have been received + 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 + + /// BASIC UAS TYPE, NAME AND STATE + int status; ///< The current status of the MAV + + /// TIMEKEEPING + quint64 startTime; ///< The time the UAS was switched on + quint64 onboardTimeOffset; + + /// MANUAL CONTROL + bool controlRollManual; ///< status flag, true if roll is controlled manually + bool controlPitchManual; ///< status flag, true if pitch is controlled manually + bool controlYawManual; ///< status flag, true if yaw is controlled manually + bool controlThrustManual; ///< status flag, true if thrust is controlled manually + + double manualRollAngle; ///< Roll angle set by human pilot (radians) + double manualPitchAngle; ///< Pitch angle set by human pilot (radians) + double manualYawAngle; ///< Yaw angle set by human pilot (radians) + double manualThrust; ///< Thrust set by human pilot (radians) + + /// POSITION + bool isGlobalPositionKnown; ///< If the global position has been received for this MAV + + double localX; + double localY; + double localZ; + + double latitude; ///< Global latitude as estimated by position estimator + double longitude; ///< Global longitude as estimated by position estimator + double altitudeAMSL; ///< Global altitude as estimated by position estimator, AMSL + double altitudeAMSLFT; ///< Global altitude as estimated by position estimator, AMSL + double altitudeRelative; ///< Altitude above home as estimated by position estimator + + double satRawHDOP; + double satRawVDOP; + double satRawCOG; + + double satelliteCount; ///< Number of satellites visible to raw GPS + 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 longitude_gps; ///< Global longitude as estimated by raw GPS + double altitude_gps; ///< Global altitude as estimated by raw GPS + double speedX; ///< True speed in X axis + double speedY; ///< True speed in Y axis + double speedZ; ///< True speed in Z axis + + /// WAYPOINT NAVIGATION + double distToWaypoint; ///< Distance to next waypoint + double airSpeed; ///< Airspeed + double groundSpeed; ///< Groundspeed + double bearingToWaypoint; ///< Bearing to next waypoint +#ifndef __mobile__ + FileManager fileManager; +#endif + + /// ATTITUDE + bool attitudeKnown; ///< True if attitude was received, false else + 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 + double roll; + double pitch; + double yaw; + + // dongfang: This looks like a candidate for being moved off to a separate class. + /// IMAGING + int imageSize; ///< Image size being transmitted (bytes) + int imagePackets; ///< Number of data packets being sent for this image + int imagePacketsArrived; ///< Number of data packets received + int imagePayload; ///< Payload size per transmitted packet (bytes). Standard is 254, and decreases when image resolution increases. + int imageQuality; ///< Quality of the transmitted image (percentage) + int imageType; ///< Type of the transmitted image (BMP, PNG, JPEG, RAW 8 bit, RAW 32 bit) + int imageWidth; ///< Width of the image stream + int imageHeight; ///< Width of the image stream + QByteArray imageRecBuffer; ///< Buffer for the incoming bytestream + QImage image; ///< Image data of last completely transmitted image + quint64 imageStart; + bool blockHomePositionChanges; ///< Block changes to the home position + bool receivedMode; ///< True if mode was retrieved from current conenction to UAS + + /// SIMULATION NOISE + float xacc_var; ///< variance of x acclerometer noise for HIL sim (mg) + float yacc_var; ///< variance of y acclerometer noise for HIL sim (mg) + float zacc_var; ///< variance of z acclerometer noise for HIL sim (mg) + float rollspeed_var; ///< variance of x gyroscope noise for HIL sim (rad/s) + float pitchspeed_var; ///< variance of y gyroscope noise for HIL sim (rad/s) + float yawspeed_var; ///< variance of z gyroscope noise for HIL sim (rad/s) + float xmag_var; ///< variance of x magnatometer noise for HIL sim (???) + float ymag_var; ///< variance of y magnatometer noise for HIL sim (???) + float zmag_var; ///< variance of z magnatometer noise for HIL sim (???) + 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) + float pressure_alt_var; ///< variance of altitude pressure noise for HIL sim (hPa) + float temperature_var; ///< variance of temperature noise for HIL sim (C) + + /// SIMULATION +#ifndef __mobile__ + QGCHilLink* simulation; ///< Hardware in the loop simulation link +#endif + +public: + /** @brief Get the human-readable status message for this code */ + void getStatusForCode(int statusCode, QString& uasState, QString& stateDescription); + +#ifndef __mobile__ + virtual FileManager* getFileManager() { return &fileManager; } +#endif + + /** @brief Get the HIL simulation */ +#ifndef __mobile__ + QGCHilLink* getHILSimulation() const { + return simulation; + } +#endif + + QImage getImage(); + void requestImage(); + +public slots: + /** @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 Order the robot to pair its receiver **/ + void pairRX(int rxType, int rxSubType); + + /** @brief Enable / disable HIL */ +#ifndef __mobile__ + void enableHilFlightGear(bool enable, QString options, bool sensorHil, QObject * configuration); + void enableHilJSBSim(bool enable, QString options); + void enableHilXPlane(bool enable); + + /** @brief Send the full HIL state to the MAV */ + void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate, + 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); + + void sendHilGroundTruth(quint64 time_us, float roll, float pitch, float yaw, float rollRotationRate, + 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 RAW sensors for sensor HIL */ + void sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, float rollspeed, float pitchspeed, float yawspeed, + float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, quint32 fields_changed); + + /** @brief Send Optical Flow sensor message for HIL, (arguments and units accoding to mavlink documentation*/ + void sendHilOpticalFlow(quint64 time_us, qint16 flow_x, qint16 flow_y, float flow_comp_m_x, + float flow_comp_m_y, quint8 quality, float ground_distance); + + float addZeroMeanNoise(float truth_meas, float noise_var); + + /** + * @param time_us + * @param lat + * @param lon + * @param alt + * @param fix_type + * @param eph + * @param epv + * @param vel + * @param cog course over ground, in radians, -pi..pi + * @param satellites + */ + 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); + + + /** @brief Places the UAV in Hardware-in-the-Loop simulation status **/ + void startHil(); + + /** @brief Stops the UAV's Hardware-in-the-Loop simulation status **/ + void stopHil(); +#endif + + /** @brief Set the values for the manual control of the vehicle */ + void setExternalControlSetpoint(float roll, float pitch, float yaw, float thrust, quint16 buttons, int joystickMode); + + /** @brief Set the values for the 6dof manual control of the vehicle */ +#ifndef __mobile__ + void setManual6DOFControlCommands(double x, double y, double z, double roll, double pitch, double yaw); +#endif + + /** @brief Receive a message from one of the communication links. */ + virtual void receiveMessage(mavlink_message_t message); + + void startCalibration(StartCalibrationType calType); + void stopCalibration(void); + + void startBusConfig(StartBusConfigType calType); + void stopBusConfig(void); + + /** @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); + + /** @brief Send command to disable all bindings/maps between RC and parameters */ + void unsetRCToParameterMap(); +signals: + void loadChanged(UASInterface* uas, double load); + void imageStarted(quint64 timestamp); + /** @brief A new camera image has arrived */ + void imageReady(UASInterface* uas); + /** @brief HIL controls have changed */ + void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode); + /** @brief HIL actuator controls (replaces HIL controls) */ + void hilActuatorControlsChanged(quint64 time, quint64 flags, float ctl_0, float ctl_1, float ctl_2, float ctl_3, float ctl_4, float ctl_5, float ctl_6, float ctl_7, float ctl_8, float ctl_9, float ctl_10, float ctl_11, float ctl_12, float ctl_13, float ctl_14, float ctl_15, quint8 mode); + + void localXChanged(double val,QString name); + void localYChanged(double val,QString name); + void localZChanged(double val,QString name); + void longitudeChanged(double val,QString name); + void latitudeChanged(double val,QString name); + void altitudeAMSLChanged(double val,QString name); + void altitudeAMSLFTChanged(double val,QString name); + void altitudeRelativeChanged(double val,QString name); + + void satRawHDOPChanged (double value); + void satRawVDOPChanged (double value); + void satRawCOGChanged (double value); + + void rollChanged(double val,QString name); + void pitchChanged(double val,QString name); + void yawChanged(double val,QString name); + void satelliteCountChanged(double val,QString name); + void distToWaypointChanged(double val,QString name); + void groundSpeedChanged(double val, QString name); + void airSpeedChanged(double val, QString name); + void bearingToWaypointChanged(double val,QString name); +protected: + /** @brief Get the UNIX timestamp in milliseconds, enter microseconds */ + quint64 getUnixTime(quint64 time=0); + /** @brief Get the UNIX timestamp in milliseconds, enter milliseconds */ + quint64 getUnixTimeFromMs(quint64 time); + /** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */ + quint64 getUnixReferenceTime(quint64 time); + + virtual void processParamValueMsg(mavlink_message_t& msg, const QString& paramName,const mavlink_param_value_t& rawValue, mavlink_param_union_t& paramValue); + + int componentID[256]; + bool componentMulti[256]; + bool connectionLost; ///< Flag indicates a timed out connection + quint64 connectionLossTime; ///< Time the connection was interrupted + quint64 lastVoltageWarning; ///< Time at which the last voltage warning occurred + 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 + 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_ diff --git a/src/ui/QGCHilXPlaneConfiguration.cc b/src/ui/QGCHilXPlaneConfiguration.cc index c85a237dccfe25f47991b979ae518410b0f758da..bc0bd438566fe239d28621dc4c081dbb2abd3c63 100644 --- a/src/ui/QGCHilXPlaneConfiguration.cc +++ b/src/ui/QGCHilXPlaneConfiguration.cc @@ -3,7 +3,7 @@ #include "QGCXPlaneLink.h" #include "QGCHilConfiguration.h" -QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink *link, QGCHilConfiguration *parent) : +QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink* link, QGCHilConfiguration *parent) : QWidget(parent), ui(new Ui::QGCHilXPlaneConfiguration) { @@ -12,7 +12,7 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink *link, QGCHilCon connect(ui->startButton, &QPushButton::clicked, this, &QGCHilXPlaneConfiguration::toggleSimulation); - connect(ui->hostComboBox, static_cast(&QComboBox::activated), + connect(ui->hostComboBox, static_cast(&QComboBox::activated), link, &QGCHilLink::setRemoteHost); connect(link, &QGCHilLink::remoteChanged, ui->hostComboBox, &QComboBox::setEditText); @@ -23,7 +23,7 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink *link, QGCHilCon ui->startButton->setText(tr("Connect")); - QGCXPlaneLink *xplane = dynamic_cast(link); + QGCXPlaneLink* xplane = dynamic_cast(link); if (xplane) { @@ -58,20 +58,17 @@ void QGCHilXPlaneConfiguration::setVersion(int version) void QGCHilXPlaneConfiguration::toggleSimulation(bool connect) { - if (!link) - { + if (!link) { return; } Q_UNUSED(connect); - if (!link->isConnected()) { link->setRemoteHost(ui->hostComboBox->currentText()); link->connectSimulation(); ui->startButton->setText(tr("Disconnect")); } - else { link->disconnectSimulation();