diff --git a/src/comm/QGCFlightGearLink.cc b/src/comm/QGCFlightGearLink.cc index 12f8a42..83e2088 100644 --- a/src/comm/QGCFlightGearLink.cc +++ b/src/comm/QGCFlightGearLink.cc @@ -37,15 +37,16 @@ This file is part of the QGROUNDCONTROL project #include "QGC.h" #include -QGCFlightGearLink::QGCFlightGearLink(QHostAddress host, quint16 port) +QGCFlightGearLink::QGCFlightGearLink(QString remoteHost, QHostAddress host, quint16 port) { this->host = host; this->port = port; this->connectState = false; + this->currentPort = 49000; // Set unique ID and add link to the list of links this->name = tr("FlightGear Link (port:%1)").arg(port); - setRemoteHost(QString("127.0.0.1:%1").arg(port)); + setRemoteHost(remoteHost); connect(&refreshTimer, SIGNAL(timeout()), this, SLOT(sendUAVUpdate())); refreshTimer.start(20); // 50 Hz UAV -> Simulation update rate } @@ -122,7 +123,17 @@ void QGCFlightGearLink::updateGlobalPosition(quint64 time, double lat, double lo void QGCFlightGearLink::sendUAVUpdate() { - QString state(""); + // 37.613548,-122.357246,-9999.000000,0.000000,0.424000,297.899994,0.000000\n + // magnetos,aileron,elevator,rudder,throttle\n + + float magnetos = 3.0f; + float aileron = 0.0f; + float elevator = 0.0f; + float rudder = 0.0f; + float throttle = 90.0f; + + QString state("%1,%2,%3,%4,%5\n"); + state = state.arg(magnetos).arg(aileron).arg(elevator).arg(rudder).arg(throttle); writeBytes(state.toAscii().constData(), state.length()); } diff --git a/src/comm/QGCFlightGearLink.h b/src/comm/QGCFlightGearLink.h index 49aaa02..8b23a31 100644 --- a/src/comm/QGCFlightGearLink.h +++ b/src/comm/QGCFlightGearLink.h @@ -46,7 +46,7 @@ class QGCFlightGearLink : public QThread //Q_INTERFACES(QGCFlightGearLinkInterface:LinkInterface) public: - QGCFlightGearLink(QHostAddress host = QHostAddress::Any, quint16 port = 49005); + QGCFlightGearLink(QString remoteHost=QString("127.0.0.1:49000"), QHostAddress host = QHostAddress::Any, quint16 port = 49005); ~QGCFlightGearLink(); bool isConnected(); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 1270ff1..f2d581e 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -74,7 +74,9 @@ statusTimeout(new QTimer(this)), paramsOnceRequested(false), airframe(0), attitudeKnown(false), -paramManager(NULL) +paramManager(NULL), +attitudeStamped(true), +lastAttitude(0) { color = UASInterface::getNextColor(); setBattery(LIPOLY, 3); @@ -187,7 +189,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq; - if (message.sysid == uasId) + // 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; @@ -457,7 +462,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_attitude_t attitude; mavlink_msg_attitude_decode(&message, &attitude); - quint64 time = getUnixTime(attitude.usec); + quint64 time = getUnixReferenceTime(attitude.usec); + lastAttitude = time; roll = QGC::limitAngleToPMPIf(attitude.roll); pitch = QGC::limitAngleToPMPIf(attitude.pitch); yaw = QGC::limitAngleToPMPIf(attitude.yaw); @@ -1252,8 +1258,65 @@ void UAS::startPressureCalibration() sendMessage(msg); } +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::getUnixTime(quint64 time) { + if (attitudeStamped) + { + return lastAttitude; + } if (time == 0) { // qDebug() << "XNEW time:" <heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name())); QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; border: 2px solid %1; background-color: %2; }").arg(borderColor, warnColor.name()); m_ui->uasViewFrame->setStyleSheet(style); + + refreshTimer->setInterval(errorUpdateInterval); } iconIsRed = !iconIsRed; } else { @@ -609,10 +611,11 @@ void UASView::refresh() { // Fade heartbeat icon // Make color darker - heartbeatColor = heartbeatColor.darker(150); + heartbeatColor = heartbeatColor.darker(210); //m_ui->heartbeatIcon->setAutoFillBackground(true); m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name())); + refreshTimer->setInterval(updateInterval); } } //setUpdatesEnabled(true); diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h index c29edaf..5e83232 100644 --- a/src/ui/uas/UASView.h +++ b/src/ui/uas/UASView.h @@ -122,7 +122,8 @@ protected: QAction* selectAction; QAction* selectAirframeAction; QAction* setBatterySpecsAction; - static const int updateInterval = 700; + static const int updateInterval = 800; + static const int errorUpdateInterval = 200; bool lowPowerModeEnabled; ///< Low power mode reduces update rates