feat(sensor_gps_sim): add GPS failure injection via VEHICLE_CMD_INJECT_FAILURE

Implements FAILURE_UNIT_SENSOR_GPS support in the SITL sensor_gps_sim module, mirroring the existing motor failure injection pattern in FailureInjector.

Supported failure types per GPS instance (param3=0 all, 1=GPS0, 2=GPS1):
- FAILURE_TYPE_OFF, stop publishing (simulates dead receiver)
- FAILURE_TYPE_STUCK, freeze last known position (simulates frozen fix)
- FAILURE_TYPE_WRONG, corrupt position by +1 deg (~111 km offset, triggers gpsRedundancyCheck divergence gate)
- FAILURE_TYPE_OK, recover all active failures for that instance

Requires SYS_FAILURE_EN=1. The enable gate is re-read every cycle so runtime parameter changes take effect without restarting the module.

src/modules/simulation/sensor_gps_sim/CMakeLists.txt

@@ -38,6 +38,8 @@ px4_add_module(
 	SRCS
 		SensorGpsSim.cpp
 		SensorGpsSim.hpp
+		SensorGpsFailureInjector.cpp
+		SensorGpsFailureInjector.hpp
 	MODULE_CONFIG
 		parameters.yaml
 	DEPENDS

src/modules/simulation/sensor_gps_sim/SensorGpsFailureInjector.cpp

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "SensorGpsFailureInjector.hpp"
+
+#include <drivers/drv_hrt.h>
+
+SensorGpsFailureInjector::SensorGpsFailureInjector()
+{
+	// Cache the handle once — param_find() is a string search and is expensive.
+	// The value itself is re-read each update() cycle so runtime changes take effect.
+	_param_sys_failure_en = param_find("SYS_FAILURE_EN");
+}
+
+void SensorGpsFailureInjector::update()
+{
+	// Re-read the enable gate every cycle so toggling SYS_FAILURE_EN at runtime
+	// takes effect immediately without restarting the module.
+	int32_t sys_failure_en = 0;
+	const bool enabled = (_param_sys_failure_en != PARAM_INVALID)
+			     && (param_get(_param_sys_failure_en, &sys_failure_en) == PX4_OK)
+			     && (sys_failure_en == 1);
+
+	if (!enabled) { return; }
+
+	vehicle_command_s vehicle_command;
+
+	while (_vehicle_command_sub.update(&vehicle_command)) {
+		const int failure_unit = static_cast<int>(lroundf(vehicle_command.param1));
+		const int failure_type = static_cast<int>(lroundf(vehicle_command.param2));
+
+		if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE
+		    || failure_unit != vehicle_command_s::FAILURE_UNIT_SENSOR_GPS) {
+			continue;
+		}
+
+		// param3: 0 = all instances, 1 = GPS0, 2 = GPS1
+		const int requested_instance = static_cast<int>(lroundf(vehicle_command.param3));
+
+		// Build a bitmask of which instances to affect directly, without a loop.
+		// requested_instance=0 → all → 0b11, otherwise select the single bit.
+		const uint8_t target_mask = (requested_instance == 0)
+					    ? static_cast<uint8_t>((1u << GPS_MAX_INSTANCES) - 1u)
+					    : static_cast<uint8_t>(1u << (requested_instance - 1));
+
+		bool supported = false;
+
+		switch (failure_type) {
+		case vehicle_command_s::FAILURE_TYPE_OK:
+			supported = true;
+			PX4_INFO("CMD_INJECT_FAILURE, GPS ok (mask=0x%x)", target_mask);
+			_gps_blocked_mask &= ~target_mask;
+			_gps_stuck_mask   &= ~target_mask;
+			_gps_wrong_mask   &= ~target_mask;
+			break;
+
+		case vehicle_command_s::FAILURE_TYPE_OFF:
+			supported = true;
+			PX4_INFO("CMD_INJECT_FAILURE, GPS off (mask=0x%x)", target_mask);
+			_gps_blocked_mask |= target_mask;
+			break;
+
+		case vehicle_command_s::FAILURE_TYPE_STUCK:
+			supported = true;
+			PX4_INFO("CMD_INJECT_FAILURE, GPS stuck (mask=0x%x)", target_mask);
+			_gps_stuck_mask |= target_mask;
+			break;
+
+		case vehicle_command_s::FAILURE_TYPE_WRONG:
+			supported = true;
+			PX4_INFO("CMD_INJECT_FAILURE, GPS wrong (mask=0x%x)", target_mask);
+			_gps_wrong_mask |= target_mask;
+			break;
+
+		default:
+			break;
+		}
+
+		vehicle_command_ack_s ack{};
+		ack.command = vehicle_command.command;
+		ack.from_external = false;
+		ack.result = supported ?
+			     vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED :
+			     vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+		ack.timestamp = hrt_absolute_time();
+		_command_ack_pub.publish(ack);
+	}
+}

src/modules/simulation/sensor_gps_sim/SensorGpsFailureInjector.hpp

@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <parameters/param.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+
+/**
+ * GPS failure injector for SITL simulation.
+ *
+ * Listens for VEHICLE_CMD_INJECT_FAILURE commands targeting FAILURE_UNIT_SENSOR_GPS
+ * and maintains per-instance state bitmasks that SensorGpsSim consults before
+ * each publish to suppress or corrupt GPS data.
+ *
+ * Mirrors the design of FailureInjector (motor failures) in commander.
+ * Requires SYS_FAILURE_EN = 1.
+ *
+ * Instance mapping (param3):
+ *   0 = all instances
+ *   1 = GPS instance 0 (primary)
+ *   2 = GPS instance 1 (secondary)
+ */
+class SensorGpsFailureInjector
+{
+public:
+	static constexpr int GPS_MAX_INSTANCES = 2;
+
+	SensorGpsFailureInjector();
+
+	/**
+	 * Poll vehicle_command and update failure state bitmasks.
+	 * Must be called once per SensorGpsSim::Run() cycle.
+	 */
+	void update();
+
+	/** Returns true if the given GPS instance should stop publishing (FAILURE_TYPE_OFF). */
+	bool isBlocked(int instance) const { return (_gps_blocked_mask >> instance) & 1u; }
+
+	/** Returns true if the given GPS instance should freeze on its last position (FAILURE_TYPE_STUCK). */
+	bool isStuck(int instance) const { return (_gps_stuck_mask >> instance) & 1u; }
+
+	/** Returns true if the given GPS instance should publish corrupted position data (FAILURE_TYPE_WRONG). */
+	bool isWrong(int instance) const { return (_gps_wrong_mask >> instance) & 1u; }
+
+private:
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+	// Cached handle from param_find() — avoids repeated string searches.
+	// The actual value is re-read in update() so runtime changes take effect.
+	param_t _param_sys_failure_en{PARAM_INVALID};
+
+	uint8_t _gps_blocked_mask{0}; ///< FAILURE_TYPE_OFF  — stop publishing
+	uint8_t _gps_stuck_mask{0};   ///< FAILURE_TYPE_STUCK — freeze last position
+	uint8_t _gps_wrong_mask{0};   ///< FAILURE_TYPE_WRONG — corrupt position
+};

src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp

@@ -108,6 +108,8 @@ void SensorGpsSim::Run()
 		updateParams();
 	}
 
+	_failure_injector.update();
+
 	if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated()) {
 
 		vehicle_local_position_s lpos{};
@@ -196,17 +198,53 @@ void SensorGpsSim::Run()
 		sensor_gps.vel_ned_valid = true;
 		sensor_gps.satellites_used = _sim_gps_used.get();
 
-		sensor_gps.timestamp = hrt_absolute_time();
+		// --- GPS instance 0 (primary) ---
-		_sensor_gps_pub.publish(sensor_gps);
+		if (!_failure_injector.isBlocked(0)) {
+			if (_failure_injector.isStuck(0)) {
+				// Replay the last known position with a fresh timestamp so the
+				// consumer sees live-looking data whose position never changes.
+				_last_gps0.timestamp = hrt_absolute_time();
+				_sensor_gps_pub.publish(_last_gps0);
 
+			} else {
+				if (_failure_injector.isWrong(0)) {
+					// Corrupt position by +1 degree lat/lon (~111 km offset).
+					// GPS0 and GPS1 will now diverge well beyond the redundancy gate.
+					sensor_gps.latitude_deg  += 1.0;
+					sensor_gps.longitude_deg += 1.0;
+				}
+
+				sensor_gps.timestamp = hrt_absolute_time();
+				_last_gps0 = sensor_gps;
+				_sensor_gps_pub.publish(sensor_gps);
+			}
+		}
+
+		// --- GPS instance 1 (secondary) ---
 		const float gps1_offx = _param_gps1_offx.get();
 		const float gps1_offy = _param_gps1_offy.get();
 
 		if (fabsf(gps1_offx) > 0.f || fabsf(gps1_offy) > 0.f) {
-			sensor_gps.latitude_deg  = latitude  + (double)gps1_offx / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI);
+			if (!_failure_injector.isBlocked(1)) {
-			sensor_gps.longitude_deg = longitude + (double)gps1_offy / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI) / cos(latitude * M_PI / 180.0);
+				sensor_gps_s gps1 = sensor_gps;
-			sensor_gps.timestamp     = hrt_absolute_time();
+				gps1.latitude_deg  = latitude  + (double)gps1_offx / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI);
-			_sensor_gps_pub2.publish(sensor_gps);
+				gps1.longitude_deg = longitude + (double)gps1_offy / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI) / cos(latitude * M_PI / 180.0);
+
+				if (_failure_injector.isStuck(1)) {
+					_last_gps1.timestamp = hrt_absolute_time();
+					_sensor_gps_pub2.publish(_last_gps1);
+
+				} else {
+					if (_failure_injector.isWrong(1)) {
+						gps1.latitude_deg  += 1.0;
+						gps1.longitude_deg += 1.0;
+					}
+
+					gps1.timestamp = hrt_absolute_time();
+					_last_gps1 = gps1;
+					_sensor_gps_pub2.publish(gps1);
+				}
+			}
 		}
 	}
 

src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp

@@ -33,6 +33,8 @@
 
 #pragma once
 
+#include "SensorGpsFailureInjector.hpp"
+
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
@@ -85,6 +87,12 @@ private:
 
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
+	SensorGpsFailureInjector _failure_injector{};
+
+	// Last good sensor_gps snapshots, used to replay frozen data in FAILURE_TYPE_STUCK
+	sensor_gps_s _last_gps0{};
+	sensor_gps_s _last_gps1{};
+
 	// GPS Markov process noise state
 	float _gps_pos_noise_n{0.0f};
 	float _gps_pos_noise_e{0.0f};