diff --git a/mavsdk_tests/CMakeLists.txt b/mavsdk_tests/CMakeLists.txt
index 144932dd7e..903e125b1a 100644
--- a/mavsdk_tests/CMakeLists.txt
+++ b/mavsdk_tests/CMakeLists.txt
@@ -7,6 +7,7 @@ add_definitions("-std=c++17 -Wall -Wextra -Werror")
 find_package(MAVSDK REQUIRED)
 
 add_executable(test_mission_multicopter
+    test_main.cpp
     autopilot_tester.cpp
     test_mission_multicopter.cpp
 )
diff --git a/mavsdk_tests/autopilot_tester.cpp b/mavsdk_tests/autopilot_tester.cpp
index 1b9bcf61f7..12ad2eb7d4 100644
--- a/mavsdk_tests/autopilot_tester.cpp
+++ b/mavsdk_tests/autopilot_tester.cpp
@@ -5,6 +5,8 @@
 using namespace mavsdk;
 using namespace mavsdk::geometry;
 
+std::string connection_url {"udp://"};
+
 void AutopilotTester::connect(const std::string uri)
 {
     ConnectionResult ret = _mavsdk.add_any_connection(uri);
diff --git a/mavsdk_tests/autopilot_tester.h b/mavsdk_tests/autopilot_tester.h
index 7213b02496..c029e52599 100644
--- a/mavsdk_tests/autopilot_tester.h
+++ b/mavsdk_tests/autopilot_tester.h
@@ -10,6 +10,8 @@
 #include <memory>
 #include <thread>
 
+extern std::string connection_url;
+
 class AutopilotTester {
 public:
     struct MissionOptions {
diff --git a/mavsdk_tests/test_main.cpp b/mavsdk_tests/test_main.cpp
new file mode 100644
index 0000000000..cabd433626
--- /dev/null
+++ b/mavsdk_tests/test_main.cpp
@@ -0,0 +1,66 @@
+//
+// Multicopter mission test.
+//
+// Author: Julian Oes <julian@oes.ch>
+
+#define CATCH_CONFIG_RUNNER
+#include "catch2/catch.hpp"
+
+#include <mavsdk/mavsdk.h>
+#include <iostream>
+#include <string>
+#include "autopilot_tester.h"
+
+
+static void usage(const std::string& bin_name);
+static void remove_argv(int& argc, char** argv, int pos);
+
+int main(int argc, char** argv)
+{
+    for (int i = 0; i < argc; ++i) {
+        const std::string argv_string(argv[i]);
+
+        if (argv_string == "-h") {
+            usage(argv[0]);
+        }
+
+        if (argv_string == "--url") {
+            if (argc > i + 1) {
+                connection_url = argv[i+1];
+                remove_argv(argc, argv, i);
+                remove_argv(argc, argv, i);
+            } else {
+                std::cerr << "No connection URL supplied" << std::endl;
+                usage(argv[0]);
+                return -1;
+            }
+        }
+    }
+
+    Catch::Session session;
+    const int catch_ret = session.applyCommandLine(argc, argv);
+    if (catch_ret != 0) {
+        return catch_ret;
+    }
+    return session.run();
+}
+
+void usage(const std::string& bin_name)
+{
+    std::cout << std::endl
+              << "Usage : " << bin_name << " [--url CONNECTION_URL] [catch2 arguments]" << std::endl
+              << "Connection URL format should be :" << std::endl
+              << " For TCP : tcp://[server_host][:server_port]" << std::endl
+              << " For UDP : udp://[bind_host][:bind_port]" << std::endl
+              << " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl
+              << "For example, to connect to the simulator use URL: udp://:14540" << std::endl
+              << std::endl;
+}
+
+void remove_argv(int& argc, char** argv, int pos)
+{
+    for (int i = pos; i+1 < argc; ++i) {
+        argv[i] = argv[i+1];
+    }
+    argv[--argc] = nullptr;
+}
\ No newline at end of file
diff --git a/mavsdk_tests/test_mission_multicopter.cpp b/mavsdk_tests/test_mission_multicopter.cpp
index 35ca216fbd..8a27935ffc 100644
--- a/mavsdk_tests/test_mission_multicopter.cpp
+++ b/mavsdk_tests/test_mission_multicopter.cpp
@@ -3,9 +3,6 @@
 //
 // Author: Julian Oes <julian@oes.ch>
 
-#define CATCH_CONFIG_RUNNER
-#include "catch2/catch.hpp"
-
 #include <mavsdk/mavsdk.h>
 #include <mavsdk/plugins/action/action.h>
 #include <mavsdk/plugins/telemetry/telemetry.h>
@@ -13,62 +10,6 @@
 #include <string>
 #include "autopilot_tester.h"
 
-using namespace mavsdk;
-
-static std::string connection_url{"udp://"};
-
-static void usage(const std::string& bin_name);
-static void remove_argv(int& argc, char** argv, int pos);
-
-int main(int argc, char** argv)
-{
-    for (int i = 0; i < argc; ++i) {
-        const std::string argv_string(argv[i]);
-
-        if (argv_string == "-h") {
-            usage(argv[0]);
-        }
-
-        if (argv_string == "--url") {
-            if (argc > i + 1) {
-                connection_url = argv[i+1];
-                remove_argv(argc, argv, i);
-                remove_argv(argc, argv, i);
-            } else {
-                std::cerr << "No connection URL supplied" << std::endl;
-                usage(argv[0]);
-                return -1;
-            }
-        }
-    }
-
-    Catch::Session session;
-    const int catch_ret = session.applyCommandLine(argc, argv);
-    if (catch_ret != 0) {
-        return catch_ret;
-    }
-    return session.run();
-}
-
-void usage(const std::string& bin_name)
-{
-    std::cout << std::endl
-              << "Usage : " << bin_name << " [--url CONNECTION_URL] [catch2 arguments]" << std::endl
-              << "Connection URL format should be :" << std::endl
-              << " For TCP : tcp://[server_host][:server_port]" << std::endl
-              << " For UDP : udp://[bind_host][:bind_port]" << std::endl
-              << " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl
-              << "For example, to connect to the simulator use URL: udp://:14540" << std::endl
-              << std::endl;
-}
-
-void remove_argv(int& argc, char** argv, int pos)
-{
-    for (int i = pos; i+1 < argc; ++i) {
-        argv[i] = argv[i+1];
-    }
-    argv[--argc] = nullptr;
-}
 
 TEST_CASE("We can takeoff and land", "[multicopter]")
 {