mavsdk_tests: check for stuck baro

src/modules/simulator/simulator.h

@@ -257,6 +257,7 @@ private:
 	bool _accel_blocked{false};
 	bool _gyro_blocked{false};
 	bool _baro_blocked{false};
+	bool _baro_stuck{false};
 	bool _mag_blocked{false};
 	bool _gps_blocked{false};
 	bool _airspeed_blocked{false};

src/modules/simulator/simulator_mavlink.cpp

@@ -253,8 +253,14 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
 
 	// baro
 	if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_baro_blocked) {
-		_px4_baro_0.update(time, sensors.abs_pressure);
+		if (_baro_stuck) {
-		_px4_baro_1.update(time, sensors.abs_pressure);
+			_px4_baro_0.update(time, _px4_baro_0.get().pressure);
+			_px4_baro_1.update(time, _px4_baro_1.get().pressure);
+
+		} else {
+			_px4_baro_0.update(time, sensors.abs_pressure);
+			_px4_baro_1.update(time, sensors.abs_pressure);
+		}
 	}
 
 	// differential pressure
@@ -995,6 +1001,11 @@ void Simulator::check_failure_injections()
 				supported = true;
 				_baro_blocked = true;
 
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
+				supported = true;
+				_baro_stuck = true;
+				_baro_blocked = false;
+
 			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
 				supported = true;
 				_baro_blocked = false;

test/mavsdk_tests/autopilot_tester.cpp

@@ -355,7 +355,35 @@ void AutopilotTester::execute_mission_and_lose_baro()
 		auto progress = _mission->mission_progress();
 		return progress.current == progress.total;
 	}, std::chrono::seconds(60)));
+}
 
+void AutopilotTester::execute_mission_and_get_baro_stuck()
+{
+	CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success);
+
+	_mission->subscribe_mission_progress([this](Mission::MissionProgress progress) {
+		std::cout << "Progress: " << progress.current << "/" << progress.total;
+
+		if (progress.current == 1) {
+			CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Stuck)
+			      == Failure::Result::Success);
+		}
+	});
+
+	std::promise<void> prom;
+	auto fut = prom.get_future();
+	_mission->start_mission_async([&prom](Mission::Result result) {
+		REQUIRE(Mission::Result::Success == result);
+		prom.set_value();
+	});
+	REQUIRE(fut.wait_for(std::chrono::seconds(1)) == std::future_status::ready);
+
+	// We except the mission to continue with a stuck baro just fine.
+	REQUIRE(poll_condition_with_timeout(
+	[this]() {
+		auto progress = _mission->mission_progress();
+		return progress.current == progress.total;
+	}, std::chrono::seconds(60)));
 }
 
 CoordinateTransformation AutopilotTester::get_coordinate_transformation()

test/mavsdk_tests/autopilot_tester.h

@@ -80,6 +80,7 @@ public:
 	void execute_mission_and_lose_gps();
 	void execute_mission_and_lose_mag();
 	void execute_mission_and_lose_baro();
+	void execute_mission_and_get_baro_stuck();
 	void execute_rtl();
 	void offboard_goto(const Offboard::PositionNedYaw &target, float acceptance_radius_m = 0.3f,
 			   std::chrono::seconds timeout_duration = std::chrono::seconds(60));

test/mavsdk_tests/test_multicopter_failsafe.cpp

@@ -80,3 +80,17 @@ TEST_CASE("Continue on baro lost during mission", "[multicopter][vtol]")
 	tester.execute_mission_and_lose_baro();
 	tester.wait_until_disarmed();
 }
+
+TEST_CASE("Continue on baro stuck during mission", "[multicopter][vtol]")
+{
+	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);
+	tester.arm();
+	tester.execute_mission_and_get_baro_stuck();
+	tester.wait_until_disarmed();
+}