feat(safety): GNSS redundancy failsafe (#26863)

* feat(gpsRedundancyCheck): add GPS redundancy failsafe with divergence check

- Monitors GPS count and triggers configurable failsafe (COM_GPS_LOSS_ACT) when count drops below SYS_HAS_NUM_GPS
- Tracks online (present+fresh) and fixed (3D fix) receivers separately; emits "receiver offline" vs "receiver lost fix"
- Detects position divergence between two receivers against combined RMS eph uncertainty plus lever-arm separation
- Pre-arm warns immediately; in-flight requires 2s sustained divergence to suppress multipath false alarms
- Adds GpsRedundancyCheckTest functional test suite

New parameters: SYS_HAS_NUM_GPS, COM_GPS_LOSS_ACT

* feat(sensor_gps_sim): publish second GPS instance using SENS_GPS1 lever arm params

When SENS_GPS1_OFFX or SENS_GPS1_OFFY is non-zero, publish a second sensor_gps instance offset by those values from the vehicle position.

fix(sensor_gps_sim): give second instance distinct device_id

Both simulated GPS instances previously shared the same device_id (address 0x00). This prevented testing the device-ID matching path in SITL since both slots would match the same receiver.

* refactor(gpsRedundancyCheck): address code review feedback

* refactor(gpsRedundancyCheck): address code review feedback

* docs: add GNSS check failsafe documentation

Update safety.md and releases/main.md to document the new GNSS check
failsafe (SYS_HAS_NUM_GNSS, COM_GPS_LOSS_ACT) introduced in PX4.

* docs(update): Subedit to taste

* refactor(gps): move GNSS redundancy detection into sensors module

Add GnssRedundancyStatus topic and GnssRedundancyMonitor in
vehicle_gps_position. Commander's gpsRedundancyCheck becomes a thin
consumer of the new topic. Detection lives with blending/fallback in
one module.

Also rename COM_GPS_LOSS_ACT -> COM_GNSS_LSS_ACT.

* docs(safety): clarify GNSS failsafe wording and rename COM_GNSS_LSS_ACT

* refactor(failsafe): consistent default case as fallback for existing option

* Rename COM_GNSS_LSS_ACT -> COM_GNSSLOSS_ACT

for readability

* fix(gnssRedundancyCheck): move logic back into the commander checks and various improvement suggestions

- Rename to GNSS instead of gps
- Use hysteresis
- Small logic refactorings
- Adapt unit tests to different interface
- User reporting on which GPS is offline or doesn't have a fix

* docs(gnssRedundancyCheck): simplify explanations

* refactor(gnssRedundancyCheck): update year numbers in copyright

---------

Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>

docs/en/config/safety.md

@@ -229,6 +229,23 @@ In Fixed-wing, the position estimate is never strictly invalidated as long as we
 
 Note that if there is no horizontal aiding source anymore, the position estimate is invalidated after `EKF2_NOAID_TOUT`, and the standard position loss failsafe applies.
 
+### GNSS Check Failsafe
+
+<Badge type="tip" text="PX4 v1.18" />
+
+Triggers on either of:
+
+- **Count drop**: receivers with a 3D fix drop below [SYS_HAS_NUM_GNSS](#SYS_HAS_NUM_GNSS). No failsafe action when `SYS_HAS_NUM_GNSS=0` (default).
+- **Position divergence**: two receivers disagree beyond their expected separation (configured via [SENS_GPS0_OFFX/Y](../advanced_config/parameter_reference.md#SENS_GPS0_OFFX), [SENS_GPS1_OFFX/Y](../advanced_config/parameter_reference.md#SENS_GPS1_OFFX)) plus reported accuracy. Only triggers a failsafe action if `SYS_HAS_NUM_GNSS=2`.
+
+At least a warning is emitted, additional failsafe actions can be configured using [COM_GNSSLOSS_ACT](#COM_GNSSLOSS_ACT).
+Loss of a single GPS when none are required is handled by other GPS health checks.
+
+| Parameter                                                                                                   | Description                                                                                                                                                   |
+| ----------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| <a id="SYS_HAS_NUM_GNSS"></a>[SYS_HAS_NUM_GNSS](../advanced_config/parameter_reference.md#SYS_HAS_NUM_GNSS) | Number of usable GNSS receivers required for arming and flight. If two are required then they also need to be consistent. |
+| <a id="COM_GNSSLOSS_ACT"></a>[COM_GNSSLOSS_ACT](../advanced_config/parameter_reference.md#COM_GNSSLOSS_ACT) | Failsafe action when a GNSS failure is detected. Actions other than a warning also lead to arming being blocked. |
+
 ## Offboard Loss Failsafe
 
 The _Offboard Loss Failsafe_ is triggered if the offboard link is lost while under [Offboard control](../flight_modes/offboard.md).

docs/en/releases/main.md

@@ -53,6 +53,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
 ### Safety
 
 - Rotary-wing vehicles now support uncommanded altitude loss detection: if the vehicle descends more than [FD_ALT_LOSS](../advanced_config/parameter_reference.md#FD_ALT_LOSS) meters below its setpoint in altitude-controlled flight, flight termination (and parachute deployment) is triggered. See [Altitude Loss Trigger](../config/safety.md#altitude-loss-trigger). ([PX4-Autopilot#26837](https://github.com/PX4/PX4-Autopilot/pull/26837))
+- [GNSS check failsafe](../config/safety.md#gnss-check-failsafe): new failsafe that monitors the number of usable GNSS receivers with a 3D fix and their position consistency. The required number of receivers is set via [SYS_HAS_NUM_GNSS](../advanced_config/parameter_reference.md#SYS_HAS_NUM_GNSS) and the failsafe action via [COM_GNSSLOSS_ACT](../advanced_config/parameter_reference.md#COM_GNSSLOSS_ACT). ([PX4-Autopilot#26863](https://github.com/PX4/PX4-Autopilot/pull/26863))
 
 ### Estimation
 

msg/FailsafeFlags.msg

@@ -61,3 +61,4 @@ bool position_accuracy_low            # Position estimate has dropped below thre
 bool navigator_failure        	      # Navigator failed to execute a mode
 bool parachute_unhealthy              # Parachute system missing or unhealthy
 bool remote_id_unhealthy              # Remote ID (Open Drone ID) system missing or unhealthy
+bool gnss_lost                        # Active GNSS count dropped below SYS_HAS_NUM_GNSS, or two receivers report inconsistent positions

src/lib/systemlib/system_params.yaml

@@ -136,6 +136,20 @@ parameters:
       type: boolean
       default: 1
       reboot_required: true
+    SYS_HAS_NUM_GNSS:
+      description:
+        short: Control if and how many GNSS receivers are required
+        long: |-
+          0: No minimum receiver count required. Position divergence between two receivers
+          and loss of a GNSS receiver still produce a warning but never trigger the
+          COM_GNSSLOSS_ACT failsafe action.
+          1-N: Require the presence of N GNSS receivers for arming and during flight.
+          If the active count drops below this value in flight, COM_GNSSLOSS_ACT is triggered.
+          When set to 2, position divergence between the two receivers also triggers COM_GNSSLOSS_ACT.
+      type: int32
+      default: 0
+      min: 0
+      max: 2
     SYS_HAS_MAG:
       description:
         short: Control if and how many magnetometers are expected

src/modules/commander/HealthAndArmingChecks/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022-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
@@ -48,6 +48,7 @@ px4_add_library(health_and_arming_checks
 	checks/failureDetectorCheck.cpp
 	checks/flightTimeCheck.cpp
 	checks/geofenceCheck.cpp
+	checks/gnssRedundancyCheck.cpp
 	checks/gyroCheck.cpp
 	checks/homePositionCheck.cpp
 	checks/imuConsistencyCheck.cpp
@@ -74,7 +75,10 @@ px4_add_library(health_and_arming_checks
 )
 set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/esc_check_params.yaml)
 add_dependencies(health_and_arming_checks mode_util)
+target_link_libraries(health_and_arming_checks PRIVATE geo hysteresis)
 
 px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp
 	LINKLIBS health_and_arming_checks mode_util
 	)
+
+px4_add_functional_gtest(SRC gnssRedundancyChecksTest.cpp LINKLIBS health_and_arming_checks mode_util)

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -75,6 +75,7 @@
 #include "checks/openDroneIDCheck.hpp"
 #include "checks/trafficAvoidanceCheck.hpp"
 #include "checks/externalChecks.hpp"
+#include "checks/gnssRedundancyCheck.hpp"
 
 class HealthAndArmingChecks : public ModuleParams
 {
@@ -164,6 +165,7 @@ private:
 	VtolChecks _vtol_checks;
 	OffboardChecks _offboard_checks;
 	TrafficAvoidanceChecks _traffic_avoidance_checks;
+	GnssRedundancyChecks _gnss_redundancy_checks;
 #ifndef CONSTRAINED_FLASH
 	ExternalChecks _external_checks;
 #endif
@@ -206,5 +208,6 @@ private:
 		&_rc_and_data_link_checks,
 		&_vtol_checks,
 		&_traffic_avoidance_checks,
+		&_gnss_redundancy_checks,
 	};
 };

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

@@ -0,0 +1,161 @@
+/****************************************************************************
+ *
+ *   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 "gnssRedundancyCheck.hpp"
+#include <lib/geo/geo.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+GnssRedundancyChecks::GnssRedundancyChecks()
+{
+	_divergence_hysteresis.set_hysteresis_time_from(false, 2_s);
+	_divergence_hysteresis.set_hysteresis_time_from(true, 2_s);
+}
+
+void GnssRedundancyChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	bool gps_online[GPS_MAX_INSTANCES] {};
+	bool gps_has_fix[GPS_MAX_INSTANCES] {};
+	uint8_t fixed_count = 0;
+	sensor_gps_s fixed_gps[GPS_MAX_INSTANCES] {};
+
+	for (int i = 0; i < GPS_MAX_INSTANCES; i++) {
+		sensor_gps_s gps{};
+
+		if (_sensor_gps_sub[i].copy(&gps)
+		    && (gps.device_id != 0)
+		    && (hrt_elapsed_time(&gps.timestamp) < 1_s)) {
+			gps_online[i] = true;
+
+			if (gps.fix_type >= 3) {
+				gps_has_fix[i] = true;
+				fixed_gps[fixed_count++] = gps;
+			}
+		}
+	}
+
+	// Track the highest fixed count seen to warn about GNSS loss regardless of SYS_HAS_NUM_GNSS
+	if (fixed_count > _peak_fixed_count) {
+		_peak_fixed_count = fixed_count;
+	}
+
+	// Position divergence check: flag if two fixed receivers disagree beyond their
+	// combined uncertainty. Gate = 3 * RSS(eph), centered on the expected lever-arm separation.
+	float divergence_m = 0.f;
+
+	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 separation_m = Vector2f(north, east).length();
+
+		const Vector2f offset0(_param_sens_gps0_offx.get(), _param_sens_gps0_offy.get());
+		const Vector2f offset1(_param_sens_gps1_offx.get(), _param_sens_gps1_offy.get());
+		const float expected_d = (offset0 - offset1).length();
+		divergence_m = fabsf(separation_m - expected_d);
+		// Use quadrature sum for standard deviation of the difference taking the firmware dependent eph as standard deviation
+		// and a heuristic factor of 3 because then it's unlikely just noise.
+		const float divergence_gate_m = 3.f * Vector2f(fixed_gps[0].eph, fixed_gps[1].eph).length();
+		_divergence_hysteresis.set_state_and_update(divergence_m > divergence_gate_m, hrt_absolute_time());
+
+	} else {
+		_divergence_hysteresis.set_state_and_update(false, hrt_absolute_time());
+	}
+
+	const bool below_required = (_param_sys_has_num_gnss.get() > 0) && (fixed_count < _param_sys_has_num_gnss.get());
+	const bool dropped_below_peak = (_peak_fixed_count > 1) && (fixed_count < _peak_fixed_count);
+	const bool act_configured = (_param_com_gnssloss_act.get() > 0);
+
+	// Divergence triggers the failsafe only when the operator explicitly expects two
+	// receivers (SYS_HAS_NUM_GNSS >= 2); otherwise it remains a warning.
+	const bool divergence_triggers_failsafe = _divergence_hysteresis.get_state() && (_param_sys_has_num_gnss.get() >= 2);
+
+	reporter.failsafeFlags().gnss_lost = below_required || divergence_triggers_failsafe;
+
+	if (below_required || dropped_below_peak) {
+		const bool block_arming = below_required && act_configured;
+		const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None;
+		const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning;
+		const int expected = below_required ? _param_sys_has_num_gnss.get() : _peak_fixed_count;
+
+		for (int i = 0; i < expected; i++) {
+			if (!gps_online[i]) {
+				/* EVENT
+				 * @description
+				 * <profile name="dev">
+				 * Configure the minimum required GPS count with <param>SYS_HAS_NUM_GNSS</param>.
+				 * Configure the failsafe action with <param>COM_GNSSLOSS_ACT</param>.
+				 * </profile>
+				 */
+				reporter.healthFailure<uint8_t>(nav_modes, health_component_t::gps,
+								events::ID("check_gnss_receiver_offline"),
+								log_level, "GPS {1} offline", (uint8_t)i);
+
+			} else if (!gps_has_fix[i]) {
+				/* EVENT
+				 * @description
+				 * <profile name="dev">
+				 * Configure the minimum required GPS count with <param>SYS_HAS_NUM_GNSS</param>.
+				 * Configure the failsafe action with <param>COM_GNSSLOSS_ACT</param>.
+				 * </profile>
+				 */
+				reporter.healthFailure<uint8_t>(nav_modes, health_component_t::gps,
+								events::ID("check_gnss_receiver_no_fix"),
+								log_level, "GPS {1} lost fix", (uint8_t)i);
+			}
+		}
+	}
+
+	if (_divergence_hysteresis.get_state()) {
+		const bool block_arming = divergence_triggers_failsafe && act_configured;
+		const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None;
+		const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning;
+
+		/* EVENT
+		 * @description
+		 * Two GNSS receivers report positions that are inconsistent with their reported accuracy.
+		 *
+		 * <profile name="dev">
+		 * Configure the failsafe action with <param>COM_GNSSLOSS_ACT</param>.
+		 * The failsafe action is only triggered when <param>SYS_HAS_NUM_GNSS</param> is set to 2.
+		 * </profile>
+		 */
+		reporter.healthFailure<float>(nav_modes, health_component_t::gps,
+					      events::ID("check_gps_position_divergence"),
+					      log_level,
+					      "GPS receivers disagree by {1:.1}m",
+					      (double)divergence_m);
+	}
+}

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

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   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 "../Common.hpp"
+#include <lib/hysteresis/hysteresis.h>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/sensor_gps.h>
+
+class GnssRedundancyChecks : public HealthAndArmingCheckBase
+{
+public:
+	GnssRedundancyChecks();
+	~GnssRedundancyChecks() = 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};
+
+	uint8_t _peak_fixed_count{0};
+	systemlib::Hysteresis _divergence_hysteresis;
+
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+					(ParamInt<px4::params::SYS_HAS_NUM_GNSS>) _param_sys_has_num_gnss,
+					(ParamInt<px4::params::COM_GNSSLOSS_ACT>) _param_com_gnssloss_act,
+					(ParamFloat<px4::params::SENS_GPS0_OFFX>) _param_sens_gps0_offx,
+					(ParamFloat<px4::params::SENS_GPS0_OFFY>) _param_sens_gps0_offy,
+					(ParamFloat<px4::params::SENS_GPS1_OFFX>) _param_sens_gps1_offx,
+					(ParamFloat<px4::params::SENS_GPS1_OFFY>) _param_sens_gps1_offy
+				       )
+};

src/modules/commander/HealthAndArmingChecks/gnssRedundancyChecksTest.cpp

@@ -0,0 +1,219 @@
+/****************************************************************************
+ *
+ *   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 <gtest/gtest.h>
+
+#include "checks/gnssRedundancyCheck.hpp"
+
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/param.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_gps.h>
+
+// to run: make tests TESTFILTER=gnssRedundancyChecks
+
+/* EVENT
+ * @skip-file
+ */
+
+// Base position (PX4 SITL default home).
+static constexpr double BASE_LAT = 47.397742;
+static constexpr double BASE_LON = 8.545594;
+
+// With GPS0 at +35cm X and GPS1 at -35cm X, expected_d = 0.70m.
+// With RTK eph = 0.02m: gate = 3 * sqrt(0.02² + 0.02²) ≈ 0.085m.
+// AGREEING_LAT puts GPS1 ~0.70m north of GPS0 so separation ≈ expected_d.
+// DIVERGING_FAR_LAT puts GPS1 ~2.0m north, well outside the gate.
+static constexpr double AGREEING_LAT     = BASE_LAT + 6.3e-6;  // ~0.70m north
+static constexpr double DIVERGING_FAR_LAT = BASE_LAT + 1.8e-5; // ~2.0m north
+
+class GnssRedundancyChecksTest : public ::testing::Test
+{
+public:
+	void SetUp() override
+	{
+		param_control_autosave(false);
+
+		// Reset params that tests modify so state doesn't leak between tests.
+		param_reset(param_find("SYS_HAS_NUM_GNSS"));
+		param_reset(param_find("COM_GNSSLOSS_ACT"));
+
+		// Set lever arms so expected_d = 0.70m, enabling the "too close" direction of divergence detection.
+		float v = 0.35f;  param_set(param_find("SENS_GPS0_OFFX"), &v);
+		v = -0.35f;       param_set(param_find("SENS_GPS1_OFFX"), &v);
+
+		// Claim uORB instances 0 and 1 before the check subscribes on first copy().
+		sensor_gps_s empty{};
+		_gps0_pub.publish(empty);
+		_gps1_pub.publish(empty);
+	}
+
+	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.eph           = eph;
+		gps.fix_type      = fix_type;
+		return gps;
+	}
+
+	// Run the check and store results in _failsafe_flags and _health_warning_gps.
+	void runCheck(bool armed = false)
+	{
+		vehicle_status_s status{};
+
+		if (armed) { status.arming_state = vehicle_status_s::ARMING_STATE_ARMED; }
+
+		_check.updateParams();
+		Context context{status};
+		_failsafe_flags = {};
+		Report reporter{_failsafe_flags, 0};
+		_check.checkAndReport(context, reporter);
+		// Capture any GPS health issue regardless of log level (Warning or Error).
+		_health_warning_gps = (reporter.healthResults().warning | reporter.healthResults().error) & health_component_t::gps;
+	}
+
+	uORB::PublicationMulti<sensor_gps_s> _gps0_pub{ORB_ID(sensor_gps)};
+	uORB::PublicationMulti<sensor_gps_s> _gps1_pub{ORB_ID(sensor_gps)};
+	failsafe_flags_s  _failsafe_flags{};
+	bool              _health_warning_gps{false};
+	GnssRedundancyChecks _check;
+};
+
+// No GPS data → no flags.
+TEST_F(GnssRedundancyChecksTest, NoGpsNoFlags)
+{
+	runCheck();
+	EXPECT_FALSE(_failsafe_flags.gnss_lost);
+	EXPECT_FALSE(_health_warning_gps);
+}
+
+// One receiver fixed, SYS_HAS_NUM_GNSS not configured → no failsafe.
+TEST_F(GnssRedundancyChecksTest, SingleGpsNoFailsafe)
+{
+	_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
+	runCheck();
+	EXPECT_FALSE(_failsafe_flags.gnss_lost);
+}
+
+// Two receivers at expected lever-arm separation → no divergence.
+TEST_F(GnssRedundancyChecksTest, TwoGpsAgreeingNoFlags)
+{
+	_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
+	_gps1_pub.publish(makeGps(AGREEING_LAT, BASE_LON));
+	runCheck();
+	EXPECT_FALSE(_failsafe_flags.gnss_lost);
+	EXPECT_FALSE(_health_warning_gps);
+}
+
+// Two receivers at the same position while a 0.70m lever arm is configured →
+// divergence detected (separation = 0, expected_d = 0.70m, deviation = 0.70m >> gate).
+// This exercises the "too close" direction of the improved check.
+TEST_F(GnssRedundancyChecksTest, TwoGpsTooCloseDivergenceDetected)
+{
+	_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
+	_gps1_pub.publish(makeGps(BASE_LAT, BASE_LON));
+
+	// First call starts the hysteresis timer, no flag yet.
+	runCheck();
+	EXPECT_FALSE(_failsafe_flags.gnss_lost);
+
+	// Hysteresis fires after ~2 s; calling immediately again stays false.
+	runCheck();
+	EXPECT_FALSE(_failsafe_flags.gnss_lost);
+}
+
+// Two receivers too far apart → hysteresis timer starts but has not elapsed on first call.
+TEST_F(GnssRedundancyChecksTest, TwoGpsDivergingFarNotYetSustained)
+{
+	_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
+	_gps1_pub.publish(makeGps(DIVERGING_FAR_LAT, BASE_LON));
+	runCheck();
+	EXPECT_FALSE(_failsafe_flags.gnss_lost);
+}
+
+// After divergence the receivers recover → hysteresis resets, no flag.
+TEST_F(GnssRedundancyChecksTest, TwoGpsDivergingClearsOnRecovery)
+{
+	_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
+	_gps1_pub.publish(makeGps(DIVERGING_FAR_LAT, BASE_LON));
+	runCheck();
+
+	_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
+	_gps1_pub.publish(makeGps(AGREEING_LAT, BASE_LON));
+	runCheck();
+	EXPECT_FALSE(_failsafe_flags.gnss_lost);
+	EXPECT_FALSE(_health_warning_gps);
+}
+
+// fix_type < 3 is not counted as a fixed receiver → no divergence check triggered.
+TEST_F(GnssRedundancyChecksTest, FixTypeBelow3NotCounted)
+{
+	_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON, 0.02f, 6));
+	_gps1_pub.publish(makeGps(DIVERGING_FAR_LAT, BASE_LON, 0.02f, 2));
+	runCheck();
+	EXPECT_FALSE(_failsafe_flags.gnss_lost);
+	EXPECT_FALSE(_health_warning_gps);
+}
+
+// SYS_HAS_NUM_GNSS = 2, COM_GNSSLOSS_ACT > 0, only one receiver fixed → gnss_lost.
+TEST_F(GnssRedundancyChecksTest, BelowRequiredSetsGnssLost)
+{
+	int required = 2;   param_set(param_find("SYS_HAS_NUM_GNSS"), &required);
+	int act = 1;        param_set(param_find("COM_GNSSLOSS_ACT"), &act);
+
+	_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
+	runCheck();
+	EXPECT_TRUE(_failsafe_flags.gnss_lost);
+	EXPECT_TRUE(_health_warning_gps);
+}
+
+// After seeing two fixed receivers, losing one emits a health warning
+// even when SYS_HAS_NUM_GNSS is not set (dropped_below_peak path).
+TEST_F(GnssRedundancyChecksTest, DroppedBelowPeakSetsHealthWarning)
+{
+	_gps0_pub.publish(makeGps(BASE_LAT, BASE_LON));
+	_gps1_pub.publish(makeGps(AGREEING_LAT, BASE_LON));
+	runCheck();
+	EXPECT_FALSE(_health_warning_gps); // both present, no warning
+
+	// GPS1 disappears.
+	sensor_gps_s gone{};
+	_gps1_pub.publish(gone); // device_id = 0 → treated as absent
+	runCheck();
+	EXPECT_TRUE(_health_warning_gps);
+	EXPECT_FALSE(_failsafe_flags.gnss_lost); // no failsafe without COM_GNSSLOSS_ACT + below_required
+}

src/modules/commander/commander_params.yaml

@@ -131,6 +131,22 @@ parameters:
         1: Allow arming (with warning)
         2: Allow arming (no warning)
       default: 1
+    COM_GNSSLOSS_ACT:
+      description:
+        short: GNSS loss failsafe mode
+        long: |-
+          Action the system takes when a GNSS failure is detected during flight.
+          Triggers when the active GNSS count drops below SYS_HAS_NUM_GNSS (disabled
+          when SYS_HAS_NUM_GNSS=0), or when two receivers report positions inconsistent
+          with their reported accuracy and SYS_HAS_NUM_GNSS=2 (otherwise warning only).
+          Any value above Warning also blocks arming.
+      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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-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
@@ -46,6 +46,7 @@ FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value)
 
 	switch (gcs_connection_loss_failsafe_mode(param_value)) {
 	case gcs_connection_loss_failsafe_mode::Disabled:
+	default:
 		options.action = Action::None;
 		break;
 
@@ -74,10 +75,6 @@ FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value)
 		options.allow_user_takeover = UserTakeoverAllowed::Never;
 		options.action = Action::Disarm;
 		break;
-
-	default:
-		options.action = Action::None;
-		break;
 	}
 
 	return options;
@@ -93,6 +90,7 @@ FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value)
 		break;
 
 	case geofence_violation_action::Warning:
+	default:
 		options.action = Action::Warn;
 		break;
 
@@ -117,10 +115,6 @@ FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value)
 		options.action = Action::Land;
 		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
 		break;
-
-	default:
-		options.action = Action::Warn;
-		break;
 	}
 
 	return options;
@@ -165,8 +159,8 @@ FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value
 	ActionOptions options{};
 
 	switch (battery_warning) {
-	default:
 	case battery_status_s::WARNING_NONE:
+	default:
 		options.action = Action::None;
 		break;
 
@@ -309,6 +303,7 @@ FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value)
 		break;
 
 	case command_after_high_wind_failsafe::Warning:
+	default:
 		options.action = Action::Warn;
 		break;
 
@@ -333,10 +328,6 @@ FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value)
 		options.action = Action::Land;
 		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
 		break;
-
-	default:
-		options.action = Action::Warn;
-		break;
 	}
 
 	return options;
@@ -353,6 +344,7 @@ FailsafeBase::ActionOptions Failsafe::fromPosLowActParam(int param_value)
 		break;
 
 	case command_after_pos_low_failsafe::Warning:
+	default:
 		options.action = Action::Warn;
 		break;
 
@@ -376,10 +368,36 @@ FailsafeBase::ActionOptions Failsafe::fromPosLowActParam(int param_value)
 		options.action = Action::Land;
 		options.clear_condition = ClearCondition::WhenConditionClears;
 		break;
+	}
 
+	return options;
+}
+
+FailsafeBase::ActionOptions Failsafe::fromGnssLossActParam(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;
@@ -642,6 +660,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 
 	CHECK_FAILSAFE(status_flags, fd_imbalanced_prop, Action::Warn);
 	CHECK_FAILSAFE(status_flags, fd_motor_failure, fromActuatorFailureActParam(_param_com_actuator_failure_act.get()));
+	CHECK_FAILSAFE(status_flags, gnss_lost, fromGnssLossActParam(_param_com_gnssloss_act.get()));
 
 
 

src/modules/commander/failsafe/failsafe.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-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
@@ -159,6 +159,13 @@ private:
 		Terminate = 5,
 	};
 
+	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);
@@ -170,6 +177,7 @@ private:
 	static ActionOptions fromPosLowActParam(int param_value);
 	static ActionOptions fromRemainingFlightTimeLowActParam(int param_value);
 	static ActionOptions fromOdidFailActParam(int param_value);
+	static ActionOptions fromGnssLossActParam(int param_value);
 
 	static bool isFailsafeIgnored(uint8_t user_intended_mode, int32_t exception_mask_parameter);
 
@@ -210,7 +218,8 @@ private:
 					(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_ARM_ODID>) _param_com_arm_odid
+					(ParamInt<px4::params::COM_ARM_ODID>) _param_com_arm_odid,
+					(ParamInt<px4::params::COM_GNSSLOSS_ACT>) _param_com_gnssloss_act
 				       );
 
 };

src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-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
@@ -198,6 +198,20 @@ void SensorGpsSim::Run()
 
 		sensor_gps.timestamp = hrt_absolute_time();
 		_sensor_gps_pub.publish(sensor_gps);
+
+		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.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);
+		}
 	}
 
 	perf_end(_loop_perf);

src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-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
@@ -81,6 +81,7 @@ 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")};
 
@@ -101,6 +102,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
 	)
 };