diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 0db01398ca72b69c6ce14a2587d9e1c2eafd1383..0f0a8764565f67af4c5cffaeaf12deefd78effaa 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -74,7 +74,7 @@ void LinkManager::add(LinkInterface* link) if (!links.contains(link)) { if(!link) return; - connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*))); + connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeObj(QObject*))); links.append(link); emit newLink(link); } @@ -147,7 +147,7 @@ bool LinkManager::disconnectLink(LinkInterface* link) return link->disconnect(); } -void LinkManager::removeLink(QObject* link) +void LinkManager::removeObj(QObject* link) { LinkInterface* linkInterface = dynamic_cast(link); if (linkInterface) @@ -173,6 +173,10 @@ bool LinkManager::removeLink(LinkInterface* link) { protocolLinks.remove(proto, link); } + + // Emit removal of link + emit linkRemoved(link); + return true; } return false; diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 178b363faee75994b3618f4abf28a1e646296465..b960ebb7d23bf1e9eb76dce1be2d1f4bc14e5228 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -72,7 +72,7 @@ public slots: void add(LinkInterface* link); void addProtocol(LinkInterface* link, ProtocolInterface* protocol); - void removeLink(QObject* link); + void removeObj(QObject* obj); bool removeLink(LinkInterface* link); bool connectAll(); @@ -91,6 +91,7 @@ private: signals: void newLink(LinkInterface* link); + void linkRemoved(LinkInterface* link); }; diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index baa1d80ed11d6cfd065eb6bdd5b43733f2ae7b19..f94f4021aa2f77b7e1762a8570f4fb87e68f0fab 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -107,7 +107,6 @@ MAVLinkSimulationLink::MAVLinkSimulationLink(QString readFile, QString writeFile maxTimeNoise = 0; this->id = getNextLinkId(); LinkManager::instance()->add(this); - QObject::connect(this, SIGNAL(destroyed(QObject*)), LinkManager::instance(), SLOT(removeLink(QObject*))); } MAVLinkSimulationLink::~MAVLinkSimulationLink() diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index bcc64ac982dd4e953dd21a414199ca506ad7f417..1eed69c9ae5106b7a4ecd6488da2f042ad414ef6 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -791,24 +791,19 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) positionLock = true; isGlobalPositionKnown = true; - // Check for NaN - int alt = pos.alt; - if (!isnan(alt) && !isinf(alt)) - { - alt = 0; - emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE"); - } // Smaller than threshold and not NaN float vel = pos.vel/100.0f; - if (!globalEstimatorActive && (vel < 1000000) && !isnan(vel) && !isinf(vel)) - { - emit speedChanged(this, vel, 0.0, 0.0, time); - } - else - { - emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel)); + if (!globalEstimatorActive) { + if ((vel < 1000000) && !isnan(vel) && !isinf(vel)) + { + emit speedChanged(this, vel, 0.0, 0.0, time); + } + else + { + emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel)); + } } } } @@ -2737,11 +2732,23 @@ void UAS::sendHilState(uint64_t time_us, float roll, float pitch, float yaw, flo { if (this->mode & MAV_MODE_FLAG_HIL_ENABLED) { - mavlink_message_t msg; - mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, - time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, - lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, xacc*1000, yacc*1000, zacc*1000); - sendMessage(msg); + if (sensorHil) { + // Emit attitude for cross-check + emit attitudeChanged(this, 201, roll, pitch, yaw, time_us/1000); + emit valueChanged(uasId, "roll sim", "rad", roll, time_us/1000); + emit valueChanged(uasId, "pitch sim", "rad", pitch, time_us/1000); + emit valueChanged(uasId, "yaw sim", "rad", yaw, time_us/1000); + + emit valueChanged(uasId, "roll rate sim", "rad/s", rollspeed, time_us/1000); + emit valueChanged(uasId, "pitch rate sim", "rad/s", pitchspeed, time_us/1000); + emit valueChanged(uasId, "yaw rate sim", "rad/s", yawspeed, time_us/1000); + } else { + mavlink_message_t msg; + mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, + time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, + lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, xacc*1000, yacc*1000, zacc*1000); + sendMessage(msg); + } } else { @@ -2764,6 +2771,7 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl xmag, ymag, zmag, abs_pressure, diff_pressure, pressure_alt, temperature, fields_changed); sendMessage(msg); + sensorHil = true; } else { @@ -2802,6 +2810,7 @@ void UAS::startHil() { if (hilEnabled) return; hilEnabled = true; + sensorHil = false; mavlink_message_t msg; mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode | MAV_MODE_FLAG_HIL_ENABLED, navMode); sendMessage(msg); @@ -2819,6 +2828,7 @@ void UAS::stopHil() mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), mode & !MAV_MODE_FLAG_HIL_ENABLED, navMode); sendMessage(msg); hilEnabled = false; + sensorHil = false; } void UAS::shutdown() diff --git a/src/uas/UAS.h b/src/uas/UAS.h index de2d067f3800150efc518f493dd6cb3fe62b5b11..e4dfd783e4a61f8d5fd83d845163b6d473b2d86a 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -703,6 +703,7 @@ protected: 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; ///< Set to true if HIL mode is enabled from GCS (UAS might be in HIL even if this flag is not set, this defines the GCS HIL setting) + bool sensorHil; ///< True if sensor HIL is enabled protected slots: /** @brief Write settings to disk */ diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index 55db5f2c439da0949cef68e6f08dcc67c1060295..365417587abfeed6640758dea1dbecbebb9fc526 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/src/ui/CommConfigurationWindow.cc @@ -274,6 +274,10 @@ void CommConfigurationWindow::setConnection() { if(!link->isConnected()) { link->connect(); + QGC::SLEEP::msleep(100); + if (link->isConnected()) + // Auto-close window on connect + this->window()->close(); } else { link->disconnect(); } diff --git a/src/ui/QGCToolBar.cc b/src/ui/QGCToolBar.cc index 1d25a9bdc528c948636c3211b05a96feb1b155ce..74acfc0e4be15fcbd4a694cfe16fdb98510d1420 100644 --- a/src/ui/QGCToolBar.cc +++ b/src/ui/QGCToolBar.cc @@ -23,6 +23,7 @@ This file is part of the QGROUNDCONTROL project #include #include +#include #include "QGCToolBar.h" #include "UASManager.h" #include "MainWindow.h" @@ -39,7 +40,8 @@ QGCToolBar::QGCToolBar(QWidget *parent) : wpId(0), wpDistance(0), systemArmed(false), - lastLogDirectory(QDesktopServices::storageLocation(QDesktopServices::DesktopLocation)) + lastLogDirectory(QDesktopServices::storageLocation(QDesktopServices::DesktopLocation)), + currentLink(NULL) { setObjectName("QGC_TOOLBAR"); @@ -110,8 +112,13 @@ QGCToolBar::QGCToolBar(QWidget *parent) : toolBarMessageLabel->setToolTip(tr("Most recent system message")); addWidget(toolBarMessageLabel); + QWidget* spacer = new QWidget(); + spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + addWidget(spacer); + connectButton = new QPushButton(tr("Connect"), this); connectButton->setToolTip(tr("Connect wireless link to MAV")); + connectButton->setCheckable(true); addWidget(connectButton); connect(connectButton, SIGNAL(clicked(bool)), this, SLOT(connectLink(bool))); @@ -121,6 +128,16 @@ QGCToolBar::QGCToolBar(QWidget *parent) : setActiveUAS(UASManager::instance()->getActiveUAS()); connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + if (LinkManager::instance()->getLinks().count() > 2) + addLink(LinkManager::instance()->getLinks().last()); + // XXX implies that connect button is always active for the last used link + connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addLink(LinkInterface*))); + + // Update label if required + if (LinkManager::instance()->getLinks().count() < 3) { + connectButton->setText(tr("New Link")); + } + // Set the toolbar to be updated every 2s connect(&updateViewTimer, SIGNAL(timeout()), this, SLOT(updateView())); updateViewTimer.start(2000); @@ -465,8 +482,52 @@ void QGCToolBar::receiveTextMessage(int uasid, int componentid, int severity, QS lastSystemMessageTimeMs = QGC::groundTimeMilliseconds(); } +void QGCToolBar::addLink(LinkInterface* link) +{ + // XXX magic number + if (LinkManager::instance()->getLinks().count() > 2) { + currentLink = link; + connect(currentLink, SIGNAL(connected(bool)), this, SLOT(updateLinkState(bool))); + updateLinkState(link->isConnected()); + } +} + +void QGCToolBar::removeLink(LinkInterface* link) +{ + if (link == currentLink) { + currentLink = NULL; + // XXX magic number + if (LinkManager::instance()->getLinks().count() > 2) { + currentLink = LinkManager::instance()->getLinks().last(); + updateLinkState(currentLink->isConnected()); + } else { + connectButton->setText(tr("New Link")); + } + } +} + +void QGCToolBar::updateLinkState(bool connected) +{ + if (currentLink && currentLink->isConnected()) + { + connectButton->setText(tr("Disconnect")); + connectButton->blockSignals(true); + connectButton->setChecked(true); + connectButton->blockSignals(false); + } + else + { + connectButton->setText(tr("Connect")); + connectButton->blockSignals(true); + connectButton->setChecked(false); + connectButton->blockSignals(false); + } +} + void QGCToolBar::connectLink(bool connect) { + // No serial port yet present + // XXX magic number if (connect && LinkManager::instance()->getLinks().count() < 3) { MainWindow::instance()->addLink(); @@ -475,19 +536,6 @@ void QGCToolBar::connectLink(bool connect) } else if (!connect && LinkManager::instance()->getLinks().count() > 2) { LinkManager::instance()->getLinks().last()->disconnect(); } - - if (LinkManager::instance()->getLinks().count() > 2) { - if (LinkManager::instance()->getLinks().last()->isConnected()) - { - connectButton->setText(tr("Disconnect")); - } - else - { - connectButton->setText(tr("Connect")); - } - - } - } diff --git a/src/ui/QGCToolBar.h b/src/ui/QGCToolBar.h index 29b85f185563fe46e3fcf2930196252e5c6d8276..2fcdc3f9a2015c69e790d366c73010e8d2f064eb 100644 --- a/src/ui/QGCToolBar.h +++ b/src/ui/QGCToolBar.h @@ -45,6 +45,12 @@ public: public slots: /** @brief Set the system that is currently displayed by this widget */ void setActiveUAS(UASInterface* active); + /** @brief Set the link which is currently handled with connecting / disconnecting */ + void addLink(LinkInterface* link); + /** @brief Remove link which is currently handled */ + void removeLink(LinkInterface* link); + /** @brief Update the link state */ + void updateLinkState(bool connected); /** @brief Set the system state */ void updateState(UASInterface* system, QString name, QString description); /** @brief Set the system mode */ @@ -112,6 +118,7 @@ protected: QTimer updateViewTimer; bool systemArmed; QString lastLogDirectory; + LinkInterface* currentLink; }; #endif // QGCTOOLBAR_H