diff --git a/.github/workflows/sitl_tests.yml b/.github/workflows/sitl_tests.yml
index a2ef089be8..22e58267d3 100644
--- a/.github/workflows/sitl_tests.yml
+++ b/.github/workflows/sitl_tests.yml
@@ -16,4 +16,5 @@ jobs:
       # Build all targets
       run: DONT_RUN=1 make px4_sitl gazebo mavsdk_tests
     - name: Run simulation tests
-      run:  test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 100
+      # Grind configuration: Run ~8 minutes for 35 iterations == close to the 6 hour limit of Github Actions
+      run:  test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --iterations 1 --fail-early
diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt
index 1695d2a2eb..925d4e7f6e 100644
--- a/test/mavsdk_tests/CMakeLists.txt
+++ b/test/mavsdk_tests/CMakeLists.txt
@@ -1,21 +1,25 @@
 cmake_minimum_required(VERSION 3.5.1)
 
-find_package(MAVSDK)
+if(${PX4_BOARD_MODEL} MATCHES "sitl")
+    find_package(MAVSDK QUIET)
 
-if (MAVSDK_FOUND)
-    add_executable(mavsdk_tests
-        test_main.cpp
-        autopilot_tester.cpp
-        test_mission_multicopter.cpp
-    )
+    if (MAVSDK_FOUND)
+        add_executable(mavsdk_tests
+            test_main.cpp
+            autopilot_tester.cpp
+            test_mission_multicopter.cpp
+        )
 
-    target_link_libraries(mavsdk_tests
-        MAVSDK::mavsdk
-        MAVSDK::mavsdk_action
-        MAVSDK::mavsdk_mission
-        MAVSDK::mavsdk_telemetry
-    )
+        target_link_libraries(mavsdk_tests
+            MAVSDK::mavsdk
+            MAVSDK::mavsdk_action
+            MAVSDK::mavsdk_mission
+            MAVSDK::mavsdk_telemetry
+        )
 
-    target_compile_options(mavsdk_tests
-        PRIVATE -std=c++17 -Wall -Wextra -Werror)
+        target_compile_options(mavsdk_tests
+            PRIVATE -std=c++17 -Wall -Wextra -Werror)
+    else()
+        message("MAVSDK C++ not found, skipping mavsdk_tests build..")
+    endif()
 endif()
diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py
index 43d9d2ff6c..981247ce49 100755
--- a/test/mavsdk_tests/mavsdk_test_runner.py
+++ b/test/mavsdk_tests/mavsdk_test_runner.py
@@ -243,12 +243,6 @@ def main():
 
                 returncode = test_runner.wait(group['timeout_min'])
                 was_success = (returncode == 0)
-                print("Test '{}': {}".
-                    format(test, "Success" if was_success else "Fail"))
-                if not was_success:
-                    overall_success = False
-                    if args.fail_early:
-                        break
 
                 if args.gui:
                     returncode = gzclient_runner.stop()
@@ -260,7 +254,16 @@ def main():
                 px4_runner.stop()
                 print("px4 exited with {}".format(returncode))
 
-        if not overall_success and x > 0:
+                # 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)