feat(failure-injection): implement GNSS failure injection handling and add GPS wrong-failure fix type parameter (#28139)

docs/en/debug/failure_injection.md

@@ -5,7 +5,7 @@ This enables easier testing of [safety failsafe](../config/safety.md) behaviour,
 
 Failure injection is disabled by default, and can be enabled using the [SYS_FAILURE_EN](../advanced_config/parameter_reference.md#SYS_FAILURE_EN) parameter.
 
-Failures can be injected both in simulation and on real hardware. In simulation the available failures depend on the simulator. On hardware the `off` (stop publishing) and `stuck` (freeze the last value) types are supported for the `gyro`, `accel`, `mag`, `baro`, `distance_sensor` and `gps` components; this requires firmware built with the failure-injection module. In addition, the `battery` component supports `off` (report a depleted pack, triggering the battery failsafe).
+Failures can be injected both in simulation and on real hardware. In simulation the available failures depend on the simulator. On hardware the `off` (stop publishing) and `stuck` (freeze the last value) types are supported for the `gyro`, `accel`, `mag`, `baro`, `distance_sensor` and `gps` components; this requires firmware built with the failure-injection module. In addition, the `battery` component supports `off` (report a depleted pack, triggering the battery failsafe), and the `gps` component supports `wrong` (report the fix type selected by [SYS_FAIL_GPS_WRG](../advanced_config/parameter_reference.md#SYS_FAIL_GPS_WRG), leaving the reported position untouched).
 
 ::: info
 PX4 may accept a command to set a particular failure mode even it that mode is not supported by your simulator.
@@ -61,7 +61,8 @@ where:
 - _instance bitmask_ (optional): address several instances at once (bit 0 = first instance, bit 1 = second, …; decimal or `0x` hex). Used only when `-i` is omitted. Example: `-m 0x5` targets instances 1 and 3.
 
 ::: info
-The simulated GPS (SITL) implements only the `off`, `stuck`, and `wrong` failure modes; the other failure types have no effect on it.
+GPS implements only the `off`, `stuck`, and `wrong` failure modes; the other failure types have no effect on it.
+`gps wrong` makes the addressed receiver report the fix type selected by [SYS_FAIL_GPS_WRG](../advanced_config/parameter_reference.md#SYS_FAIL_GPS_WRG) and leaves the reported position untouched.
 :::
 
 ## RC Switch Trigger

src/drivers/gnss/septentrio/septentrio.cpp

@@ -1739,8 +1739,7 @@ void SeptentrioDriver::publish()
 
 	_failure_config.update();
 
-	if (!failure_injection::process(_failure_config, failure_injection_s::FAILURE_UNIT_SENSOR_GPS,
-					_sensor_gps_pub.get_instance(), _sensor_gps, _stuck)) {
+	if (!failure_injection::process_gnss(_failure_config, _sensor_gps_pub.get_instance(), _sensor_gps, _stuck)) {
 		return;
 	}
 

src/drivers/gps/gps.cpp

@@ -1609,8 +1609,7 @@ GPS::publish()
 
 		_failure_config.update();
 
-		if (!failure_injection::process(_failure_config, failure_injection_s::FAILURE_UNIT_SENSOR_GPS,
-						_sensor_gps_pub.get_instance(), _sensor_gps, _stuck)) {
+		if (!failure_injection::process_gnss(_failure_config, _sensor_gps_pub.get_instance(), _sensor_gps, _stuck)) {
 			return;
 		}
 

src/drivers/uavcan/sensors/gnss.cpp

@@ -622,8 +622,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 	const int instance = get_orb_instance_for_node(node_id);
 
 	if (channel >= 0 && instance >= 0
-	    && !failure_injection::process(_failure_config, failure_injection_s::FAILURE_UNIT_SENSOR_GPS,
-					   instance, sensor_gps, _stuck[channel])) {
+	    && !failure_injection::process_gnss(_failure_config, instance, sensor_gps, _stuck[channel])) {
 		return;
 	}
 

src/lib/failure_injection/FailureInjection.cpp

@@ -35,7 +35,9 @@
 
 #if defined(CONFIG_MODULES_FAILURE_INJECTION_MANAGER)
 
+#include <parameters/param.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/sensor_gps.h>
 
 namespace failure_injection
 {
@@ -91,6 +93,28 @@ void process_battery(const Config &config, uint8_t instance, battery_status_s &b
 	battery_status.warning = battery_status_s::WARNING_EMERGENCY;
 }
 
+bool process_gnss(const Config &config, uint8_t uorb_instance, sensor_gps_s &sensor_gps,
+		  Stuck<sensor_gps_s> &stuck)
+{
+	const Mode mode = config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_GPS, uorb_instance + 1);
+
+	// Off and Stuck are message-agnostic; run them first so the Stuck cache keeps the
+	// uncorrupted sample and a later Stuck replays a healthy fix.
+	if (!process(mode, sensor_gps, stuck)) {
+		return false;
+	}
+
+	if (mode == Mode::Wrong) {
+		static const param_t fix_type_handle = param_find("SYS_FAIL_GPS_WRG");
+
+		int32_t fix_type = sensor_gps_s::FIX_TYPE_2D;
+		param_get(fix_type_handle, &fix_type);
+		sensor_gps.fix_type = (uint8_t)fix_type;
+	}
+
+	return true;
+}
+
 esc_status_s process_esc(const Config &config, const esc_status_s &status)
 {
 	esc_status_s result = status;

src/lib/failure_injection/FailureInjection.hpp

@@ -53,6 +53,7 @@
 #include <uORB/topics/failure_injection.h>
 
 struct battery_status_s;
+struct sensor_gps_s;
 
 namespace failure_injection
 {
@@ -196,6 +197,17 @@ inline bool process(const Config &config, uint8_t unit, uint8_t uorb_instance)
  */
 void process_battery(const Config &config, uint8_t instance, battery_status_s &battery_status);
 
+/**
+ * GNSS counterpart to process(): on FAILURE_UNIT_SENSOR_GPS for the receiver publishing on the
+ * given 0-based uORB instance, Off and Stuck behave as in the generic process() and Wrong reports
+ * the fix type selected by SYS_FAIL_GPS_WRG while leaving the position untouched.
+ *
+ * @param uorb_instance 0-based uORB instance of the publisher (not the 1-based failure instance).
+ * @return false if the sensor_gps publication must be suppressed (Off), true otherwise.
+ */
+bool process_gnss(const Config &config, uint8_t uorb_instance, sensor_gps_s &sensor_gps,
+		  Stuck<sensor_gps_s> &stuck);
+
 /**
  * ESC counterpart to process(): apply the active FAILURE_UNIT_SYSTEM_ESC failures to a copy of
  * status (matched per ESC by actuator_function). Off zeroes the ESC's telemetry (keeping only
@@ -233,6 +245,8 @@ inline bool process(const Config &, uint8_t, uint8_t) { return true; }
 
 inline void process_battery(const Config &, uint8_t, battery_status_s &) {}
 
+inline bool process_gnss(const Config &, uint8_t, sensor_gps_s &, Stuck<sensor_gps_s> &) { return true; }
+
 inline esc_status_s process_esc(const Config &, const esc_status_s &status) { return status; }
 
 #endif // CONFIG_MODULES_FAILURE_INJECTION_MANAGER

src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp

@@ -39,7 +39,9 @@
 #include <gtest/gtest.h>
 
 #include <lib/failure_injection/FailureInjection.hpp>
+#include <parameters/param.h>
 #include <uORB/Publication.hpp>
+#include <uORB/topics/sensor_gps.h>
 
 // FailureInjection.hpp transitively pulls in px4_platform_common/defines.h (via
 // uORB Subscription), which defines an OK macro that would clash with the local
@@ -76,6 +78,28 @@ failure_injection_s make_config(uint8_t unit, uint16_t instance_mask, uint8_t fa
 	return cfg;
 }
 
+// Healthy 3D-fix sample, as a GNSS driver would publish it.
+sensor_gps_s clean_gps()
+{
+	sensor_gps_s gps{};
+	gps.timestamp = 1000;
+	gps.timestamp_sample = 900;
+	gps.fix_type = sensor_gps_s::FIX_TYPE_3D;
+	gps.latitude_deg = 47.0;
+	gps.longitude_deg = 8.0;
+	gps.altitude_msl_m = 500.0;
+	gps.altitude_ellipsoid_m = 500.0;
+	gps.eph = 0.9f;
+	gps.epv = 1.78f;
+	gps.satellites_used = 25;
+	gps.vel_n_m_s = 1.f;
+	gps.vel_e_m_s = 2.f;
+	gps.vel_d_m_s = 0.5f;
+	gps.vel_m_s = 2.236f;
+	gps.vel_ned_valid = true;
+	return gps;
+}
+
 constexpr uint8_t GYRO  = failure_injection_s::FAILURE_UNIT_SENSOR_GYRO;
 constexpr uint8_t GPS   = failure_injection_s::FAILURE_UNIT_SENSOR_GPS;
 constexpr uint8_t MOTOR = failure_injection_s::FAILURE_UNIT_SYSTEM_MOTOR;
@@ -269,6 +293,132 @@ TEST(FailureInjectionConfig, ProcessMessageLessSuppressesOnlyOff)
 	EXPECT_TRUE(process(config, GYRO, 0));
 }
 
+// ===========================================================================
+// process_gnss()
+// ===========================================================================
+
+TEST(FailureInjectionConfig, ProcessGnssWrongSetsConfiguredFixType)
+{
+	// The test runner has no work queue for the autosave param_set would schedule.
+	param_control_autosave(false);
+
+	Config config;
+	config.set(make_config(GPS, 0x1, WRONG)); // uORB instance 0 -> failure instance 1
+
+	const int32_t fix_types[] = {
+		sensor_gps_s::FIX_TYPE_2D,
+		sensor_gps_s::FIX_TYPE_NONE,
+		sensor_gps_s::FIX_TYPE_RTK_FLOAT,
+		sensor_gps_s::FIX_TYPE_RTK_FIXED,
+	};
+
+	for (const int32_t expected : fix_types) {
+		ASSERT_EQ(param_set(param_find("SYS_FAIL_GPS_WRG"), &expected), 0);
+
+		Stuck<sensor_gps_s> stuck;
+		sensor_gps_s gps = clean_gps();
+
+		EXPECT_TRUE(process_gnss(config, 0, gps, stuck));
+		EXPECT_EQ(gps.fix_type, (uint8_t)expected);
+	}
+
+	const int32_t default_fix_type = sensor_gps_s::FIX_TYPE_2D;
+	param_set(param_find("SYS_FAIL_GPS_WRG"), &default_fix_type);
+}
+
+TEST(FailureInjectionConfig, ProcessGnssWrongLeavesPositionUntouched)
+{
+	param_control_autosave(false);
+
+	const int32_t fix_type = sensor_gps_s::FIX_TYPE_2D;
+	ASSERT_EQ(param_set(param_find("SYS_FAIL_GPS_WRG"), &fix_type), 0);
+
+	Config config;
+	config.set(make_config(GPS, 0x1, WRONG));
+
+	Stuck<sensor_gps_s> stuck;
+	const sensor_gps_s truth = clean_gps();
+	sensor_gps_s gps = truth;
+
+	EXPECT_TRUE(process_gnss(config, 0, gps, stuck));
+
+	// Only the fix type changes: the reported solution stays coherent with the truth.
+	EXPECT_EQ(gps.fix_type, (uint8_t)sensor_gps_s::FIX_TYPE_2D);
+	EXPECT_DOUBLE_EQ(gps.latitude_deg, truth.latitude_deg);
+	EXPECT_DOUBLE_EQ(gps.longitude_deg, truth.longitude_deg);
+	EXPECT_DOUBLE_EQ(gps.altitude_msl_m, truth.altitude_msl_m);
+	EXPECT_DOUBLE_EQ(gps.altitude_ellipsoid_m, truth.altitude_ellipsoid_m);
+	EXPECT_FLOAT_EQ(gps.vel_n_m_s, truth.vel_n_m_s);
+	EXPECT_FLOAT_EQ(gps.vel_e_m_s, truth.vel_e_m_s);
+	EXPECT_FLOAT_EQ(gps.vel_d_m_s, truth.vel_d_m_s);
+	EXPECT_FLOAT_EQ(gps.vel_m_s, truth.vel_m_s);
+	EXPECT_FLOAT_EQ(gps.eph, truth.eph);
+	EXPECT_EQ(gps.satellites_used, truth.satellites_used);
+}
+
+TEST(FailureInjectionConfig, ProcessGnssWrongLeavesUnselectedInstanceUntouched)
+{
+	param_control_autosave(false);
+
+	const int32_t fix_type = sensor_gps_s::FIX_TYPE_2D;
+	ASSERT_EQ(param_set(param_find("SYS_FAIL_GPS_WRG"), &fix_type), 0);
+
+	Config config;
+	config.set(make_config(GPS, 0x2, WRONG)); // failure instance 2 -> uORB instance 1
+
+	Stuck<sensor_gps_s> stuck;
+	sensor_gps_s gps = clean_gps();
+
+	EXPECT_TRUE(process_gnss(config, 0, gps, stuck));
+	EXPECT_EQ(gps.fix_type, (uint8_t)sensor_gps_s::FIX_TYPE_3D);
+
+	// The addressed instance is degraded.
+	Stuck<sensor_gps_s> stuck_1;
+	sensor_gps_s gps_1 = clean_gps();
+
+	EXPECT_TRUE(process_gnss(config, 1, gps_1, stuck_1));
+	EXPECT_EQ(gps_1.fix_type, (uint8_t)sensor_gps_s::FIX_TYPE_2D);
+}
+
+TEST(FailureInjectionConfig, ProcessGnssOffSuppressesPublication)
+{
+	Config config;
+	config.set(make_config(GPS, 0x1, OFF));
+
+	Stuck<sensor_gps_s> stuck;
+	sensor_gps_s gps = clean_gps();
+
+	EXPECT_FALSE(process_gnss(config, 0, gps, stuck));
+	// Off suppresses instead of mutating.
+	EXPECT_EQ(gps.fix_type, (uint8_t)sensor_gps_s::FIX_TYPE_3D);
+}
+
+TEST(FailureInjectionConfig, ProcessGnssStuckReplaysLastGoodSample)
+{
+	Config config;
+	Stuck<sensor_gps_s> stuck;
+
+	// A healthy cycle records the last good sample.
+	sensor_gps_s good = clean_gps();
+	EXPECT_TRUE(process_gnss(config, 0, good, stuck));
+
+	config.set(make_config(GPS, 0x1, STUCK));
+
+	sensor_gps_s moved = clean_gps();
+	moved.timestamp = 2000;
+	moved.timestamp_sample = 1900;
+	moved.latitude_deg = 48.0;
+	moved.longitude_deg = 9.0;
+
+	EXPECT_TRUE(process_gnss(config, 0, moved, stuck));
+
+	// The frozen position comes back, with the live timestamps.
+	EXPECT_DOUBLE_EQ(moved.latitude_deg, good.latitude_deg);
+	EXPECT_DOUBLE_EQ(moved.longitude_deg, good.longitude_deg);
+	EXPECT_EQ(moved.timestamp, 2000u);
+	EXPECT_EQ(moved.timestamp_sample, 1900u);
+}
+
 // ===========================================================================
 // process_esc(): ESC Off / Wrong on the multi-instance esc_status
 // ===========================================================================

src/modules/failure_injection_manager/FailureInjectionManager.hpp

@@ -102,6 +102,9 @@ private:
 		(ParamInt<px4::params::SYS_FAIL_RC_SRC>) _param_sys_fail_rc_src,
 		(ParamInt<px4::params::SYS_FAIL_RC_UNIT>) _param_sys_fail_rc_unit,
 		(ParamInt<px4::params::SYS_FAIL_RC_MODE>) _param_sys_fail_rc_mode,
-		(ParamInt<px4::params::SYS_FAIL_RC_INST>) _param_sys_fail_rc_inst
+		(ParamInt<px4::params::SYS_FAIL_RC_INST>) _param_sys_fail_rc_inst,
+		// Consumed via param_find() in failure_injection::process_gnss(); registered
+		// here so it is marked used and shows up in the GCS parameter list.
+		(ParamInt<px4::params::SYS_FAIL_GPS_WRG>) _param_sys_fail_gps_wrg
 	)
 };

src/modules/failure_injection_manager/failure_injection_manager_params.yaml

@@ -80,3 +80,21 @@ parameters:
         max: 16
         default: 1
         reboot_required: true
+      SYS_FAIL_GPS_WRG:
+        description:
+          short: GPS Wrong-failure fix type
+          long: |-
+            GNSS fix type reported by the addressed receiver while a GPS 'wrong'
+            failure injection is active. The reported position is left untouched.
+            The default 2D fix is rejected by the estimator, which requires a 3D
+            fix, and makes the GNSS redundancy check report a lost fix, while the
+            receiver stays eligible for GPS blending. Values above 3D fix report a
+            better solution than the receiver really has.
+        type: enum
+        default: 2
+        values:
+          1: "Fix: None"
+          2: "Fix: 2D"
+          3: "Fix: 3D"
+          5: "Fix: RTK float"
+          6: "Fix: RTK fixed"

src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp

@@ -198,7 +198,7 @@ void SensorGpsSim::Run()
 		sensor_gps.vel_ned_valid = true;
 		sensor_gps.satellites_used = _sim_gps_used.get();
 
-		publishWithFailures(0, sensor_gps, _last_gps0, _sensor_gps_pub);
+		publishWithFailures(0, sensor_gps, _sensor_gps_pub);
 
 		const float gps1_offx = _param_gps1_offx.get();
 		const float gps1_offy = _param_gps1_offy.get();
@@ -212,33 +212,22 @@ void SensorGpsSim::Run()
 			gps1.latitude_deg  = latitude  + (double)gps1_offx / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI);
 			gps1.longitude_deg = longitude + (double)gps1_offy / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI) / cos(latitude * M_PI / 180.0);
 
-			publishWithFailures(1, gps1, _last_gps1, _sensor_gps_pub2);
+			publishWithFailures(1, gps1, _sensor_gps_pub2);
 		}
 	}
 
 	perf_end(_loop_perf);
 }
 
-void SensorGpsSim::publishWithFailures(int instance, sensor_gps_s gps, sensor_gps_s &snapshot,
-				       uORB::PublicationMulti<sensor_gps_s> &pub)
+void SensorGpsSim::publishWithFailures(int instance, sensor_gps_s gps, uORB::PublicationMulti<sensor_gps_s> &pub)
 {
-	// Precedence when multiple failure masks are set: BLOCKED > STUCK > WRONG.
-	if (!isBlocked(instance)) {
-		if (isStuck(instance)) {
-			snapshot.timestamp = hrt_absolute_time();
-			pub.publish(snapshot);
+	gps.timestamp = hrt_absolute_time();
 
-		} else {
-			if (isWrong(instance)) {
-				gps.latitude_deg  += 1.0;
-				gps.longitude_deg += 1.0;
-			}
-
-			gps.timestamp = hrt_absolute_time();
-			snapshot = gps;
-			pub.publish(gps);
-		}
+	if (!failure_injection::process_gnss(_failure_config, instance, gps, _stuck[instance])) {
+		return;
 	}
+
+	pub.publish(gps);
 }
 
 void SensorGpsSim::updateFailureConfig()

src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp

@@ -77,17 +77,7 @@ private:
 
 	void updateFailureConfig();
 
-	void publishWithFailures(int instance, sensor_gps_s gps, sensor_gps_s &snapshot,
-				 uORB::PublicationMulti<sensor_gps_s> &pub);
-
-	// instance is 0-based here; the failure_injection topic addresses 1-based instances.
-	failure_injection::Mode failureMode(int instance) const
-	{
-		return _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_GPS, instance + 1);
-	}
-	bool isBlocked(int instance) const { return failureMode(instance) == failure_injection::Mode::Off; }
-	bool isStuck(int instance)   const { return failureMode(instance) == failure_injection::Mode::Stuck; }
-	bool isWrong(int instance)   const { return failureMode(instance) == failure_injection::Mode::Wrong; }
+	void publishWithFailures(int instance, sensor_gps_s gps, uORB::PublicationMulti<sensor_gps_s> &pub);
 
 	// generate white Gaussian noise sample with std=1
 	static float generate_wgn();
@@ -104,10 +94,9 @@ private:
 
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
+	// Failure injection (FAILURE_UNIT_SENSOR_GPS): active config + per-instance last-good sample.
 	failure_injection::Config _failure_config;
-
-	sensor_gps_s _last_gps0{};
-	sensor_gps_s _last_gps1{};
+	failure_injection::Stuck<sensor_gps_s> _stuck[GPS_MAX_INSTANCES];
 
 	// GPS Markov process noise state
 	float _gps_pos_noise_n{0.0f};

src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

@@ -391,99 +391,87 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg)
 	mavlink_hil_gps_t hil_gps;
 	mavlink_msg_hil_gps_decode(msg, &hil_gps);
 
-	if (!_gps_blocked) {
-		sensor_gps_s gps{};
+	sensor_gps_s gps{};
 
-		if (!_gps_stuck) {
-			if (!_gps_wrong) {
-				gps.latitude_deg = hil_gps.lat / 1e7;
-				gps.longitude_deg = hil_gps.lon / 1e7;
-				gps.altitude_msl_m = hil_gps.alt / 1e3;
-				gps.altitude_ellipsoid_m = hil_gps.alt / 1e3;
+	gps.latitude_deg = hil_gps.lat / 1e7;
+	gps.longitude_deg = hil_gps.lon / 1e7;
+	gps.altitude_msl_m = hil_gps.alt / 1e3;
+	gps.altitude_ellipsoid_m = hil_gps.alt / 1e3;
 
-			} else {
-				gps.latitude_deg = hil_gps.lat / 1e7 + 1.0;
-				gps.longitude_deg = hil_gps.lon / 1e7 + 1.0;
-				gps.altitude_msl_m = hil_gps.alt / 1e3 + 100.0;
-				gps.altitude_ellipsoid_m = hil_gps.alt / 1e3 - 100.0;
-			}
+	gps.s_variance_m_s = 0.25f;
+	gps.c_variance_rad = 0.5f;
+	gps.fix_type = hil_gps.fix_type;
 
-			gps.s_variance_m_s = 0.25f;
-			gps.c_variance_rad = 0.5f;
-			gps.fix_type = hil_gps.fix_type;
+	gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
+	gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
 
-			gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
-			gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
+	gps.hdop = 0; // TODO
+	gps.vdop = 0; // TODO
 
-			gps.hdop = 0; // TODO
-			gps.vdop = 0; // TODO
+	gps.noise_per_ms = 0;
+	gps.automatic_gain_control = 0;
+	gps.jamming_indicator = 0;
+	gps.jamming_state = 0;
+	gps.spoofing_state = 0;
 
-			gps.noise_per_ms = 0;
-			gps.automatic_gain_control = 0;
-			gps.jamming_indicator = 0;
-			gps.jamming_state = 0;
-			gps.spoofing_state = 0;
+	gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s
+	gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s
+	gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s
+	gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s
 
-			if (!_gps_wrong) {
-				gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s
-				gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s
-				gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s
-				gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s
+	gps.cog_rad = ((hil_gps.cog == 65535) ? NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad
+	gps.vel_ned_valid = true;
 
-			} else {
-				gps.vel_m_s = (float)(hil_gps.vel) / 100.0f - 1.f; // cm/s -> m/s
-				gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f + 5.f; // cm/s -> m/s
-				gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f - 8.f; // cm/s -> m/s
-				gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f + 2.f; // cm/s -> m/s
-			}
+	gps.timestamp_time_relative = 0;
+	gps.time_utc_usec = hil_gps.time_usec;
 
-			gps.cog_rad = ((hil_gps.cog == 65535) ? NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad
-			gps.vel_ned_valid = true;
+	gps.satellites_used = hil_gps.satellites_visible;
 
-			gps.timestamp_time_relative = 0;
-			gps.time_utc_usec = hil_gps.time_usec;
+	gps.heading = NAN;
+	gps.heading_offset = NAN;
 
-			gps.satellites_used = hil_gps.satellites_visible;
+	gps.timestamp = hrt_absolute_time();
 
-			gps.heading = NAN;
-			gps.heading_offset = NAN;
+	// Resolve the uORB instance for this HIL_GPS id first, so the failure injection state can be
+	// kept per instance. New publishers are created based on the HIL_GPS ID's being different or not.
+	int instance = -1;
 
-			_gps_prev = gps;
-
-		} else {
-			gps = _gps_prev;
+	for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) {
+		if (_sensor_gps_pubs[i] && _gps_ids[i] == hil_gps.id) {
+			instance = i;
+			break;
 		}
 
-		gps.timestamp = hrt_absolute_time();
-
-		// New publishers will be created based on the HIL_GPS ID's being different or not
-		for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) {
-			if (_sensor_gps_pubs[i] && _gps_ids[i] == hil_gps.id) {
-				_sensor_gps_pubs[i]->publish(gps);
-				break;
-			}
+		if (_sensor_gps_pubs[i] == nullptr) {
+			_sensor_gps_pubs[i] = new uORB::PublicationMulti<sensor_gps_s> {ORB_ID(sensor_gps)};
 
 			if (_sensor_gps_pubs[i] == nullptr) {
-				_sensor_gps_pubs[i] = new uORB::PublicationMulti<sensor_gps_s> {ORB_ID(sensor_gps)};
-
-				if (_sensor_gps_pubs[i] == nullptr) {
-					break;
-				}
-
-				_gps_ids[i] = hil_gps.id;
-
-				device::Device::DeviceId device_id;
-				device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
-				device_id.devid_s.bus = 0;
-				device_id.devid_s.address = i;
-				device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
-				gps.device_id = device_id.devid;
-
-				_sensor_gps_pubs[i]->publish(gps);
-				break;
+				return;
 			}
+
+			_gps_ids[i] = hil_gps.id;
+
+			device::Device::DeviceId device_id;
+			device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
+			device_id.devid_s.bus = 0;
+			device_id.devid_s.address = i;
+			device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
+			gps.device_id = device_id.devid;
+
+			instance = i;
+			break;
 		}
 	}
+
+	if (instance < 0) {
+		return;
+	}
+
+	if (!failure_injection::process_gnss(_failure_config, (uint8_t)instance, gps, _gps_stuck[instance])) {
+		return;
+	}
+
+	_sensor_gps_pubs[instance]->publish(gps);
 }
 
 void SimulatorMavlink::handle_message_hil_sensor(const mavlink_message_t *msg)
@@ -1463,11 +1451,6 @@ void SimulatorMavlink::updateFailureConfig()
 {
 	_failure_config.update();
 
-	const failure_injection::Mode gps_mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_GPS, 1);
-	_gps_blocked = (gps_mode == failure_injection::Mode::Off);
-	_gps_stuck = (gps_mode == failure_injection::Mode::Stuck);
-	_gps_wrong = (gps_mode == failure_injection::Mode::Wrong);
-
 	const failure_injection::Mode airspeed_mode = _failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_AIRSPEED, 1);
 	_airspeed_disconnected = (airspeed_mode == failure_injection::Mode::Off);
 

src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp

@@ -311,11 +311,10 @@ private:
 	hrt_abstime _last_accel_fifo_timestamp{0};
 	hrt_abstime _last_gyro_fifo_timestamp{0};
 
-	// GPS, airspeed and VIO failure injection (no PX4* helper class applies these).
-	bool _gps_blocked{false};
-	bool _gps_stuck{false};
-	bool _gps_wrong{false};
-	sensor_gps_s _gps_prev{};
+	// Per-HIL_GPS-instance last-good sample, for the Stuck failure.
+	failure_injection::Stuck<sensor_gps_s> _gps_stuck[MAX_GPS];
+
+	// airspeed and VIO failure injection (no PX4* helper class applies these).
 	bool _airspeed_disconnected{false};
 	hrt_abstime _airspeed_blocked_timestamp{0};
 	bool _vio_blocked{false};