feat(sensor_gps_sim): add GPS failure injection via VEHICLE_CMD_INJEC…

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,135 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 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()
+{
+	_param_sys_failure_en = param_find("SYS_FAILURE_EN");
+}
+
+void SensorGpsFailureInjector::update()
+{
+	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) {
+		// 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;
+		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, 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);
+	}
+}

src/modules/simulation/sensor_gps_sim/SensorGpsFailureInjector.hpp

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 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>
+
+class SensorGpsFailureInjector
+{
+public:
+	static constexpr int GPS_MAX_INSTANCES = 2;
+
+	SensorGpsFailureInjector();
+
+	void update();
+
+	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; }
+
+private:
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+	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};
+};

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,27 +198,48 @@ 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
+			sensor_gps_s gps1 = sensor_gps;
+
 			device_id.devid_s.address = 1;
-			sensor_gps.device_id = device_id.devid;
+			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);
 
-			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);
+			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)
+{
+	if (!_failure_injector.isBlocked(instance)) {
+		if (_failure_injector.isStuck(instance)) {
+			snapshot.timestamp = hrt_absolute_time();
+			pub.publish(snapshot);
+
+		} else {
+			if (_failure_injector.isWrong(instance)) {
+				gps.latitude_deg  += 1.0;
+				gps.longitude_deg += 1.0;
+			}
+
+			gps.timestamp = hrt_absolute_time();
+			snapshot = gps;
+			pub.publish(gps);
+		}
+	}
+}
+
 int SensorGpsSim::task_spawn(int argc, char *argv[])
 {
 	SensorGpsSim *instance = new SensorGpsSim();

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>
@@ -70,6 +72,9 @@ class SensorGpsSim : public ModuleBase, public ModuleParams, public px4::Schedul
 private:
 	void Run() override;
 
+	void publishWithFailures(int instance, sensor_gps_s gps, sensor_gps_s &snapshot,
+				 uORB::PublicationMulti<sensor_gps_s> &pub);
+
 	// generate white Gaussian noise sample with std=1
 	static float generate_wgn();
 
@@ -85,6 +90,11 @@ class SensorGpsSim : public ModuleBase, public ModuleParams, public px4::Schedul
 
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
+	SensorGpsFailureInjector _failure_injector{};
+
+	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};