diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp
index 1b7c6a366c..9b4295a2f8 100644
--- a/test/mavsdk_tests/autopilot_tester.cpp
+++ b/test/mavsdk_tests/autopilot_tester.cpp
@@ -159,6 +159,18 @@ void AutopilotTester::set_takeoff_altitude(const float altitude_m)
 	CHECK(result.second == Approx(altitude_m));
 }
 
+void AutopilotTester::set_height_source(AutopilotTester::HeightSource height_source)
+{
+	switch (height_source) {
+	case HeightSource::Baro:
+		CHECK(_param->set_param_int("EKF2_HGT_MODE", 0) == Param::Result::Success);
+		break;
+
+	case HeightSource::Gps:
+		CHECK(_param->set_param_int("EKF2_HGT_MODE", 1) == Param::Result::Success);
+	}
+}
+
 void AutopilotTester::arm()
 {
 	const auto result = _action->arm();
diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h
index 6b45e14f77..7b781ca93f 100644
--- a/test/mavsdk_tests/autopilot_tester.h
+++ b/test/mavsdk_tests/autopilot_tester.h
@@ -61,12 +61,18 @@ public:
 		bool fly_through {false};
 	};
 
+	enum class HeightSource {
+		Baro,
+		Gps
+	};
+
 	void connect(const std::string uri);
 	void wait_until_ready();
 	void wait_until_ready_local_position_only();
 	void store_home();
 	void check_home_within(float acceptance_radius_m);
 	void set_takeoff_altitude(const float altitude_m);
+	void set_height_source(HeightSource height_source);
 	void arm();
 	void takeoff();
 	void land();
diff --git a/test/mavsdk_tests/test_multicopter_failsafe.cpp b/test/mavsdk_tests/test_multicopter_failsafe.cpp
index 4fafc0fe35..09d1a70f98 100644
--- a/test/mavsdk_tests/test_multicopter_failsafe.cpp
+++ b/test/mavsdk_tests/test_multicopter_failsafe.cpp
@@ -39,10 +39,28 @@
 #include "autopilot_tester.h"
 
 
-TEST_CASE("Land on GPS lost during mission", "[multicopter][vtol]")
+TEST_CASE("Land on GPS lost during mission (baro height mode)", "[multicopter][vtol]")
 {
 	AutopilotTester tester;
 	tester.connect(connection_url);
+
+	tester.set_height_source(AutopilotTester::HeightSource::Baro);
+	tester.wait_until_ready();
+
+	AutopilotTester::MissionOptions mission_options;
+	mission_options.rtl_at_end = true;
+	tester.prepare_square_mission(mission_options);
+	tester.arm();
+	tester.execute_mission_and_lose_gps();
+	tester.wait_until_disarmed();
+}
+
+TEST_CASE("Land on GPS lost during mission (GPS height mode)", "[multicopter][vtol]")
+{
+	AutopilotTester tester;
+	tester.connect(connection_url);
+
+	tester.set_height_source(AutopilotTester::HeightSource::Gps);
 	tester.wait_until_ready();
 
 	AutopilotTester::MissionOptions mission_options;
@@ -81,10 +99,11 @@ TEST_CASE("Continue on mag stuck during mission", "[multicopter][vtol]")
 	tester.wait_until_disarmed();
 }
 
-TEST_CASE("Continue on baro lost during mission", "[multicopter][vtol]")
+TEST_CASE("Continue on baro lost during mission (baro height mode)", "[multicopter][vtol]")
 {
 	AutopilotTester tester;
 	tester.connect(connection_url);
+	tester.set_height_source(AutopilotTester::HeightSource::Baro);
 	tester.wait_until_ready();
 
 	AutopilotTester::MissionOptions mission_options;
@@ -95,10 +114,41 @@ TEST_CASE("Continue on baro lost during mission", "[multicopter][vtol]")
 	tester.wait_until_disarmed();
 }
 
-TEST_CASE("Continue on baro stuck during mission", "[multicopter][vtol]")
+TEST_CASE("Continue on baro lost during mission (GPS height mode)", "[multicopter][vtol]")
 {
 	AutopilotTester tester;
 	tester.connect(connection_url);
+	tester.set_height_source(AutopilotTester::HeightSource::Gps);
+	tester.wait_until_ready();
+
+	AutopilotTester::MissionOptions mission_options;
+	mission_options.rtl_at_end = true;
+	tester.prepare_square_mission(mission_options);
+	tester.arm();
+	tester.execute_mission_and_lose_baro();
+	tester.wait_until_disarmed();
+}
+
+TEST_CASE("Continue on baro stuck during mission (baro height mode)", "[multicopter][vtol]")
+{
+	AutopilotTester tester;
+	tester.connect(connection_url);
+	tester.set_height_source(AutopilotTester::HeightSource::Baro);
+	tester.wait_until_ready();
+
+	AutopilotTester::MissionOptions mission_options;
+	mission_options.rtl_at_end = true;
+	tester.prepare_square_mission(mission_options);
+	tester.arm();
+	tester.execute_mission_and_get_baro_stuck();
+	tester.wait_until_disarmed();
+}
+
+TEST_CASE("Continue on baro stuck during mission (GPS height mode)", "[multicopter][vtol]")
+{
+	AutopilotTester tester;
+	tester.connect(connection_url);
+	tester.set_height_source(AutopilotTester::HeightSource::Gps);
 	tester.wait_until_ready();
 
 	AutopilotTester::MissionOptions mission_options;