diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py
index d5c31f9e3f..792f81e84a 100755
--- a/test/mavsdk_tests/mavsdk_test_runner.py
+++ b/test/mavsdk_tests/mavsdk_test_runner.py
@@ -150,6 +150,27 @@ class TestRunner(Runner):
         self.log_prefix = "test_runner"
 
 
+def main():
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--log-dir",
+                        help="Directory for log files, stdout if not provided")
+    parser.add_argument("--speed-factor", default=1,
+                        help="How fast to run the simulation")
+    parser.add_argument("--iterations", type=int, default=1,
+                        help="How often to run all tests")
+    parser.add_argument("--fail-early", action='store_true',
+                        help="Abort on first unsuccessful test")
+    parser.add_argument("--gui", default=False, action='store_true',
+                        help="Display gzclient with simulation")
+    args = parser.parse_args()
+
+    if not is_everything_ready():
+        sys.exit(1)
+
+    run(args)
+
+
 def determine_tests(workspace_dir, filter):
     cmd = workspace_dir + "/build/px4_sitl_default/mavsdk_tests/mavsdk_tests"
     args = ["--list-test-names-only", filter]
@@ -196,90 +217,84 @@ def is_everything_ready():
     return result
 
 
-def main():
-
-    parser = argparse.ArgumentParser()
-    parser.add_argument("--log-dir",
-                        help="Directory for log files, stdout if not provided")
-    parser.add_argument("--speed-factor", default=1,
-                        help="How fast to run the simulation")
-    parser.add_argument("--iterations", type=int, default=1,
-                        help="How often to run the simulation")
-    parser.add_argument("--fail-early", action='store_true',
-                        help="Abort on first failure")
-    parser.add_argument("--gui", default=False, action='store_true',
-                        help="Display gzclient with")
-    args = parser.parse_args()
-
-    if not is_everything_ready():
-        sys.exit(1)
-
+def run(args):
     overall_success = True
 
-    for x in range(args.iterations):
-        print("Test iterations: %d" % (x + 1))
-        for group in test_matrix:
-            print("Running test group for '{}' with filter '{}'"
-                  .format(group['model'], group['test_filter']))
+    for iteration in range(args.iterations):
+        if args.iterations > 1:
+            print("Test iteration: {}".format(iteration + 1, args.iterations))
 
-            tests = determine_tests(os.getcwd(), group['test_filter'])
+        was_success = run_test_group(args)
 
-            for test in tests:
+        if not was_success:
+            overall_success = False
 
-                print("Running test '{}'".format(test))
-
-                px4_runner = Px4Runner(
-                    os.getcwd(), args.log_dir, args.speed_factor)
-                px4_runner.start(group)
-
-                gzserver_runner = GzserverRunner(
-                    os.getcwd(), args.log_dir, args.speed_factor)
-                gzserver_runner.start(group)
-
-                if args.gui:
-                    gzclient_runner = GzclientRunner(
-                        os.getcwd(), args.log_dir)
-                    gzclient_runner.start(group)
-
-                test_runner = TestRunner(
-                    os.getcwd(), args.log_dir, group, test)
-                test_runner.start(group)
-
-                returncode = test_runner.wait(group['timeout_min'])
-                was_success = (returncode == 0)
-
-                if args.gui:
-                    returncode = gzclient_runner.stop()
-                    print("gzclient exited with {}".format(returncode))
-
-                returncode = gzserver_runner.stop()
-                print("gzserver exited with {}".format(returncode))
-
-                px4_runner.stop()
-                print("px4 exited with {}".format(returncode))
-
-                # Test run results
-                print("Test '{}': {}".
-                      format(test, "Success" if was_success else "Fail"))
-
-                # Flag it as group test failure, but finish the rest of the
-                # test targets.
-                if not was_success:
-                    overall_success = False
-
-        # Abort after the full matrix / test group
-        if not overall_success and x > 0 and args.fail_early:
-            print("Aborting with a failure in test run %d" % (x + 1))
-            sys.exit(0 if overall_success else 1)
+        if args.iterations > 1 and not was_success and args.fail_early:
+            print("Aborting with a failure in test run {}/{}".
+                  format(iteration + 1, args.iterations))
+            break
 
     print("Overall result: {}".
           format("SUCCESS" if overall_success else "FAIL"))
-
-    if x > 0:
-        print("Test iterations: %d" % (x + 1))
-
     sys.exit(0 if overall_success else 1)
 
 
+def run_test_group(args):
+    overall_success = True
+    for group in test_matrix:
+        print("Running test group for '{}' with filter '{}'"
+              .format(group['model'], group['test_filter']))
+
+        tests = determine_tests(os.getcwd(), group['test_filter'])
+
+        for test in tests:
+            print("Running test '{}'".format(test))
+            was_success = run_test(test, group, args)
+            print("Test '{}': {}".
+                  format(test, "Success" if was_success else "Fail"))
+
+            if not was_success:
+                overall_success = False
+
+            if not was_success and args.fail_early:
+                print("Aborting early")
+                return False
+
+    return overall_success
+
+
+def run_test(test, group, args):
+    px4_runner = Px4Runner(
+        os.getcwd(), args.log_dir, args.speed_factor)
+    px4_runner.start(group)
+
+    gzserver_runner = GzserverRunner(
+        os.getcwd(), args.log_dir, args.speed_factor)
+    gzserver_runner.start(group)
+
+    if args.gui:
+        gzclient_runner = GzclientRunner(
+            os.getcwd(), args.log_dir)
+        gzclient_runner.start(group)
+
+    test_runner = TestRunner(os.getcwd(), args.log_dir, group, test)
+    test_runner.start(group)
+
+    returncode = test_runner.wait(group['timeout_min'])
+    is_success = (returncode == 0)
+
+    if args.gui:
+        returncode = gzclient_runner.stop()
+        print("gzclient exited with {}".format(returncode))
+
+    returncode = gzserver_runner.stop()
+    print("gzserver exited with {}".format(returncode))
+
+    px4_runner.stop()
+    print("px4 exited with {}".format(returncode))
+
+    return is_success
+
+
 if __name__ == '__main__':
     main()