diff --git a/mavsdk_tests/autopilot_tester.cpp b/mavsdk_tests/autopilot_tester.cpp
index fe4a46d912..4a0a2fa241 100644
--- a/mavsdk_tests/autopilot_tester.cpp
+++ b/mavsdk_tests/autopilot_tester.cpp
@@ -127,4 +127,9 @@ std::shared_ptr<MissionItem>  AutopilotTester::_create_mission_item(
     mission_item->set_position(pos_north.latitude_deg, pos_north.longitude_deg);
     mission_item->set_relative_altitude(mission_options.relative_altitude_m);
     return mission_item;
+}
+
+void AutopilotTester::execute_rtl()
+{
+    REQUIRE(Action::Result::SUCCESS == _action->return_to_launch());
 }
\ No newline at end of file
diff --git a/mavsdk_tests/autopilot_tester.h b/mavsdk_tests/autopilot_tester.h
index c029e52599..6418b95072 100644
--- a/mavsdk_tests/autopilot_tester.h
+++ b/mavsdk_tests/autopilot_tester.h
@@ -30,6 +30,7 @@ public:
     void wait_until_hovering();
     void prepare_square_mission(MissionOptions mission_options);
     void execute_mission();
+    void execute_rtl();
 
 private:
     mavsdk::geometry::CoordinateTransformation _get_coordinate_transformation();
diff --git a/mavsdk_tests/test_mission_multicopter.cpp b/mavsdk_tests/test_mission_multicopter.cpp
index f5ee5e7584..562f8c1a2e 100644
--- a/mavsdk_tests/test_mission_multicopter.cpp
+++ b/mavsdk_tests/test_mission_multicopter.cpp
@@ -11,7 +11,7 @@
 #include "autopilot_tester.h"
 
 
-TEST_CASE("We can takeoff and land", "[multicopter]")
+TEST_CASE("Takeoff and land", "[multicopter]")
 {
     AutopilotTester tester;
     tester.connect(connection_url);
@@ -23,17 +23,29 @@ TEST_CASE("We can takeoff and land", "[multicopter]")
     tester.wait_until_disarmed();
 }
 
-TEST_CASE("We can fly a square mission and do RTL", "[multicopter]")
+TEST_CASE("Fly a square missions", "[multicopter]")
 {
     AutopilotTester tester;
     tester.connect(connection_url);
     tester.wait_until_ready();
 
-    AutopilotTester::MissionOptions mission_options;
-    mission_options.rtl_at_end = true;
-    tester.prepare_square_mission(mission_options);
+    SECTION("Mission including RTL") {
+        AutopilotTester::MissionOptions mission_options;
+        mission_options.rtl_at_end = true;
+        tester.prepare_square_mission(mission_options);
+        tester.arm();
+        tester.execute_mission();
+        tester.wait_until_disarmed();
+    }
 
-    tester.arm();
-    tester.execute_mission();
-    tester.wait_until_disarmed();
+    SECTION("Mission with manual RTL") {
+        AutopilotTester::MissionOptions mission_options;
+        mission_options.rtl_at_end = false;
+        tester.prepare_square_mission(mission_options);
+        tester.arm();
+        tester.execute_mission();
+        tester.wait_until_hovering();
+        tester.execute_rtl();
+        tester.wait_until_disarmed();
+    }
 }
\ No newline at end of file