diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc
index a714bcca027efe3958bdcf60f7a55f061d8042af..0312dc9688cac40b77c7c764446ccea2f4d2b8a8 100644
--- a/src/uas/UASManager.cc
+++ b/src/uas/UASManager.cc
@@ -310,38 +310,23 @@ void UASManager::removeUAS(UASInterface* uas)
     {
         int listindex = systems.indexOf(uas);
 
-        // If this is the active UAS, select a new one.
+        // Remove this system from local data store.
+        systems.removeAt(listindex);
+
+        // If this is the active UAS, select a new one if one exists otherwise
+        // indicate that there are no active UASes.
         if (uas == activeUAS)
         {
-            if (systems.count() > 1)
+            if (systems.count())
             {
-                // We only set a new UAS if more than one is present
-                if (listindex != 0)
-                {
-                    // The system to be removed is not at position 1
-                    // set position one as new active system
-                    setActiveUAS(systems.first());
-                }
-                else
-                {
-                    // The system to be removed is at position 1,
-                    // select the next system
-                    setActiveUAS(systems.at(1));
-                }
+                setActiveUAS(systems.first());
             }
             else
             {
-                // TODO send a null pointer if no UAS is present any more
-                // This has to be properly tested however, since it might
-                // crash code parts not handling null pointers correctly.
-                activeUAS = NULL;
-                // XXX Not emitting the null pointer yet
+                setActiveUAS(NULL);
             }
         }
 
-        // Finally delete a local reference to this UAS
-        systems.removeAt(listindex);
-
         // Notify other UI elements that a UAS is being deleted before finally deleting it.
         qDebug() << "Deleting UAS object: " << uas->getUASName();
         emit UASDeleted(uas);
@@ -449,21 +434,24 @@ UASInterface* UASManager::getUASForId(int id)
 
 void UASManager::setActiveUAS(UASInterface* uas)
 {
-    if (uas != NULL) {
-        activeUASMutex.lock();
-        if (activeUAS != NULL) {
-            emit activeUASStatusChanged(activeUAS, false);
-            emit activeUASStatusChanged(activeUAS->getUASID(), false);
-        }
-        activeUAS = uas;
-        activeUASMutex.unlock();
+    // Signal components that the last UAS is no longer active.
+    activeUASMutex.lock();
+    if (activeUAS != NULL) {
+        emit activeUASStatusChanged(activeUAS, false);
+        emit activeUASStatusChanged(activeUAS->getUASID(), false);
+    }
+    activeUAS = uas;
+    activeUASMutex.unlock();
 
+    // And signal that a new UAS is.
+    emit activeUASSet(activeUAS);
+    if (activeUAS)
+    {
         activeUAS->setSelected();
-        emit activeUASSet(uas);
-        emit activeUASSet(uas->getUASID());
-        emit activeUASSetListIndex(systems.indexOf(uas));
-        emit activeUASStatusChanged(uas, true);
-        emit activeUASStatusChanged(uas->getUASID(), true);
+        emit activeUASSet(activeUAS->getUASID());
+        emit activeUASSetListIndex(systems.indexOf(activeUAS));
+        emit activeUASStatusChanged(activeUAS, true);
+        emit activeUASStatusChanged(activeUAS->getUASID(), true);
     }
 }
 
diff --git a/src/uas/UASManager.h b/src/uas/UASManager.h
index c1243520e4f3770c924269da367454355f3cb763..86a13d6dea0d36d24582d323280baa07b341f22f 100644
--- a/src/uas/UASManager.h
+++ b/src/uas/UASManager.h
@@ -151,12 +151,12 @@ public slots:
      **/
     void addUAS(UASInterface* UAS);
 
-    /** @brief Remove a system from the list. Also triggers the UAS to kill itself. */
+    /** @brief Remove a system from the list. If this is the active UAS, it switches to another one calling setActiveUAS. Also triggers the UAS to kill itself. */
     void removeUAS(UASInterface* uas);
 
 
     /**
-      * @brief Set a UAS as currently selected
+      * @brief Set a UAS as currently selected. NULL is a valid value for when no other valid UAS's are available.
       *
       * @param UAS Unmanned Air System to set
       **/
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index 933fa9a200b73f0d0d4e4d571bd86be1b3fdd253..b024584a8ae74516a7032388709ce070ffd34b0a 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -177,15 +177,14 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
     setStatusTip(tr("View from top in body frame. Scroll with mouse wheel to change the horizontal field of view of the widget."));
 
     connect(&statusClearTimer, SIGNAL(timeout()), this, SLOT(clearStatusMessage()));
-    statusClearTimer.start(3000);
 
     if (UASManager::instance()->getActiveUAS())
     {
         setActiveUAS(UASManager::instance()->getActiveUAS());
     }
 
-    // Listen for the removal of the active UAS.
-    setActiveUAS(UASManager::instance()->getActiveUAS());
+    connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)),
+            this, SLOT(setActiveUAS(UASInterface*)));
 
     setFocusPolicy(Qt::StrongFocus);
 }
@@ -239,7 +238,7 @@ void HSIDisplay::paintEvent(QPaintEvent * event)
 
 void HSIDisplay::renderOverlay()
 {
-    if (!isVisible()) return;
+    if (!isVisible() || !uas) return;
 #if (QGC_EVENTLOOP_DEBUG)
     qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
 #endif
@@ -914,9 +913,6 @@ void HSIDisplay::setMetricWidth(double width)
  */
 void HSIDisplay::setActiveUAS(UASInterface* uas)
 {
-    if (!uas)
-        return;
-
     if (this->uas != NULL) {
         disconnect(this->uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool)));
         disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
@@ -949,47 +945,49 @@ void HSIDisplay::setActiveUAS(UASInterface* uas)
         disconnect(this->uas, SIGNAL(actuatorStatusChanged(bool,bool,bool)), this, SLOT(updateActuatorStatus(bool,bool,bool)));
     }
 
-    connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool)));
-    connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
-    connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
-    connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
-    connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
-    connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
-    connect(uas, SIGNAL(velocityChanged_NED(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
-    connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
-
-    connect(uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool)));
-    connect(uas, SIGNAL(positionXYControlEnabled(bool)), this, SLOT(updatePositionXYControllerEnabled(bool)));
-    connect(uas, SIGNAL(positionZControlEnabled(bool)), this, SLOT(updatePositionZControllerEnabled(bool)));
-    connect(uas, SIGNAL(positionYawControlEnabled(bool)), this, SLOT(updatePositionYawControllerEnabled(bool)));
-
-    connect(uas, SIGNAL(localizationChanged(UASInterface*,int)), this, SLOT(updateLocalization(UASInterface*,int)));
-    connect(uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int)));
-    connect(uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int)));
-    connect(uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int)));
-    connect(uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float)));
-
-    connect(uas, SIGNAL(gyroStatusChanged(bool,bool,bool)), this, SLOT(updateGyroStatus(bool,bool,bool)));
-    connect(uas, SIGNAL(accelStatusChanged(bool,bool,bool)), this, SLOT(updateAccelStatus(bool,bool,bool)));
-    connect(uas, SIGNAL(magSensorStatusChanged(bool,bool,bool)), this, SLOT(updateMagSensorStatus(bool,bool,bool)));
-    connect(uas, SIGNAL(baroStatusChanged(bool,bool,bool)), this, SLOT(updateBaroStatus(bool,bool,bool)));
-    connect(uas, SIGNAL(airspeedStatusChanged(bool,bool,bool)), this, SLOT(updateAirspeedStatus(bool,bool,bool)));
-    connect(uas, SIGNAL(opticalFlowStatusChanged(bool,bool,bool)), this, SLOT(updateOpticalFlowStatus(bool,bool,bool)));
-    connect(uas, SIGNAL(laserStatusChanged(bool,bool,bool)), this, SLOT(updateLaserStatus(bool,bool,bool)));
-    connect(uas, SIGNAL(groundTruthSensorStatusChanged(bool,bool,bool)), this, SLOT(updateGroundTruthSensorStatus(bool,bool,bool)));
-    connect(uas, SIGNAL(actuatorStatusChanged(bool,bool,bool)), this, SLOT(updateActuatorStatus(bool,bool,bool)));
+    if (uas)
+    {
+        connect(uas, SIGNAL(gpsSatelliteStatusChanged(int,int,float,float,float,bool)), this, SLOT(updateSatellite(int,int,float,float,float,bool)));
+        connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
+        connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
+        connect(uas, SIGNAL(attitudeThrustSetPointChanged(UASInterface*,double,double,double,double,quint64)), this, SLOT(updateAttitudeSetpoints(UASInterface*,double,double,double,double,quint64)));
+        connect(uas, SIGNAL(positionSetPointsChanged(int,float,float,float,float,quint64)), this, SLOT(updatePositionSetpoints(int,float,float,float,float,quint64)));
+        connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), this, SLOT(updateUserPositionSetpoints(int,float,float,float,float)));
+        connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
+        connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
+
+        connect(uas, SIGNAL(attitudeControlEnabled(bool)), this, SLOT(updateAttitudeControllerEnabled(bool)));
+        connect(uas, SIGNAL(positionXYControlEnabled(bool)), this, SLOT(updatePositionXYControllerEnabled(bool)));
+        connect(uas, SIGNAL(positionZControlEnabled(bool)), this, SLOT(updatePositionZControllerEnabled(bool)));
+        connect(uas, SIGNAL(positionYawControlEnabled(bool)), this, SLOT(updatePositionYawControllerEnabled(bool)));
+
+        connect(uas, SIGNAL(localizationChanged(UASInterface*,int)), this, SLOT(updateLocalization(UASInterface*,int)));
+        connect(uas, SIGNAL(visionLocalizationChanged(UASInterface*,int)), this, SLOT(updateVisionLocalization(UASInterface*,int)));
+        connect(uas, SIGNAL(gpsLocalizationChanged(UASInterface*,int)), this, SLOT(updateGpsLocalization(UASInterface*,int)));
+        connect(uas, SIGNAL(irUltraSoundLocalizationChanged(UASInterface*,int)), this, SLOT(updateInfraredUltrasoundLocalization(UASInterface*,int)));
+        connect(uas, SIGNAL(objectDetected(uint,int,int,QString,int,float,float)), this, SLOT(updateObjectPosition(uint,int,int,QString,int,float,float)));
+
+        connect(uas, SIGNAL(gyroStatusChanged(bool,bool,bool)), this, SLOT(updateGyroStatus(bool,bool,bool)));
+        connect(uas, SIGNAL(accelStatusChanged(bool,bool,bool)), this, SLOT(updateAccelStatus(bool,bool,bool)));
+        connect(uas, SIGNAL(magSensorStatusChanged(bool,bool,bool)), this, SLOT(updateMagSensorStatus(bool,bool,bool)));
+        connect(uas, SIGNAL(baroStatusChanged(bool,bool,bool)), this, SLOT(updateBaroStatus(bool,bool,bool)));
+        connect(uas, SIGNAL(airspeedStatusChanged(bool,bool,bool)), this, SLOT(updateAirspeedStatus(bool,bool,bool)));
+        connect(uas, SIGNAL(opticalFlowStatusChanged(bool,bool,bool)), this, SLOT(updateOpticalFlowStatus(bool,bool,bool)));
+        connect(uas, SIGNAL(laserStatusChanged(bool,bool,bool)), this, SLOT(updateLaserStatus(bool,bool,bool)));
+        connect(uas, SIGNAL(groundTruthSensorStatusChanged(bool,bool,bool)), this, SLOT(updateGroundTruthSensorStatus(bool,bool,bool)));
+        connect(uas, SIGNAL(actuatorStatusChanged(bool,bool,bool)), this, SLOT(updateActuatorStatus(bool,bool,bool)));
+        statusClearTimer.start(3000);
+    }
+    else
+    {
+        statusClearTimer.stop();
+    }
 
     this->uas = uas;
 
     resetMAVState();
 }
 
-void HSIDisplay::removeUAS(UASInterface* uas)
-{
-    this->uas = NULL;
-    resetMAVState();
-}
-
 void HSIDisplay::updateSpeed(UASInterface* uas, double vx, double vy, double vz, quint64 time)
 {
     Q_UNUSED(uas);
@@ -1094,7 +1092,7 @@ void HSIDisplay::updateAttitude(UASInterface* uas, double roll, double pitch, do
 
 void HSIDisplay::updateUserPositionSetpoints(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired)
 {
-	Q_UNUSED(uasid);
+    Q_UNUSED(uasid);
     uiXSetCoordinate = xDesired;
     uiYSetCoordinate = yDesired;
     uiZSetCoordinate = zDesired;
diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h
index 2ddcc6dd1d4b6b1e3d1c12304817805c5e05d38a..6ebfeb73880c4532fca3e752d087fd4b4a4faf54 100644
--- a/src/ui/HSIDisplay.h
+++ b/src/ui/HSIDisplay.h
@@ -52,7 +52,6 @@ public:
 
 public slots:
     void setActiveUAS(UASInterface* uas);
-    void removeUAS(UASInterface* uas);
     /** @brief Set the width in meters this widget shows from top */
     void setMetricWidth(double width);
     void updateSatellite(int uasid, int satid, float azimuth, float direction, float snr, bool used);
diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc
index 23ac8405127e0d2107c75d38698855307532fd1f..6820d0306fb95ce21356f13a1967de506c1f98ce 100644
--- a/src/ui/HUD.cc
+++ b/src/ui/HUD.cc
@@ -327,10 +327,10 @@ void HUD::setActiveUAS(UASInterface* uas)
             connect(u, SIGNAL(imageStarted(quint64)), this, SLOT(startImage(quint64)));
             connect(u, SIGNAL(imageReady(UASInterface*)), this, SLOT(copyImage()));
         }
-
-        // Set new UAS
-        this->uas = uas;
     }
+
+    // Set new UAS
+    this->uas = uas;
 }
 
 //void HUD::updateAttitudeThrustSetPoint(UASInterface* uas, double rollDesired, double pitchDesired, double yawDesired, double thrustDesired, quint64 msec)
diff --git a/src/ui/QGCHilXPlaneConfiguration.cc b/src/ui/QGCHilXPlaneConfiguration.cc
index 6941af04076531d042dd31a3e769a18cecf34d18..abbc31768e723df5e2e75a7d6c12af0789e5a185 100644
--- a/src/ui/QGCHilXPlaneConfiguration.cc
+++ b/src/ui/QGCHilXPlaneConfiguration.cc
@@ -44,7 +44,7 @@ QGCHilXPlaneConfiguration::QGCHilXPlaneConfiguration(QGCHilLink* link, QWidget *
 
 void QGCHilXPlaneConfiguration::setVersion(int version)
 {
-
+    Q_UNUSED(version);
 }
 
 void QGCHilXPlaneConfiguration::toggleSimulation(bool connect)
diff --git a/src/ui/QGCToolBar.cc b/src/ui/QGCToolBar.cc
index 314aafe8101797cfb18b92264cbd4d2bff00decc..9e50a53ca15f59e1b67c11dfd9dff13d0f1e7adf 100644
--- a/src/ui/QGCToolBar.cc
+++ b/src/ui/QGCToolBar.cc
@@ -166,7 +166,6 @@ void QGCToolBar::createUI()
     // Configure the toolbar for the current default UAS
     setActiveUAS(UASManager::instance()->getActiveUAS());
     connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*)));
-    connect(UASManager::instance(), SIGNAL(UASDeleted(UASInterface*)), this, SLOT(removeUAS(UASInterface*)));
 
     if (LinkManager::instance()->getLinks().count() > 2)
         addLink(LinkManager::instance()->getLinks().last());
@@ -288,8 +287,8 @@ void QGCToolBar::advancedActivityTriggered(QAction* action)
 
 void QGCToolBar::setActiveUAS(UASInterface* active)
 {
-    // Do nothing if system is the same or NULL
-    if ((active == NULL) || mav == active) return;
+    // Do nothing if system is the same
+    if (mav == active) return;
 
     // If switching UASes, disconnect the only one.
     if (mav)
@@ -317,54 +316,43 @@ void QGCToolBar::setActiveUAS(UASInterface* active)
 
     // Connect new system
     mav = active;
-    connect(active, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*, QString,QString)));
-    connect(active, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
-    connect(active, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString)));
-    connect(active, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
-    connect(active, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(receiveTextMessage(int,int,int,QString)));
-    connect(active, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBatteryRemaining(UASInterface*, double, double, double, int)));
-    connect(active, SIGNAL(armingChanged(bool)), this, SLOT(updateArmingState(bool)));
-    connect(active, SIGNAL(heartbeatTimeout(bool, unsigned int)), this, SLOT(heartbeatTimeout(bool,unsigned int)));
-    connect(active, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(globalPositionChanged(UASInterface*,double,double,double,quint64)));
-    if (active->getWaypointManager())
+    if (mav)
     {
-        connect(active->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(updateCurrentWaypoint(quint16)));
-        connect(active->getWaypointManager(), SIGNAL(waypointDistanceChanged(double)), this, SLOT(updateWaypointDistance(double)));
-    }
-
-    // Update all values once
-    systemName = mav->getUASName();
-    systemArmed = mav->isArmed();
-    toolBarNameLabel->setText(mav->getUASName());
-    toolBarNameLabel->setStyleSheet(QString("QLabel {color: %1;}").arg(mav->getColor().name()));
-    symbolLabel->setStyleSheet(QString("QWidget {background-color: %1;}").arg(mav->getColor().name()));
-    toolBarModeLabel->setText(mav->getShortMode());
-    toolBarStateLabel->setText(mav->getShortState());
-    toolBarTimeoutLabel->setText("");
-    toolBarDistLabel->setText("");
-    toolBarBatteryBar->setEnabled(true);
-    setSystemType(mav, mav->getSystemType());
-}
+        connect(mav, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*, QString,QString)));
+        connect(mav, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
+        connect(mav, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString)));
+        connect(mav, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint)));
+        connect(mav, SIGNAL(textMessageReceived(int,int,int,QString)), this, SLOT(receiveTextMessage(int,int,int,QString)));
+        connect(mav, SIGNAL(batteryChanged(UASInterface*,double,double,int)), this, SLOT(updateBatteryRemaining(UASInterface*,double,double,int)));
+        connect(mav, SIGNAL(armingChanged(bool)), this, SLOT(updateArmingState(bool)));
+        connect(mav, SIGNAL(heartbeatTimeout(bool, unsigned int)), this, SLOT(heartbeatTimeout(bool,unsigned int)));
+        connect(mav, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(globalPositionChanged(UASInterface*,double,double,double,quint64)));
+        if (mav->getWaypointManager())
+        {
+            connect(mav->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(updateCurrentWaypoint(quint16)));
+            connect(mav->getWaypointManager(), SIGNAL(waypointDistanceChanged(double)), this, SLOT(updateWaypointDistance(double)));
+        }
 
-/**
- * @brief Handle removal of the UAS that is currently being displayed.
- * Stop updating the UI periodically, reset the UI, and reset our stored UAS.
- * @param uas The UAS to remove.
- */
-void QGCToolBar::removeUAS(UASInterface* uas)
-{
-    if (mav == uas) {
+        // Update all values once
+        systemName = mav->getUASName();
+        systemArmed = mav->isArmed();
+        toolBarNameLabel->setText(mav->getUASName());
+        toolBarNameLabel->setStyleSheet(QString("QLabel {color: %1;}").arg(mav->getColor().name()));
+        symbolLabel->setStyleSheet(QString("QWidget {background-color: %1;}").arg(mav->getColor().name()));
+        toolBarModeLabel->setText(mav->getShortMode());
+        toolBarStateLabel->setText(mav->getShortState());
+        toolBarTimeoutLabel->setText("");
+        toolBarDistLabel->setText("");
+        toolBarBatteryBar->setEnabled(true);
+        setSystemType(mav, mav->getSystemType());
+    }
+    else
+    {
         updateViewTimer.stop();
         resetToolbarUI();
-        mav = NULL;
     }
 }
 
-void QGCToolBar::createCustomWidgets()
-{
-
-}
-
 void QGCToolBar::updateArmingState(bool armed)
 {
     systemArmed = armed;
diff --git a/src/ui/QGCToolBar.h b/src/ui/QGCToolBar.h
index 9ad1f5a9a6337b06589a597d0d61627ecb9a94e4..d1ba366b9f5cfd8b3e115a6c6b81b326cb13b1a3 100644
--- a/src/ui/QGCToolBar.h
+++ b/src/ui/QGCToolBar.h
@@ -46,8 +46,6 @@ public:
 public slots:
     /** @brief Set the system that is currently displayed by this widget */
     void setActiveUAS(UASInterface* active);
-    /** @brief Remove the provided UAS if it's currently active from the toolbar */
-    void removeUAS(UASInterface* uas);
     /** @brief Set the link which is currently handled with connecting / disconnecting */
     void addLink(LinkInterface* link);
     /** @brief Remove link which is currently handled */
@@ -86,7 +84,6 @@ public slots:
     void advancedActivityTriggered(QAction* action);
 
 protected:
-    void createCustomWidgets();
     void storeSettings();
     void loadSettings();
     void createUI();
diff --git a/src/ui/QGCVehicleConfig.cc b/src/ui/QGCVehicleConfig.cc
index 282f18e9f79aa07111dd8aa834b5562fdc48acda..81ffb140d43d12c72bfdf153020f88bce25cccbd 100644
--- a/src/ui/QGCVehicleConfig.cc
+++ b/src/ui/QGCVehicleConfig.cc
@@ -36,11 +36,6 @@ QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) :
     ui(new Ui::QGCVehicleConfig)
 {
     doneLoadingConfig = false;
-    systemTypeToParamMap["FIXED_WING"] = new QMap<QString,QGCToolWidget*>();
-    systemTypeToParamMap["QUADROTOR"] = new QMap<QString,QGCToolWidget*>();
-    systemTypeToParamMap["GROUND_ROVER"] = new QMap<QString,QGCToolWidget*>();
-    systemTypeToParamMap["BOAT"] = new QMap<QString,QGCToolWidget*>();
-    libParamToWidgetMap = new QMap<QString,QGCToolWidget*>();
 
     setObjectName("QGC_VEHICLECONFIG");
     ui->setupUi(this);
@@ -690,19 +685,19 @@ void QGCVehicleConfig::loadConfig()
                                     //Based on the airframe, we add the parameter to different categories.
                                     if (parametersname == "ArduPlane") //MAV_TYPE_FIXED_WING FIXED_WING
                                     {
-                                        systemTypeToParamMap["FIXED_WING"]->insert(paramlist[i],tool);
+                                        systemTypeToParamMap["FIXED_WING"].insert(paramlist[i],tool);
                                     }
                                     else if (parametersname == "ArduCopter") //MAV_TYPE_QUADROTOR "QUADROTOR
                                     {
-                                        systemTypeToParamMap["QUADROTOR"]->insert(paramlist[i],tool);
+                                        systemTypeToParamMap["QUADROTOR"].insert(paramlist[i],tool);
                                     }
                                     else if (parametersname == "APMrover2") //MAV_TYPE_GROUND_ROVER GROUND_ROVER
                                     {
-                                        systemTypeToParamMap["GROUND_ROVER"]->insert(paramlist[i],tool);
+                                        systemTypeToParamMap["GROUND_ROVER"].insert(paramlist[i],tool);
                                     }
                                     else
                                     {
-                                        libParamToWidgetMap->insert(paramlist[i],tool);
+                                        libParamToWidgetMap.insert(paramlist[i],tool);
                                     }
                                 }
 
@@ -735,19 +730,19 @@ void QGCVehicleConfig::loadConfig()
                                     //Based on the airframe, we add the parameter to different categories.
                                     if (parametersname == "ArduPlane") //MAV_TYPE_FIXED_WING FIXED_WING
                                     {
-                                        systemTypeToParamMap["FIXED_WING"]->insert(paramlist[i],tool);
+                                        systemTypeToParamMap["FIXED_WING"].insert(paramlist[i],tool);
                                     }
                                     else if (parametersname == "ArduCopter") //MAV_TYPE_QUADROTOR "QUADROTOR
                                     {
-                                        systemTypeToParamMap["QUADROTOR"]->insert(paramlist[i],tool);
+                                        systemTypeToParamMap["QUADROTOR"].insert(paramlist[i],tool);
                                     }
                                     else if (parametersname == "APMrover2") //MAV_TYPE_GROUND_ROVER GROUND_ROVER
                                     {
-                                        systemTypeToParamMap["GROUND_ROVER"]->insert(paramlist[i],tool);
+                                        systemTypeToParamMap["GROUND_ROVER"].insert(paramlist[i],tool);
                                     }
                                     else
                                     {
-                                        libParamToWidgetMap->insert(paramlist[i],tool);
+                                        libParamToWidgetMap.insert(paramlist[i],tool);
                                     }
                                 }
 
@@ -791,7 +786,7 @@ void QGCVehicleConfig::loadConfig()
 void QGCVehicleConfig::setActiveUAS(UASInterface* active)
 {
     // Do nothing if system is the same or NULL
-    if ((active == NULL) || mav == active) return;
+    if (mav == active) return;
 
     if (mav)
     {
@@ -815,48 +810,61 @@ void QGCVehicleConfig::setActiveUAS(UASInterface* active)
 
     // Connect new system
     mav = active;
-    connect(active, SIGNAL(remoteControlChannelRawChanged(int,float)), this,
-               SLOT(remoteControlChannelRawChanged(int,float)));
-    connect(active, SIGNAL(parameterChanged(int,int,QString,QVariant)), this,
-               SLOT(parameterChanged(int,int,QString,QVariant)));
+    if (mav)
+        {
+        connect(active, SIGNAL(remoteControlChannelRawChanged(int,float)), this,
+                   SLOT(remoteControlChannelRawChanged(int,float)));
+        connect(active, SIGNAL(parameterChanged(int,int,QString,QVariant)), this,
+                   SLOT(parameterChanged(int,int,QString,QVariant)));
 
-    if (systemTypeToParamMap.contains(mav->getSystemTypeName()))
-    {
-        paramToWidgetMap = systemTypeToParamMap[mav->getSystemTypeName()];
-    }
-    else
-    {
-        //Indication that we have no meta data for this system type.
-        qDebug() << "No parameters defined for system type:" << mav->getSystemTypeName();
-        systemTypeToParamMap[mav->getSystemTypeName()] = new QMap<QString,QGCToolWidget*>();
-        paramToWidgetMap = systemTypeToParamMap[mav->getSystemTypeName()];
-    }
+        if (systemTypeToParamMap.contains(mav->getSystemTypeName()))
+        {
+            paramToWidgetMap = systemTypeToParamMap[mav->getSystemTypeName()];
+        }
+        else
+        {
+            //Indication that we have no meta data for this system type.
+            qDebug() << "No parameters defined for system type:" << mav->getSystemTypeName();
+            paramToWidgetMap = systemTypeToParamMap[mav->getSystemTypeName()];
+        }
 
-    if (!paramTooltips.isEmpty())
-    {
-           mav->getParamManager()->setParamInfo(paramTooltips);
-    }
+        if (!paramTooltips.isEmpty())
+        {
+               mav->getParamManager()->setParamInfo(paramTooltips);
+        }
 
-    qDebug() << "CALIBRATION!! System Type Name:" << mav->getSystemTypeName();
+        qDebug() << "CALIBRATION!! System Type Name:" << mav->getSystemTypeName();
 
-    //Load configuration after 1ms. This allows it to go into the event loop, and prevents application hangups due to the
-    //amount of time it actually takes to load the configuration windows.
-    QTimer::singleShot(1,this,SLOT(loadConfig()));
+        //Load configuration after 1ms. This allows it to go into the event loop, and prevents application hangups due to the
+        //amount of time it actually takes to load the configuration windows.
+        QTimer::singleShot(1,this,SLOT(loadConfig()));
 
-    updateStatus(QString("Reading from system %1").arg(mav->getUASName()));
+        updateStatus(QString("Reading from system %1").arg(mav->getUASName()));
 
-    // Since a system is now connected, enable the VehicleConfig UI.
-    //ui->tabWidget->setEnabled(true);
-    ui->setButton->setEnabled(true);
-    ui->refreshButton->setEnabled(true);
-    ui->readButton->setEnabled(true);
-    ui->writeButton->setEnabled(true);
-    ui->loadFileButton->setEnabled(true);
-    ui->saveFileButton->setEnabled(true);
-    if (mav->getAutopilotTypeName() == "ARDUPILOTMEGA")
+        // Since a system is now connected, enable the VehicleConfig UI.
+        //ui->tabWidget->setEnabled(true);
+        ui->setButton->setEnabled(true);
+        ui->refreshButton->setEnabled(true);
+        ui->readButton->setEnabled(true);
+        ui->writeButton->setEnabled(true);
+        ui->loadFileButton->setEnabled(true);
+        ui->saveFileButton->setEnabled(true);
+        if (mav->getAutopilotTypeName() == "ARDUPILOTMEGA")
+        {
+            ui->readButton->hide();
+            ui->writeButton->hide();
+        }
+    }
+    else
     {
-        ui->readButton->hide();
-        ui->writeButton->hide();
+        ui->setButton->setEnabled(false);
+        ui->refreshButton->setEnabled(false);
+        ui->readButton->show();
+        ui->readButton->setEnabled(false);
+        ui->writeButton->show();
+        ui->writeButton->setEnabled(false);
+        ui->loadFileButton->setEnabled(false);
+        ui->saveFileButton->setEnabled(false);
     }
 }
 
@@ -1086,26 +1094,26 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
         return;
     }
 
-    if (paramToWidgetMap->contains(parameterName))
+    if (paramToWidgetMap.contains(parameterName))
     {
         //Main group of parameters of the selected airframe
-        paramToWidgetMap->value(parameterName)->setParameterValue(uas,component,parameterName,value);
-        if (toolToBoxMap.contains(paramToWidgetMap->value(parameterName)))
+        paramToWidgetMap.value(parameterName)->setParameterValue(uas,component,parameterName,value);
+        if (toolToBoxMap.contains(paramToWidgetMap.value(parameterName)))
         {
-            toolToBoxMap[paramToWidgetMap->value(parameterName)]->show();
+            toolToBoxMap[paramToWidgetMap.value(parameterName)]->show();
         }
         else
         {
             qCritical() << "Widget with no box, possible memory corruption for param:" << parameterName;
         }
     }
-    else if (libParamToWidgetMap->contains(parameterName))
+    else if (libParamToWidgetMap.contains(parameterName))
     {
         //All the library parameters
-        libParamToWidgetMap->value(parameterName)->setParameterValue(uas,component,parameterName,value);
-        if (toolToBoxMap.contains(libParamToWidgetMap->value(parameterName)))
+        libParamToWidgetMap.value(parameterName)->setParameterValue(uas,component,parameterName,value);
+        if (toolToBoxMap.contains(libParamToWidgetMap.value(parameterName)))
         {
-            toolToBoxMap[libParamToWidgetMap->value(parameterName)]->show();
+            toolToBoxMap[libParamToWidgetMap.value(parameterName)]->show();
         }
         else
         {
@@ -1123,7 +1131,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
             {
                 //It should be grouped with this one, add it.
                 toolWidgets[i]->addParam(uas,component,parameterName,value);
-                libParamToWidgetMap->insert(parameterName,toolWidgets[i]);
+                libParamToWidgetMap.insert(parameterName,toolWidgets[i]);
                 found  = true;
                 break;
             }
@@ -1141,7 +1149,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete
             tool->setObjectName(tooltitle);
             //tool->setSettings(set);
             tool->addParam(uas,component,parameterName,value);
-            libParamToWidgetMap->insert(parameterName,tool);
+            libParamToWidgetMap.insert(parameterName,tool);
             toolWidgets.append(tool);
             QGroupBox *box = new QGroupBox(this);
             box->setTitle(tool->objectName());
diff --git a/src/ui/QGCVehicleConfig.h b/src/ui/QGCVehicleConfig.h
index a38e450ec5e3e35d8c3c55140157a0b0b586da35..4db21ed1c850eecbfe6f615baa84758f427937ce 100644
--- a/src/ui/QGCVehicleConfig.h
+++ b/src/ui/QGCVehicleConfig.h
@@ -17,7 +17,7 @@ class QGCVehicleConfig;
 class QGCVehicleConfig : public QWidget
 {
     Q_OBJECT
-    
+
 public:
     explicit QGCVehicleConfig(QWidget *parent = 0);
     ~QGCVehicleConfig();
@@ -185,12 +185,13 @@ protected:
     QList<QGCToolWidget*> toolWidgets;  ///< Configurable widgets
     bool calibrationEnabled;            ///< calibration mode on / off
 
-    QMap<QString,QGCToolWidget*> *paramToWidgetMap;                     ///< Holds the current active MAV's parameter widgets.
-    QMap<QString,QGCToolWidget*> *libParamToWidgetMap;                  ///< Holds the library parameter widgets
-    QMap<QString,QMap<QString,QGCToolWidget*>*> systemTypeToParamMap;   ///< Holds all loaded MAV specific parameter widgets, for every MAV.
+    QMap<QString,QGCToolWidget*> paramToWidgetMap;                     ///< Holds the current active MAV's parameter widgets.
+    QList<QWidget*> additionalTabs;                                   ///< Stores additional tabs loaded for this vehicle/autopilot configuration. Used for cleaning up.
+    QMap<QString,QGCToolWidget*> libParamToWidgetMap;                  ///< Holds the library parameter widgets
+    QMap<QString,QMap<QString,QGCToolWidget*> > systemTypeToParamMap;   ///< Holds all loaded MAV specific parameter widgets, for every MAV.
     QMap<QGCToolWidget*,QGroupBox*> toolToBoxMap;                       ///< Easy method of figuring out which QGroupBox is tied to which ToolWidget.
     QMap<QString,QString> paramTooltips;                                ///< Tooltips for the ? button next to a parameter.
-    
+
 private:
     Ui::QGCVehicleConfig *ui;
     QMap<QPushButton*,QWidget*> buttonToWidgetMap;
diff --git a/src/ui/QGCVehicleConfig.ui b/src/ui/QGCVehicleConfig.ui
index 8365d3d170ad1d5492e7789d008e39e9353b54b6..9bbc7af163ffb056189af2021cad08898a2665a4 100644
--- a/src/ui/QGCVehicleConfig.ui
+++ b/src/ui/QGCVehicleConfig.ui
@@ -7,7 +7,7 @@
     <x>0</x>
     <y>0</y>
     <width>1256</width>
-    <height>670</height>
+    <height>711</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -43,7 +43,7 @@
         <x>0</x>
         <y>0</y>
         <width>133</width>
-        <height>650</height>
+        <height>691</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_12">
@@ -876,8 +876,8 @@ Config</string>
               <string>&lt;!DOCTYPE HTML PUBLIC &quot;-//W3C//DTD HTML 4.0//EN&quot; &quot;http://www.w3.org/TR/REC-html40/strict.dtd&quot;&gt;
 &lt;html&gt;&lt;head&gt;&lt;meta name=&quot;qrichtext&quot; content=&quot;1&quot; /&gt;&lt;style type=&quot;text/css&quot;&gt;
 p, li { white-space: pre-wrap; }
-&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'MS Shell Dlg 2'; font-size:8.25pt; font-weight:400; font-style:normal;&quot;&gt;
-&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:8pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;&quot;&gt;
+&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'MS Shell Dlg 2'; font-size:8pt;&quot;&gt;&lt;br /&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
              </property>
             </widget>
            </item>
@@ -893,8 +893,8 @@ p, li { white-space: pre-wrap; }
                  <rect>
                   <x>0</x>
                   <y>0</y>
-                  <width>20</width>
-                  <height>20</height>
+                  <width>98</width>
+                  <height>28</height>
                  </rect>
                 </property>
                 <layout class="QVBoxLayout" name="verticalLayout_10">
@@ -968,8 +968,8 @@ p, li { white-space: pre-wrap; }
                   <rect>
                    <x>0</x>
                    <y>0</y>
-                   <width>16</width>
-                   <height>16</height>
+                   <width>98</width>
+                   <height>28</height>
                   </rect>
                  </property>
                  <layout class="QHBoxLayout" name="horizontalLayout_4">
@@ -1005,8 +1005,8 @@ p, li { white-space: pre-wrap; }
                   <rect>
                    <x>0</x>
                    <y>0</y>
-                   <width>16</width>
-                   <height>16</height>
+                   <width>98</width>
+                   <height>28</height>
                   </rect>
                  </property>
                  <layout class="QHBoxLayout" name="horizontalLayout_5">
@@ -1084,8 +1084,8 @@ p, li { white-space: pre-wrap; }
                   <rect>
                    <x>0</x>
                    <y>0</y>
-                   <width>16</width>
-                   <height>16</height>
+                   <width>98</width>
+                   <height>28</height>
                   </rect>
                  </property>
                  <layout class="QVBoxLayout" name="verticalLayout_7">
@@ -1121,8 +1121,8 @@ p, li { white-space: pre-wrap; }
                   <rect>
                    <x>0</x>
                    <y>0</y>
-                   <width>16</width>
-                   <height>16</height>
+                   <width>98</width>
+                   <height>28</height>
                   </rect>
                  </property>
                  <layout class="QVBoxLayout" name="verticalLayout_8">
diff --git a/src/ui/map/QGCMapWidget.cc b/src/ui/map/QGCMapWidget.cc
index d52994aae944f9219e4b70701e5ff9efa0dbc17c..2088de9c152d549ac70f3e456d97b64f5bd6979d 100644
--- a/src/ui/map/QGCMapWidget.cc
+++ b/src/ui/map/QGCMapWidget.cc
@@ -302,7 +302,6 @@ void QGCMapWidget::addUAS(UASInterface* uas)
 void QGCMapWidget::activeUASSet(UASInterface* uas)
 {
     // Only execute if proper UAS is set
-    if (!uas) return;
     this->uas = uas;
 
     // Disconnect old MAV manager
@@ -315,17 +314,26 @@ void QGCMapWidget::activeUASSet(UASInterface* uas)
         disconnect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*)));
     }
 
-    currWPManager = uas->getWaypointManager();
+    // Attach the new waypoint manager if a new UAS was selected. Otherwise, indicate
+    // that no such manager exists.
+    if (uas)
+    {
+        currWPManager = uas->getWaypointManager();
 
-    updateSelectedSystem(uas->getUASID());
-    followUAVID = uas->getUASID();
-    updateWaypointList(uas->getUASID());
+        updateSelectedSystem(uas->getUASID());
+        followUAVID = uas->getUASID();
+        updateWaypointList(uas->getUASID());
 
-    // Connect the waypoint manager / data storage to the UI
-    connect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int)));
-    connect(currWPManager, SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
-    connect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypointEditable(Waypoint*)));
-    connect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*)));
+        // Connect the waypoint manager / data storage to the UI
+        connect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int)));
+        connect(currWPManager, SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*)));
+        connect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypointEditable(Waypoint*)));
+        connect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*)));
+    }
+    else
+    {
+        currWPManager = NULL;
+    }
 }
 
 /**
diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc
index ee535434fb52c794d9c00521a370cc67bb79d8ba..43931008e0ce3e76d8dd7e0b69d89037efd0193c 100644
--- a/src/ui/map3D/Pixhawk3DWidget.cc
+++ b/src/ui/map3D/Pixhawk3DWidget.cc
@@ -113,7 +113,10 @@ Pixhawk3DWidget::~Pixhawk3DWidget()
 void
 Pixhawk3DWidget::activeSystemChanged(UASInterface* uas)
 {
-    mActiveSystemId = uas->getUASID();
+    if (uas)
+    {
+        mActiveSystemId = uas->getUASID();
+    }
 
     mActiveUAS = uas;
 
diff --git a/src/ui/uas/UASActionsWidget.cpp b/src/ui/uas/UASActionsWidget.cpp
index 2abb5bb3a2c45aea0720827b6ba030f53fb00bcd..96d08c7b0a997038bac275b24608c5d6222efc39 100644
--- a/src/ui/uas/UASActionsWidget.cpp
+++ b/src/ui/uas/UASActionsWidget.cpp
@@ -19,10 +19,13 @@ UASActionsWidget::UASActionsWidget(QWidget *parent) : QWidget(parent)
 void UASActionsWidget::activeUASSet(UASInterface *uas)
 {
     m_uas = uas;
-    connect(m_uas->getWaypointManager(),SIGNAL(waypointEditableListChanged()),this,SLOT(updateWaypointList()));
-    connect(m_uas->getWaypointManager(),SIGNAL(currentWaypointChanged(quint16)),this,SLOT(currentWaypointChanged(quint16)));
-    connect(m_uas,SIGNAL(armingChanged(bool)),this,SLOT(armingChanged(bool)));
-    armingChanged(m_uas->isArmed());
+    if (uas)
+    {
+        connect(m_uas->getWaypointManager(),SIGNAL(waypointEditableListChanged()),this,SLOT(updateWaypointList()));
+        connect(m_uas->getWaypointManager(),SIGNAL(currentWaypointChanged(quint16)),this,SLOT(currentWaypointChanged(quint16)));
+        connect(m_uas,SIGNAL(armingChanged(bool)),this,SLOT(armingChanged(bool)));
+        armingChanged(m_uas->isArmed());
+    }
     updateWaypointList();
 }
 void UASActionsWidget::armButtonClicked()
@@ -63,9 +66,12 @@ void UASActionsWidget::currentWaypointChanged(quint16 wpid)
 void UASActionsWidget::updateWaypointList()
 {
     ui.waypointComboBox->clear();
-    for (int i=0;i<m_uas->getWaypointManager()->getWaypointEditableList().size();i++)
+    if (m_uas)
     {
-        ui.waypointComboBox->addItem(QString::number(i));
+        for (int i=0;i<m_uas->getWaypointManager()->getWaypointEditableList().size();i++)
+        {
+            ui.waypointComboBox->addItem(QString::number(i));
+        }
     }
 }
 
diff --git a/src/ui/uas/UASControlWidget.cc b/src/ui/uas/UASControlWidget.cc
index 6632aa3276b936518a54e0344e6e6bd8865d9d60..8effb85e438affb9fceedb9593f4620ff79cab41 100644
--- a/src/ui/uas/UASControlWidget.cc
+++ b/src/ui/uas/UASControlWidget.cc
@@ -70,7 +70,7 @@ UASControlWidget::UASControlWidget(QWidget *parent) : QWidget(parent),
 
 void UASControlWidget::setUAS(UASInterface* uas)
 {
-    if (this->uas != 0)
+    if (this->uas)
     {
         UASInterface* oldUAS = UASManager::instance()->getUASForId(this->uas);
         disconnect(ui.controlButton, SIGNAL(clicked()), oldUAS, SLOT(armSystem()));
@@ -83,18 +83,25 @@ void UASControlWidget::setUAS(UASInterface* uas)
     }
 
     // Connect user interface controls
-    connect(ui.controlButton, SIGNAL(clicked()), this, SLOT(cycleContextButton()));
-    connect(ui.liftoffButton, SIGNAL(clicked()), uas, SLOT(launch()));
-    connect(ui.landButton, SIGNAL(clicked()), uas, SLOT(home()));
-    connect(ui.shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown()));
-    //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition()));
-    connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
-    connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));
-
-    ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName());
-
-    this->uas = uas->getUASID();
-    setBackgroundColor(uas->getColor());
+    if (uas)
+    {
+        connect(ui.controlButton, SIGNAL(clicked()), this, SLOT(cycleContextButton()));
+        connect(ui.liftoffButton, SIGNAL(clicked()), uas, SLOT(launch()));
+        connect(ui.landButton, SIGNAL(clicked()), uas, SLOT(home()));
+        connect(ui.shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown()));
+        //connect(ui.setHomeButton, SIGNAL(clicked()), uas, SLOT(setLocalOriginAtCurrentGPSPosition()));
+        connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString)));
+        connect(uas, SIGNAL(statusChanged(int)), this, SLOT(updateState(int)));
+
+        ui.controlStatusLabel->setText(tr("Connected to ") + uas->getUASName());
+
+        this->uas = uas->getUASID();
+        setBackgroundColor(uas->getColor());
+    }
+    else
+    {
+        this->uas = -1;
+    }
 }
 
 UASControlWidget::~UASControlWidget()
diff --git a/src/ui/uas/UASListWidget.cc b/src/ui/uas/UASListWidget.cc
index bf868e603839b06b7672f98feb5cc3ccb590c4fe..aef3a085e9c13f876b6d7010262d995c9cdfbe00 100644
--- a/src/ui/uas/UASListWidget.cc
+++ b/src/ui/uas/UASListWidget.cc
@@ -56,15 +56,21 @@ UASListWidget::UASListWidget(QWidget *parent) : QWidget(parent),
     m_ui->verticalLayout->addWidget(uWidget);
 
     linkToBoxMapping = QMap<LinkInterface*, QGroupBox*>();
+    uasToBoxMapping = QMap<UASInterface*, QGroupBox*>();
     uasViews = QMap<UASInterface*, UASView*>();
 
     this->setVisible(false);
 
+    // Listen for when UASes are added or removed. This does not manage the UASView
+    // widgets that are displayed within this widget.
     connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)),
             this, SLOT(addUAS(UASInterface*)));
+    connect(UASManager::instance(), SIGNAL(UASDeleted(UASInterface*)),
+            this, SLOT(removeUAS(UASInterface*)));
 
     // Get a list of all existing UAS
-    foreach (UASInterface* uas, UASManager::instance()->getUASList()) {
+    foreach (UASInterface* uas, UASManager::instance()->getUASList())
+    {
         addUAS(uas);
     }
 }
@@ -93,7 +99,7 @@ void UASListWidget::addUAS(UASInterface* uas)
     if (uasViews.isEmpty())
     {
         m_ui->verticalLayout->removeWidget(uWidget);
-        delete uWidget;
+        uWidget->deleteLater();
         uWidget = NULL;
     }
 
@@ -124,6 +130,7 @@ void UASListWidget::addUAS(UASInterface* uas)
             // And add the new UAS to the UASList
             UASView* newView = new UASView(uas, newBox);
             uasViews.insert(uas, newView);
+            uasToBoxMapping[uas] = newBox;
             newBox->layout()->addWidget(newView);
 
             // Watch for when this widget is destroyed so that we can clean up the
@@ -148,30 +155,49 @@ void UASListWidget::activeUAS(UASInterface* uas)
  */
 void UASListWidget::removeUAS(UASInterface* uas)
 {
-    // Remove the UAS from our data structures and
-    // the global listing.
+    // Remove the UASView and check if its parent GroupBox has any other children,
+    // delete it if it doesn't.
+    QGroupBox* box = uasToBoxMapping[uas];
+    uasToBoxMapping.remove(uas);
     uasViews.remove(uas);
-
-    // Check all groupboxes for all links this uas had and check if they're empty.
-    // Delete them if they are.
-    QListIterator<LinkInterface*> i = *uas->getLinks();
-    while (i.hasNext())
+    int otherViews = 0;
+    foreach (UASView* view, box->findChildren<UASView*>())
     {
-        LinkInterface* link = i.next();
+        if (view->uas == uas)
+        {
+            view->deleteLater();
+        }
+        else
+        {
+            ++otherViews;
+        }
+    }
+    if (otherViews == 0)
+    {
+        // Delete the groupbox.
+        QMap<LinkInterface*, QGroupBox*>::const_iterator i = linkToBoxMapping.constBegin();
+        while (i != linkToBoxMapping.constEnd()) {
+            if (i.value() == box)
+            {
+                linkToBoxMapping.remove(i.key());
+            }
+            ++i;
+        }
+        box->deleteLater();
 
-        QGroupBox* box = linkToBoxMapping[link];
-        if (box)
+        // And if no other QGroupBoxes are left, put the initial widget back.
+        int otherBoxes = 0;
+        foreach (const QGroupBox* otherBox, findChildren<QGroupBox*>())
         {
-            // If this was the last UAS in the GroupBox, remove it and its corresponding link.
-            int views = box->findChildren<UASView*>().size();
-            if (views == 0) {
-                box->deleteLater();
-                linkToBoxMapping.remove(link);
+            if (otherBox != box)
+            {
+                ++otherBoxes;
             }
         }
+        if (otherBoxes == 0)
+        {
+            uWidget = new QGCUnconnectedInfoWidget(this);
+            m_ui->verticalLayout->addWidget(uWidget);
+        }
     }
-
-    // And if no QGroupBoxes are left, put the initial widget back.
-    uWidget = new QGCUnconnectedInfoWidget(this);
-    m_ui->verticalLayout->addWidget(uWidget);
 }
diff --git a/src/ui/uas/UASListWidget.h b/src/ui/uas/UASListWidget.h
index 073ccc9acd46a02734a1d2f2a4b5a268651f5caf..4cd3a587b4ee720d3ce519753abbfcc2db857271 100644
--- a/src/ui/uas/UASListWidget.h
+++ b/src/ui/uas/UASListWidget.h
@@ -33,6 +33,7 @@ This file is part of the QGROUNDCONTROL project
 
 #include <QWidget>
 #include <QMap>
+#include <QList>
 #include <QVBoxLayout>
 #include <QGroupBox>
 #include "UASInterface.h"
@@ -54,9 +55,11 @@ public slots:
     void removeUAS(UASInterface* uas);
 
 protected:
+    // Keep a mapping from UASes to their GroupBox. Useful for determining when groupboxes are empty.
+    QMap<UASInterface*, QGroupBox*> uasToBoxMapping;
     // Keep a mapping from Links to GroupBoxes for adding new links.
     QMap<LinkInterface*, QGroupBox*> linkToBoxMapping;
-    // Tie each view to their UAS object.
+    // Tie each view to their UAS object so they can be removed easily.
     QMap<UASInterface*, UASView*> uasViews;
     QGCUnconnectedInfoWidget* uWidget;
     void changeEvent(QEvent *e);
diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc
index ff597984aca9ffa5203ae0dc4361bf44f9c6bae1..5d8e97f6419fd909a3a1d57d02adbe3dd7a9c308 100644
--- a/src/ui/uas/UASView.cc
+++ b/src/ui/uas/UASView.cc
@@ -43,48 +43,48 @@ This file is part of the PIXHAWK project
 #include <QGCHilFlightGearConfiguration.h>
 
 UASView::UASView(UASInterface* uas, QWidget *parent) :
-        QWidget(parent),
-        startTime(0),
-        timeout(false),
-        iconIsRed(true),
-        disconnected(false),
-        timeRemaining(0),
-        chargeLevel(0),
-        uas(uas),
-        load(0),
-        state("UNKNOWN"),
-        stateDesc(tr("Unknown state")),
-        mode("MAV_MODE_UNKNOWN"),
-        thrust(0),
-        isActive(false),
-        x(0),
-        y(0),
-        z(0),
-        totalSpeed(0),
-        lat(0),
-        lon(0),
-        alt(0),
-        groundDistance(0),
-        localFrame(false),
-        globalFrameKnown(false),
-        removeAction(new QAction("Delete this system", this)),
-        renameAction(new QAction("Rename..", this)),
-        selectAction(new QAction("Control this system", this )),
-        hilAction(new QAction("HIL - Hardware in the Loop", this )),
-        selectAirframeAction(new QAction("Choose Airframe", this)),
-        setBatterySpecsAction(new QAction("Set Battery Options", this)),
-        lowPowerModeEnabled(true),
-        generalUpdateCount(0),
-        filterTime(0),
-        m_ui(new Ui::UASView)
+    QWidget(parent),
+    uas(uas),
+    startTime(0),
+    timeout(false),
+    iconIsRed(true),
+    disconnected(false),
+    timeRemaining(0),
+    chargeLevel(0),
+    load(0),
+    state("UNKNOWN"),
+    stateDesc(tr("Unknown state")),
+    mode("MAV_MODE_UNKNOWN"),
+    thrust(0),
+    isActive(false),
+    x(0),
+    y(0),
+    z(0),
+    totalSpeed(0),
+    lat(0),
+    lon(0),
+    alt(0),
+    groundDistance(0),
+    localFrame(false),
+    globalFrameKnown(false),
+    removeAction(new QAction(tr("Delete this system"), this)),
+    renameAction(new QAction(tr("Rename.."), this)),
+    selectAction(new QAction(tr("Control this system"), this)),
+    hilAction(new QAction(tr("HIL - Hardware in the Loop"), this)),
+    selectAirframeAction(new QAction(tr("Choose Airframe"), this)),
+    setBatterySpecsAction(new QAction(tr("Set Battery Options"), this)),
+    lowPowerModeEnabled(true),
+    generalUpdateCount(0),
+    filterTime(0),
+    m_ui(new Ui::UASView)
 {
+    m_ui->setupUi(this);
+
     // FIXME XXX
     lowPowerModeEnabled = MainWindow::instance()->lowPowerModeEnabled();
 
     hilAction->setCheckable(true);
 
-    m_ui->setupUi(this);
-
     // Setup communication
     //connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(receiveValue(int,QString,double,quint64)));
     connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, double, int)));
@@ -116,8 +116,8 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
     connect(m_ui->killButton, SIGNAL(clicked()), uas, SLOT(emergencyKILL()));
     connect(m_ui->shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown()));
 
-    // Allow to delete this widget
-    connect(removeAction, SIGNAL(triggered()), this, SLOT(prepareForDeletion()));
+    // Allow deleting this widget
+    connect(removeAction, SIGNAL(triggered()), this, SLOT(triggerUASDeletion()));
     connect(renameAction, SIGNAL(triggered()), this, SLOT(rename()));
     connect(selectAction, SIGNAL(triggered()), uas, SLOT(setSelected()));
     connect(hilAction, SIGNAL(triggered(bool)), this, SLOT(showHILUi()));
@@ -170,9 +170,6 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
 UASView::~UASView()
 {
     delete m_ui;
-    delete removeAction;
-    delete renameAction;
-    delete selectAction;
 }
 
 void UASView::heartbeatTimeout(bool timeout, unsigned int ms)
@@ -562,23 +559,14 @@ void UASView::showHILUi()
      MainWindow::instance()->showHILConfigurationWidget(uas);
 }
 
-/**
- * @brief Stop updating this UASView, queue it for deletion, and also tell the UASManager to delete the UAS.
- */
-void UASView::prepareForDeletion()
+void UASView::triggerUASDeletion()
 {
     refreshTimer->stop();
     UASManager::instance()->removeUAS(uas);
-    deleteLater();
 }
 
 void UASView::refresh()
 {
-    //setUpdatesEnabled(false);
-    //setUpdatesEnabled(true);
-    //repaint();
-    //qDebug() << "UPDATING UAS WIDGET!" << uas->getUASName();
-
     if (generalUpdateCount == 4)
     {
 #if (QGC_EVENTLOOP_DEBUG)
diff --git a/src/ui/uas/UASView.h b/src/ui/uas/UASView.h
index 9af55a9671fc9c58cbe4f9a2755912a6097c8c2f..70eb11202d6ea7310c0c09cf844a5c1164d74218 100644
--- a/src/ui/uas/UASView.h
+++ b/src/ui/uas/UASView.h
@@ -49,6 +49,7 @@ class UASView : public QWidget
 public:
     UASView(UASInterface* uas, QWidget *parent = 0);
     ~UASView();
+    UASInterface* uas;
 
 public slots:
     /** @brief Update the name of the system */
@@ -65,8 +66,11 @@ public slots:
     void updateLoad(UASInterface* uas, double load);
     //void receiveValue(int uasid, QString id, double value, quint64 time);
     void showHILUi();
-    /** @brief Disables the widget from accessing the UAS object in preparation for being deleted */
-    void prepareForDeletion();
+    /**
+     * Request that the UASManager deletes this UAS. This doesn't delete this widget
+     * yet, it waits for the approprait uasDeleted signal.
+     */
+    void triggerUASDeletion();
     void refresh();
     /** @brief Receive new waypoint information */
     void setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current);
@@ -103,7 +107,6 @@ protected:
     bool disconnected;
     int timeRemaining;
     float chargeLevel;
-    UASInterface* uas;
     float load;
     QString state;
     QString stateDesc;