Simplified link creation / connection a bit

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<LinkInterface*>(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;

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);
 
 };
 

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()

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()

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 */

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();
     }

src/ui/QGCToolBar.cc

@@ -23,6 +23,7 @@ This file is part of the QGROUNDCONTROL project
 
 #include <QToolButton>
 #include <QLabel>
+#include <QSpacerItem>
 #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"));
-        }
-
-    }
-
 }
 
 

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