mavsdk_tests: add test for stuck mag

src/modules/simulator/simulator.h

@@ -259,9 +259,14 @@ private:
 	bool _baro_blocked{false};
 	bool _baro_stuck{false};
 	bool _mag_blocked{false};
+	bool _mag_stuck{false};
 	bool _gps_blocked{false};
 	bool _airspeed_blocked{false};
 
+	float _last_magx{0.0f};
+	float _last_magy{0.0f};
+	float _last_magz{0.0f};
+
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 	px4::atomic<bool> _has_initialized {false};
 #endif

src/modules/simulator/simulator_mavlink.cpp

@@ -247,8 +247,17 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
 
 	// magnetometer
 	if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_mag_blocked) {
-		_px4_mag_0.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
+		if (_mag_stuck) {
-		_px4_mag_1.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
+			_px4_mag_0.update(time, _last_magx, _last_magy, _last_magz);
+			_px4_mag_1.update(time, _last_magx, _last_magy, _last_magz);
+
+		} else {
+			_px4_mag_0.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
+			_px4_mag_1.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
+			_last_magx = sensors.xmag;
+			_last_magy = sensors.ymag;
+			_last_magz = sensors.zmag;
+		}
 	}
 
 	// baro
@@ -989,6 +998,11 @@ void Simulator::check_failure_injections()
 				supported = true;
 				_mag_blocked = true;
 
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
+				supported = true;
+				_mag_stuck = true;
+				_mag_blocked = false;
+
 			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
 				supported = true;
 				_mag_blocked = false;

test/mavsdk_tests/autopilot_tester.cpp

@@ -386,6 +386,35 @@ void AutopilotTester::execute_mission_and_get_baro_stuck()
 	}, std::chrono::seconds(60)));
 }
 
+void AutopilotTester::execute_mission_and_get_mag_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::SensorMag, 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 mag 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()
 {
 	const auto home = _telemetry->home();

test/mavsdk_tests/autopilot_tester.h

@@ -79,6 +79,7 @@ public:
 	void execute_mission();
 	void execute_mission_and_lose_gps();
 	void execute_mission_and_lose_mag();
+	void execute_mission_and_get_mag_stuck();
 	void execute_mission_and_lose_baro();
 	void execute_mission_and_get_baro_stuck();
 	void execute_rtl();

test/mavsdk_tests/test_multicopter_failsafe.cpp

@@ -67,6 +67,20 @@ TEST_CASE("Continue on mag lost during mission", "[multicopter][vtol]")
 	tester.wait_until_disarmed();
 }
 
+TEST_CASE("Continue on mag 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_mag_stuck();
+	tester.wait_until_disarmed();
+}
+
 TEST_CASE("Continue on baro lost during mission", "[multicopter][vtol]")
 {
 	AutopilotTester tester;