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.

Makefile

@@ -525,7 +525,7 @@ px4_sitl_default-clang:
 # To add manual exclusions, append to CLANG_TIDY_EXCLUDE_EXTRA below.
 # Submodules are automatically excluded - no action needed when adding new ones.
 CLANG_TIDY_SUBMODULES := $(shell git config --file .gitmodules --get-regexp path | awk '{print $$2}' | tr '\n' '|' | sed 's/|$$//')
-CLANG_TIDY_EXCLUDE_EXTRA := src/systemcmds/tests|src/examples|src/modules/gyro_fft/CMSIS_5|src/lib/drivers/smbus|src/drivers/gpio|src/modules/commander/failsafe/emscripten|failsafe_test\.dir|\.pb\.cc
+CLANG_TIDY_EXCLUDE_EXTRA := src/systemcmds/tests|src/examples|src/modules/gyro_fft/CMSIS_5|src/lib/drivers/smbus|src/drivers/gpio|src/modules/commander/failsafe/emscripten|failsafe_test\.dir|\.pb\.cc|.*Test\.cpp
 CLANG_TIDY_EXCLUDE := $(CLANG_TIDY_SUBMODULES)|$(CLANG_TIDY_EXCLUDE_EXTRA)
 
 clang-tidy: px4_sitl_default-clang

msg/FailsafeFlags.msg

@@ -59,3 +59,4 @@ bool flight_time_limit_exceeded       # Maximum flight time exceeded
 bool position_accuracy_low            # Position estimate has dropped below threshold, but is currently still declared valid
 bool navigator_failure        	      # Navigator failed to execute a mode
 bool parachute_unhealthy              # Parachute system missing or unhealthy
+bool gps_redundancy_lost              # Number of active GPS receivers dropped below COM_GPS_MIN

src/lib/systemlib/system_params.yaml

@@ -136,6 +136,18 @@ parameters:
       type: boolean
       default: 1
       reboot_required: true
+    SYS_HAS_NUM_GPS:
+      description:
+        short: Control if and how many GPS receivers are expected
+        long: |-
+          0: Disable GPS redundancy check (any number of GPS is acceptable).
+          1-N: Require the presence of N GPS receivers for arming and during flight.
+          If the active count drops below this value in flight, COM_GPS_LOSS_ACT is triggered.
+      type: int32
+      default: 0
+      min: 0
+      max: 2
+      reboot_required: false
     SYS_HAS_MAG:
       description:
         short: Control if and how many magnetometers are expected

src/modules/commander/HealthAndArmingChecks/CMakeLists.txt

@@ -48,6 +48,7 @@ px4_add_library(health_and_arming_checks
 	checks/failureDetectorCheck.cpp
 	checks/flightTimeCheck.cpp
 	checks/geofenceCheck.cpp
+	checks/gpsRedundancyCheck.cpp
 	checks/gyroCheck.cpp
 	checks/homePositionCheck.cpp
 	checks/imuConsistencyCheck.cpp
@@ -78,3 +79,7 @@ add_dependencies(health_and_arming_checks mode_util)
 px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp
 	LINKLIBS health_and_arming_checks mode_util
 	)
+
+px4_add_functional_gtest(SRC GpsRedundancyCheckTest.cpp
+	LINKLIBS health_and_arming_checks mode_util geo
+	)

src/modules/commander/HealthAndArmingChecks/GpsRedundancyCheckTest.cpp

@@ -0,0 +1,217 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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 <gtest/gtest.h>
+
+#include "Common.hpp"
+#include "checks/gpsRedundancyCheck.hpp"
+
+#include <uORB/topics/event.h>
+#include <uORB/topics/sensor_gps.h>
+#include <px4_platform_common/param.h>
+
+using namespace time_literals;
+
+// to run: make tests TESTFILTER=GpsRedundancyCheck
+
+/* EVENT
+ * @skip-file
+ */
+
+// Base position (PX4 SITL default home)
+static constexpr double BASE_LAT = 47.397742;
+static constexpr double BASE_LON = 8.545594;
+
+// 1 deg latitude ≈ 111111m
+// AGREEING:  ~0.4m north  → below gate of 0.785m (0.70m lever arm + 0.085m eph tolerance)
+// DIVERGING: ~2.0m north  → above gate
+static constexpr double AGREEING_LAT  = BASE_LAT + 3.6e-6;
+static constexpr double DIVERGING_LAT = BASE_LAT + 1.8e-5;
+
+class GpsRedundancyCheckTest : public ::testing::Test
+{
+public:
+	static void SetUpTestSuite()
+	{
+		// Advertise the event topic so queued events are not lost
+		orb_advertise(ORB_ID(event), nullptr);
+
+		// Pre-advertise sensor_gps instances 0 and 1 so they are always
+		// available to the check's SubscriptionMultiArray regardless of test order.
+		sensor_gps_s gps{};
+		_pub0 = orb_advertise(ORB_ID(sensor_gps), &gps);
+		int inst;
+		_pub1 = orb_advertise_multi(ORB_ID(sensor_gps), &gps, &inst);
+	}
+
+	void SetUp() override
+	{
+		param_control_autosave(false);
+
+		// Lever arm: GPS0 at +35cm forward, GPS1 at -35cm → expected_d = 0.70m
+		// With both RTK Fixed (eph=0.02m): gate = 0.70 + 3*√(0.02²+0.02²) = 0.785m
+		float f = 0.35f;
+		param_set(param_find("SENS_GPS0_OFFX"), &f);
+		f = 0.f;
+		param_set(param_find("SENS_GPS0_OFFY"), &f);
+		f = -0.35f;
+		param_set(param_find("SENS_GPS1_OFFX"), &f);
+		f = 0.f;
+		param_set(param_find("SENS_GPS1_OFFY"), &f);
+		int i = 0;
+		param_set(param_find("SYS_HAS_NUM_GPS"), &i);
+		param_set(param_find("COM_GPS_LOSS_ACT"), &i);
+
+		// Construct check after params are set so ParamFloat reads correct initial values
+		_check = new GpsRedundancyChecks();
+
+		// Reset both instances to "absent" — device_id=0 is filtered by the check
+		sensor_gps_s empty{};
+		orb_publish(ORB_ID(sensor_gps), _pub0, &empty);
+		orb_publish(ORB_ID(sensor_gps), _pub1, &empty);
+	}
+
+	void TearDown() override
+	{
+		delete _check;
+		_check = nullptr;
+	}
+
+	sensor_gps_s makeGps(double lat, double lon, float eph = 0.02f, uint8_t fix_type = 6)
+	{
+		sensor_gps_s gps{};
+		gps.timestamp         = hrt_absolute_time();
+		gps.device_id         = 1;
+		gps.latitude_deg      = lat;
+		gps.longitude_deg     = lon;
+		gps.altitude_ellipsoid_m = 100.0;
+		gps.eph               = eph;
+		gps.epv               = 0.05f;
+		gps.fix_type          = fix_type;
+		return gps;
+	}
+
+	// Runs one check cycle and returns true if the GPS divergence health warning fired
+	bool hasDivergenceWarning(bool armed)
+	{
+		vehicle_status_s status{};
+		status.arming_state = armed
+				      ? vehicle_status_s::ARMING_STATE_ARMED
+				      : vehicle_status_s::ARMING_STATE_DISARMED;
+		Context context{status};
+		failsafe_flags_s failsafe_flags{};
+		Report reporter{failsafe_flags, 0_s};
+		_check->checkAndReport(context, reporter);
+		return ((uint64_t)reporter.healthResults().warning
+			& (uint64_t)events::px4::enums::health_component_t::gps) != 0;
+	}
+
+	static orb_advert_t _pub0;
+	static orb_advert_t _pub1;
+	GpsRedundancyChecks *_check{nullptr};
+};
+
+orb_advert_t GpsRedundancyCheckTest::_pub0{nullptr};
+orb_advert_t GpsRedundancyCheckTest::_pub1{nullptr};
+
+// No GPS published → active_count=0 → no divergence check
+TEST_F(GpsRedundancyCheckTest, no_gps)
+{
+	EXPECT_FALSE(hasDivergenceWarning(false));
+}
+
+// Only one active receiver → divergence check requires ≥ 2
+TEST_F(GpsRedundancyCheckTest, single_gps)
+{
+	sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
+	orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
+	EXPECT_FALSE(hasDivergenceWarning(false));
+}
+
+// Two receivers ~0.4m apart (within gate of 0.785m) → no warning
+TEST_F(GpsRedundancyCheckTest, two_gps_agreeing_prearm)
+{
+	sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
+	sensor_gps_s gps1 = makeGps(AGREEING_LAT, BASE_LON);
+	orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
+	orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
+	EXPECT_FALSE(hasDivergenceWarning(false));
+}
+
+// Two receivers ~2.0m apart (beyond gate), pre-arm → immediate warning (sustain=0 pre-arm)
+TEST_F(GpsRedundancyCheckTest, two_gps_diverging_prearm)
+{
+	sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
+	sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON);
+	orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
+	orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
+	EXPECT_TRUE(hasDivergenceWarning(false));
+}
+
+// Two receivers ~2.0m apart, in-flight, first call → no warning yet (2s sustain not elapsed)
+TEST_F(GpsRedundancyCheckTest, two_gps_diverging_inflight_not_yet_sustained)
+{
+	sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
+	sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON);
+	orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
+	orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
+	EXPECT_FALSE(hasDivergenceWarning(true));
+}
+
+// GPS1 fix_type=2 (2D only) → treated as inactive → only 1 active receiver → no divergence check
+TEST_F(GpsRedundancyCheckTest, fix_type_below_3_treated_as_inactive)
+{
+	sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
+	sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON, 0.02f, 2);
+	orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
+	orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
+	EXPECT_FALSE(hasDivergenceWarning(false));
+}
+
+// Divergence then recovery: timer resets and warning stops
+TEST_F(GpsRedundancyCheckTest, divergence_clears_after_recovery)
+{
+	// First: diverging pre-arm → warning fires
+	sensor_gps_s gps0 = makeGps(BASE_LAT, BASE_LON);
+	sensor_gps_s gps1 = makeGps(DIVERGING_LAT, BASE_LON);
+	orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
+	orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
+	EXPECT_TRUE(hasDivergenceWarning(false));
+
+	// Then: receivers agree → warning clears
+	gps0 = makeGps(BASE_LAT, BASE_LON);
+	gps1 = makeGps(AGREEING_LAT, BASE_LON);
+	orb_publish(ORB_ID(sensor_gps), _pub0, &gps0);
+	orb_publish(ORB_ID(sensor_gps), _pub1, &gps1);
+	EXPECT_FALSE(hasDivergenceWarning(false));
+}

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -75,6 +75,7 @@
 #include "checks/openDroneIDCheck.hpp"
 #include "checks/trafficAvoidanceCheck.hpp"
 #include "checks/externalChecks.hpp"
+#include "checks/gpsRedundancyCheck.hpp"
 
 class HealthAndArmingChecks : public ModuleParams
 {
@@ -164,6 +165,7 @@ private:
 	VtolChecks _vtol_checks;
 	OffboardChecks _offboard_checks;
 	TrafficAvoidanceChecks _traffic_avoidance_checks;
+	GpsRedundancyChecks _gps_redundancy_checks;
 #ifndef CONSTRAINED_FLASH
 	ExternalChecks _external_checks;
 #endif
@@ -206,5 +208,6 @@ private:
 		&_rc_and_data_link_checks,
 		&_vtol_checks,
 		&_traffic_avoidance_checks,
+		&_gps_redundancy_checks,
 	};
 };

src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp

@@ -0,0 +1,181 @@
+/****************************************************************************
+ *
+ *   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 "gpsRedundancyCheck.hpp"
+
+#include <lib/geo/geo.h>
+
+using namespace time_literals;
+
+// eph is firmware-dependent and not a rigorous 1-sigma, so 3 is a heuristic consistency gate
+// rather than a precise statistical claim. It relaxes the gate automatically when receivers degrade.
+static constexpr float GPS_DIVERGENCE_SIGMA = 3.0f;
+static constexpr uint64_t GPS_DIVERGENCE_SUSTAIN = 2_s; // must be sustained before warning
+
+void GpsRedundancyChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	// Always reset — will be set below only when the condition is active
+	reporter.failsafeFlags().gps_redundancy_lost = false;
+
+	// Separate "online" (present + fresh data) from "fixed" (online + 3D fix).
+	// online_count tracks receivers that are communicating; fixed_count tracks those
+	// suitable for navigation. Keeping them separate lets us emit a more informative
+	// warning: "receiver offline" vs "receiver lost fix".
+	int online_count = 0;
+	int fixed_count = 0;
+	sensor_gps_s fixed_gps[GPS_MAX_INSTANCES] {};
+
+	for (int i = 0; i < _sensor_gps_sub.size(); i++) {
+		sensor_gps_s gps{};
+
+		if (_sensor_gps_sub[i].copy(&gps)
+		    && (gps.device_id != 0)
+		    && (hrt_elapsed_time(&gps.timestamp) < 2_s)) {
+			online_count++;
+
+			if (gps.fix_type >= 3) {
+				fixed_gps[fixed_count++] = gps;
+			}
+		}
+	}
+
+	// Position divergence check: warn if two fixed receivers disagree beyond their combined
+	// uncertainty. The gate is: lever-arm separation + GPS_DIVERGENCE_SIGMA * RMS(eph0, eph1).
+	// Using RMS(eph) means the gate tightens when both receivers are accurate and widens
+	// automatically when one degrades, avoiding false alarms without a hard-coded threshold.
+	// Pre-arm: warn immediately. In-flight: require GPS_DIVERGENCE_SUSTAIN to suppress
+	// transient multipath spikes.
+	bool divergence_active = false;
+
+	if (fixed_count >= 2) {
+		float north, east;
+		get_vector_to_next_waypoint(fixed_gps[0].latitude_deg, fixed_gps[0].longitude_deg,
+					    fixed_gps[1].latitude_deg, fixed_gps[1].longitude_deg,
+					    &north, &east);
+
+		const float divergence_m = sqrtf(north * north + east * east);
+		const float rms_eph = sqrtf(fixed_gps[0].eph * fixed_gps[0].eph + fixed_gps[1].eph * fixed_gps[1].eph);
+		const float dx = _param_gps0_offx.get() - _param_gps1_offx.get();
+		const float dy = _param_gps0_offy.get() - _param_gps1_offy.get();
+		const float expected_d = sqrtf(dx * dx + dy * dy);
+		const float gate_m = expected_d + rms_eph * GPS_DIVERGENCE_SIGMA;
+
+		// Pre-arm: trigger immediately so the operator can decide before takeoff.
+		// In-flight: require sustained divergence to avoid false alarms from transient multipath.
+		const uint64_t sustain = context.isArmed() ? GPS_DIVERGENCE_SUSTAIN : 0_s;
+
+		if (divergence_m > gate_m) {
+			if (_divergence_since == 0) {
+				_divergence_since = hrt_absolute_time();
+			}
+
+			if (hrt_elapsed_time(&_divergence_since) >= sustain) {
+				divergence_active = true;
+				const bool act = (_param_com_gps_loss_act.get() > 0);
+
+				/* EVENT
+				 * @description
+				 * Two GPS receivers report positions that are inconsistent with their reported accuracy.
+				 *
+				 * <profile name="dev">
+				 * Configure the failsafe action with <param>COM_GPS_LOSS_ACT</param>.
+				 * </profile>
+				 */
+				reporter.healthFailure<float, float>(act ? NavModes::All : NavModes::None,
+								     health_component_t::gps,
+								     events::ID("check_gps_position_divergence"),
+								     act ? events::Log::Error : events::Log::Warning,
+								     "GPS receivers disagree: {1:.1}m apart (gate {2:.1}m)",
+								     (double)divergence_m, (double)gate_m);
+			}
+
+		} else {
+			_divergence_since = 0;
+		}
+	}
+
+	// Track the highest fixed count seen — used to detect any GPS loss regardless of SYS_HAS_NUM_GPS
+	if (fixed_count > _peak_fixed_count) {
+		_peak_fixed_count = fixed_count;
+	}
+
+	const int required = _param_sys_has_num_gps.get();
+	const bool below_required = (required > 0 && fixed_count < required);
+	const bool dropped_below_peak = (_peak_fixed_count > 1 && fixed_count < _peak_fixed_count);
+
+	if (!below_required && !dropped_below_peak && !divergence_active) {
+		return;
+	}
+
+	reporter.failsafeFlags().gps_redundancy_lost = below_required || divergence_active;
+
+	if (!below_required && !dropped_below_peak) {
+		return;
+	}
+
+	// act==0: warn only, never blocks arming; act>0: blocks arming and shows red
+	const bool block_arming = below_required && (_param_com_gps_loss_act.get() > 0);
+	const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None;
+	const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning;
+	const uint8_t expected = below_required ? (uint8_t)required : (uint8_t)_peak_fixed_count;
+
+	// Differentiate: if online_count is also low the receiver is offline; otherwise it lost fix.
+	if (online_count < fixed_count || online_count < required) {
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * Configure the minimum required GPS count with <param>SYS_HAS_NUM_GPS</param>.
+		 * Configure the failsafe action with <param>COM_GPS_LOSS_ACT</param>.
+		 * </profile>
+		 */
+		reporter.healthFailure<uint8_t, uint8_t>(nav_modes, health_component_t::gps,
+				events::ID("check_gps_redundancy_offline"),
+				log_level,
+				"GPS receiver offline: {1} of {2} online",
+				(uint8_t)online_count, expected);
+
+	} else {
+		/* EVENT
+		 * @description
+		 * <profile name="dev">
+		 * Configure the minimum required GPS count with <param>SYS_HAS_NUM_GPS</param>.
+		 * Configure the failsafe action with <param>COM_GPS_LOSS_ACT</param>.
+		 * </profile>
+		 */
+		reporter.healthFailure<uint8_t, uint8_t>(nav_modes, health_component_t::gps,
+				events::ID("check_gps_redundancy_no_fix"),
+				log_level,
+				"GPS receiver lost 3D fix: {1} of {2} fixed",
+				(uint8_t)fixed_count, expected);
+	}
+}

src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp

@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 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 "../Common.hpp"
+
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/sensor_gps.h>
+
+class GpsRedundancyChecks : public HealthAndArmingCheckBase
+{
+public:
+	GpsRedundancyChecks() = default;
+	~GpsRedundancyChecks() = default;
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+	static constexpr int GPS_MAX_INSTANCES = 2;
+
+	uORB::SubscriptionMultiArray<sensor_gps_s, GPS_MAX_INSTANCES> _sensor_gps_sub{ORB_ID::sensor_gps};
+
+	int _peak_fixed_count{0};
+
+	hrt_abstime _divergence_since{0};
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamInt<px4::params::SYS_HAS_NUM_GPS>)   _param_sys_has_num_gps,
+					(ParamInt<px4::params::COM_GPS_LOSS_ACT>)  _param_com_gps_loss_act,
+					(ParamFloat<px4::params::SENS_GPS0_OFFX>)  _param_gps0_offx,
+					(ParamFloat<px4::params::SENS_GPS0_OFFY>)  _param_gps0_offy,
+					(ParamFloat<px4::params::SENS_GPS1_OFFX>)  _param_gps1_offx,
+					(ParamFloat<px4::params::SENS_GPS1_OFFY>)  _param_gps1_offy
+				       )
+};

src/modules/commander/commander_params.yaml

@@ -131,6 +131,19 @@ parameters:
         1: Allow arming (with warning)
         2: Allow arming (no warning)
       default: 1
+    COM_GPS_LOSS_ACT:
+      description:
+        short: GPS redundancy loss failsafe mode
+        long: |-
+          Action the system takes when the number of active GPS receivers drops
+          below SYS_HAS_NUM_GPS during flight.
+      type: enum
+      values:
+        0: Warning
+        1: Return
+        2: Land
+        3: Terminate
+      default: 0
     COM_ARM_SWISBTN:
       description:
         short: Arm switch is a momentary button

src/modules/commander/failsafe/failsafe.cpp

@@ -412,6 +412,36 @@ FailsafeBase::ActionOptions Failsafe::fromPosLowActParam(int param_value)
 	return options;
 }
 
+FailsafeBase::ActionOptions Failsafe::fromGpsRedundancyActParam(int param_value)
+{
+	ActionOptions options{};
+
+	switch (gps_redundancy_failsafe_mode(param_value)) {
+	case gps_redundancy_failsafe_mode::Warning:
+	default:
+		options.action = Action::Warn;
+		break;
+
+	case gps_redundancy_failsafe_mode::Return_mode:
+		options.action = Action::RTL;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
+	case gps_redundancy_failsafe_mode::Land_mode:
+		options.action = Action::Land;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
+		break;
+
+	case gps_redundancy_failsafe_mode::Terminate:
+		options.allow_user_takeover = UserTakeoverAllowed::Never;
+		options.action = Action::Terminate;
+		options.clear_condition = ClearCondition::Never;
+		break;
+	}
+
+	return options;
+}
+
 FailsafeBase::ActionOptions Failsafe::fromRemainingFlightTimeLowActParam(int param_value)
 {
 	ActionOptions options{};
@@ -634,6 +664,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 
 	CHECK_FAILSAFE(status_flags, fd_imbalanced_prop, fromImbalancedPropActParam(_param_com_imb_prop_act.get()));
 	CHECK_FAILSAFE(status_flags, fd_motor_failure, fromActuatorFailureActParam(_param_com_actuator_failure_act.get()));
+	CHECK_FAILSAFE(status_flags, gps_redundancy_lost, fromGpsRedundancyActParam(_param_com_gps_loss_act.get()));
 
 
 

src/modules/commander/failsafe/failsafe.h

@@ -157,6 +157,13 @@ private:
 		Return_mode = 3
 	};
 
+	enum class gps_redundancy_failsafe_mode : int32_t {
+		Warning = 0,
+		Return_mode = 1,
+		Land_mode = 2,
+		Terminate = 3,
+	};
+
 	static ActionOptions fromNavDllOrRclActParam(int param_value);
 
 	static ActionOptions fromGfActParam(int param_value);
@@ -168,6 +175,7 @@ private:
 	static ActionOptions fromHighWindLimitActParam(int param_value);
 	static ActionOptions fromPosLowActParam(int param_value);
 	static ActionOptions fromRemainingFlightTimeLowActParam(int param_value);
+	static ActionOptions fromGpsRedundancyActParam(int param_value);
 
 	static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter);
 
@@ -209,7 +217,8 @@ private:
 					(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
 					(ParamInt<px4::params::COM_WIND_MAX_ACT>) _param_com_wind_max_act,
 					(ParamInt<px4::params::COM_FLTT_LOW_ACT>) _param_com_fltt_low_act,
-					(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act
+					(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act,
+					(ParamInt<px4::params::COM_GPS_LOSS_ACT>) _param_com_gps_loss_act
 				       );
 
 };

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,8 +198,54 @@ 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);
+		// --- GPS instance 0 (primary) ---
+		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) {
+			if (!_failure_injector.isBlocked(1)) {
+				sensor_gps_s gps1 = sensor_gps;
+				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);
+
+				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);
+				}
+			}
+		}
 	}
 
 	perf_end(_loop_perf);

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>
@@ -81,9 +83,16 @@ private:
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)};
 
 	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
+	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub2{ORB_ID(sensor_gps)};
 
 	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};
@@ -101,6 +110,8 @@ private:
 	static constexpr float _vel_markov_time{0.54f};
 
 	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::SIM_GPS_USED>) _sim_gps_used
+		(ParamInt<px4::params::SIM_GPS_USED>)      _sim_gps_used,
+		(ParamFloat<px4::params::SENS_GPS1_OFFX>)  _param_gps1_offx,
+		(ParamFloat<px4::params::SENS_GPS1_OFFY>)  _param_gps1_offy
 	)
 };