diff --git a/src/qgcunittest/UnitTest.cc b/src/qgcunittest/UnitTest.cc
index b3f5b404c4cd9846ae96212c8eee14a9f04afd60..32165cf33d3f0103990ebdb439754edacdd8c70c 100644
--- a/src/qgcunittest/UnitTest.cc
+++ b/src/qgcunittest/UnitTest.cc
@@ -30,6 +30,7 @@
 #include "QGCApplication.h"
 #include "MAVLinkProtocol.h"
 #include "MainWindow.h"
+#include "Vehicle.h"
 
 bool UnitTest::_messageBoxRespondedTo = false;
 bool UnitTest::_badResponseButton = false;
@@ -46,6 +47,7 @@ UnitTest::UnitTest(void)
     : _linkManager(NULL)
     , _mockLink(NULL)
     , _mainWindow(NULL)
+    , _vehicle(NULL)
     , _expectMissedFileDialog(false)
     , _expectMissedMessageBox(false)
     , _unitTestRun(false)
@@ -110,6 +112,8 @@ void UnitTest::init(void)
         _linkManager = qgcApp()->toolbox()->linkManager();
         connect(_linkManager, &LinkManager::linkDeleted, this, &UnitTest::_linkDeleted);
     }
+
+    _linkManager->restart();
     
     _messageBoxRespondedTo = false;
     _missedMessageBoxCount = 0;
@@ -369,17 +373,27 @@ void UnitTest::_connectMockLink(MAV_AUTOPILOT autopilot)
 {
     Q_ASSERT(!_mockLink);
 
-    _mockLink = new MockLink();
-    _mockLink->setFirmwareType(autopilot);
-
-    _linkManager->_addLink(_mockLink);
-    _linkManager->connectLink(_mockLink);
+    switch (autopilot) {
+    case MAV_AUTOPILOT_PX4:
+        _mockLink = MockLink::startPX4MockLink(false);
+        break;
+    case MAV_AUTOPILOT_ARDUPILOTMEGA:
+        _mockLink = MockLink::startAPMArduCopterMockLink(false);
+        break;
+    case MAV_AUTOPILOT_GENERIC:
+        _mockLink = MockLink::startGenericMockLink(false);
+        break;
+    default:
+        qWarning() << "Type not supported";
+        break;
+    }
 
     // Wait for the Vehicle to get created
     QSignalSpy spyVehicle(qgcApp()->toolbox()->multiVehicleManager(), SIGNAL(parameterReadyVehicleAvailableChanged(bool)));
     QCOMPARE(spyVehicle.wait(5000), true);
     QVERIFY(qgcApp()->toolbox()->multiVehicleManager()->parameterReadyVehicleAvailable());
-    QVERIFY(qgcApp()->toolbox()->multiVehicleManager()->activeVehicle());
+    _vehicle = qgcApp()->toolbox()->multiVehicleManager()->activeVehicle();
+    QVERIFY(_vehicle);
 }
 
 void UnitTest::_disconnectMockLink(void)
@@ -392,6 +406,8 @@ void UnitTest::_disconnectMockLink(void)
         // Wait for link to go away
         linkSpy.wait(1000);
         QCOMPARE(linkSpy.count(), 1);
+
+        _vehicle = NULL;
     }
 }
 
diff --git a/src/qgcunittest/UnitTest.h b/src/qgcunittest/UnitTest.h
index 5326872e41d2e73900e00ef254e99e40d1bcd3cd..9a0893b926e79e1268a261ca45fa2f864bdf1efb 100644
--- a/src/qgcunittest/UnitTest.h
+++ b/src/qgcunittest/UnitTest.h
@@ -45,6 +45,7 @@ class QGCFileDialog;
 class LinkManager;
 class MockLink;
 class MainWindow;
+class Vehicle;
 
 class UnitTest : public QObject
 {
@@ -115,6 +116,7 @@ protected:
     LinkManager*    _linkManager;
     MockLink*       _mockLink;
     MainWindow*     _mainWindow;
+    Vehicle*        _vehicle;
 
     bool _expectMissedFileDialog;   // true: expect a missed file dialog, used for internal testing
     bool _expectMissedMessageBox;   // true: expect a missed message box, used for internal testing
diff --git a/src/qgcunittest/UnitTestList.cc b/src/qgcunittest/UnitTestList.cc
index 6a71c97e7da5cda67ac8d50278cccf8a2592e76e..e0bd09e0394c65e008bb694eb4169758a50e3689 100644
--- a/src/qgcunittest/UnitTestList.cc
+++ b/src/qgcunittest/UnitTestList.cc
@@ -33,10 +33,11 @@
 #include "MessageBoxTest.h"
 #include "MissionItemTest.h"
 #include "MissionControllerTest.h"
-#include "PX4RCCalibrationTest.h"
+#include "RadioConfigTest.h"
 #include "SetupViewTest.h"
 #include "MavlinkLogTest.h"
 
+UT_REGISTER_TEST(SetupViewTest)
 UT_REGISTER_TEST(FactSystemTestGeneric)
 UT_REGISTER_TEST(FactSystemTestPX4)
 UT_REGISTER_TEST(FileDialogTest)
@@ -48,7 +49,6 @@ UT_REGISTER_TEST(MessageBoxTest)
 UT_REGISTER_TEST(MissionItemTest)
 UT_REGISTER_TEST(MissionControllerTest)
 UT_REGISTER_TEST(RadioConfigTest)
-UT_REGISTER_TEST(SetupViewTest)
 
 // List of unit test which are currently disabled.
 // If disabling a new test, include reason in comment.