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/SensorGpsSim.cpp

@@ -45,6 +45,7 @@ SensorGpsSim::SensorGpsSim() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
 {
+	_param_sys_failure_en = param_find("SYS_FAILURE_EN");
 }
 
 SensorGpsSim::~SensorGpsSim()
@@ -108,6 +109,8 @@ void SensorGpsSim::Run()
 		updateParams();
 	}
 
+	check_failure_injection();
+
 	if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated()) {
 
 		vehicle_local_position_s lpos{};
@@ -196,27 +199,147 @@ void SensorGpsSim::Run()
 		sensor_gps.vel_ned_valid = true;
 		sensor_gps.satellites_used = _sim_gps_used.get();
 
-		sensor_gps.timestamp = hrt_absolute_time();
-		_sensor_gps_pub.publish(sensor_gps);
+		publishWithFailures(0, sensor_gps, _last_gps0, _sensor_gps_pub);
 
 		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) {
-			// Make instance 1 look like a physically distinct receiver
-			device_id.devid_s.address = 1;
-			sensor_gps.device_id = device_id.devid;
+			sensor_gps_s gps1 = sensor_gps;
 
-			sensor_gps.latitude_deg  = latitude  + (double)gps1_offx / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI);
-			sensor_gps.longitude_deg = longitude + (double)gps1_offy / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI) / cos(latitude * M_PI / 180.0);
-			sensor_gps.timestamp     = hrt_absolute_time();
-			_sensor_gps_pub2.publish(sensor_gps);
+			device_id.devid_s.address = 1;
+			gps1.device_id = device_id.devid;
+
+			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);
 		}
 	}
 
 	perf_end(_loop_perf);
 }
 
+void SensorGpsSim::publishWithFailures(int instance, sensor_gps_s gps, sensor_gps_s &snapshot,
+				       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);
+
+		} else {
+			if (isWrong(instance)) {
+				gps.latitude_deg  += 1.0;
+				gps.longitude_deg += 1.0;
+			}
+
+			gps.timestamp = hrt_absolute_time();
+			snapshot = gps;
+			pub.publish(gps);
+		}
+	}
+}
+
+void SensorGpsSim::check_failure_injection()
+{
+	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;
+
+	vehicle_command_s vehicle_command;
+
+	if (!enabled) {
+		// Clear any active failures so disabling the param at runtime
+		// stops injection instead of latching the previous state.
+		_gps_blocked_mask = 0;
+		_gps_stuck_mask = 0;
+		_gps_wrong_mask = 0;
+
+		// Drain queued commands so re-enabling doesn't replay them.
+		while (_vehicle_command_sub.update(&vehicle_command)) {}
+
+		return;
+	}
+
+	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, otherwise 1-based instance index
+		const int requested_instance = static_cast<int>(lroundf(vehicle_command.param3));
+
+		if (requested_instance < 0 || requested_instance > GPS_MAX_INSTANCES) {
+			vehicle_command_ack_s ack{};
+			ack.command = vehicle_command.command;
+			ack.from_external = false;
+			ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+			ack.timestamp = hrt_absolute_time();
+			_command_ack_pub.publish(ack);
+			continue;
+		}
+
+		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 = true;
+		const char *action = nullptr;
+
+		switch (failure_type) {
+		case vehicle_command_s::FAILURE_TYPE_OK:
+			_gps_blocked_mask &= ~target_mask;
+			_gps_stuck_mask   &= ~target_mask;
+			_gps_wrong_mask   &= ~target_mask;
+			action = "ok";
+			break;
+
+		case vehicle_command_s::FAILURE_TYPE_OFF:
+			_gps_blocked_mask |= target_mask;
+			action = "off";
+			break;
+
+		case vehicle_command_s::FAILURE_TYPE_STUCK:
+			_gps_stuck_mask |= target_mask;
+			action = "stuck";
+			break;
+
+		case vehicle_command_s::FAILURE_TYPE_WRONG:
+			_gps_wrong_mask |= target_mask;
+			action = "wrong";
+			break;
+
+		default:
+			supported = false;
+			break;
+		}
+
+		if (action != nullptr) {
+			for (int i = 0; i < GPS_MAX_INSTANCES; i++) {
+				if (target_mask & (1u << i)) {
+					PX4_INFO("CMD_INJECT_FAILURE, GPS %d %s", i + 1, action);
+				}
+			}
+		}
+
+		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);
+	}
+}
+
 int SensorGpsSim::task_spawn(int argc, char *argv[])
 {
 	SensorGpsSim *instance = new SensorGpsSim();

src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp

@@ -34,15 +34,19 @@
 #pragma once
 
 #include <lib/perf/perf_counter.h>
+#include <parameters/param.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 
@@ -68,8 +72,19 @@ public:
 	bool init();
 
 private:
+	static constexpr int GPS_MAX_INSTANCES = 2;
+
 	void Run() override;
 
+	void check_failure_injection();
+
+	void publishWithFailures(int instance, sensor_gps_s gps, sensor_gps_s &snapshot,
+				 uORB::PublicationMulti<sensor_gps_s> &pub);
+
+	bool isBlocked(int instance) const { return (_gps_blocked_mask >> instance) & 1u; }
+	bool isStuck(int instance)   const { return (_gps_stuck_mask   >> instance) & 1u; }
+	bool isWrong(int instance)   const { return (_gps_wrong_mask   >> instance) & 1u; }
+
 	// generate white Gaussian noise sample with std=1
 	static float generate_wgn();
 
@@ -79,12 +94,23 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 
 	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
 	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub2{ORB_ID(sensor_gps)};
+	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
+	param_t _param_sys_failure_en{PARAM_INVALID};
+
+	uint8_t _gps_blocked_mask{0};
+	uint8_t _gps_stuck_mask{0};
+	uint8_t _gps_wrong_mask{0};
+
+	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};