diff --git a/docs/en/config/safety.md b/docs/en/config/safety.md
index c23197a206..635962f354 100644
--- a/docs/en/config/safety.md
+++ b/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
+
+
+
+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 |
+| ----------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| [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. |
+| [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).
diff --git a/docs/en/releases/main.md b/docs/en/releases/main.md
index 9499850942..0c66826069 100644
--- a/docs/en/releases/main.md
+++ b/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
diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg
index b03e21bd86..156175e590 100644
--- a/msg/FailsafeFlags.msg
+++ b/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
diff --git a/src/lib/systemlib/system_params.yaml b/src/lib/systemlib/system_params.yaml
index ec8b55d8ce..ba38fec97b 100644
--- a/src/lib/systemlib/system_params.yaml
+++ b/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
diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
index ad18a2ad79..a4c7fc16eb 100644
--- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
+++ b/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)
diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp
index 5a5e5fb119..577c2a232a 100644
--- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp
+++ b/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,
};
};
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.cpp
new file mode 100644
index 0000000000..00b5224b70
--- /dev/null
+++ b/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
+
+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
+ *
+ * Configure the minimum required GPS count with SYS_HAS_NUM_GNSS.
+ * Configure the failsafe action with COM_GNSSLOSS_ACT.
+ *
+ */
+ reporter.healthFailure(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
+ *
+ * Configure the minimum required GPS count with SYS_HAS_NUM_GNSS.
+ * Configure the failsafe action with COM_GNSSLOSS_ACT.
+ *
+ */
+ reporter.healthFailure(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.
+ *
+ *
+ * Configure the failsafe action with COM_GNSSLOSS_ACT.
+ * The failsafe action is only triggered when SYS_HAS_NUM_GNSS is set to 2.
+ *
+ */
+ reporter.healthFailure(nav_modes, health_component_t::gps,
+ events::ID("check_gps_position_divergence"),
+ log_level,
+ "GPS receivers disagree by {1:.1}m",
+ (double)divergence_m);
+ }
+}
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.hpp
new file mode 100644
index 0000000000..f4f0b88b22
--- /dev/null
+++ b/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
+#include
+#include
+
+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_sub{ORB_ID::sensor_gps};
+
+ uint8_t _peak_fixed_count{0};
+ systemlib::Hysteresis _divergence_hysteresis;
+
+
+ DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+ (ParamInt) _param_sys_has_num_gnss,
+ (ParamInt) _param_com_gnssloss_act,
+ (ParamFloat) _param_sens_gps0_offx,
+ (ParamFloat) _param_sens_gps0_offy,
+ (ParamFloat) _param_sens_gps1_offx,
+ (ParamFloat) _param_sens_gps1_offy
+ )
+};
diff --git a/src/modules/commander/HealthAndArmingChecks/gnssRedundancyChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/gnssRedundancyChecksTest.cpp
new file mode 100644
index 0000000000..d2cb4fc5ed
--- /dev/null
+++ b/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
+
+#include "checks/gnssRedundancyCheck.hpp"
+
+#include
+#include
+#include
+#include
+
+// 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 _gps0_pub{ORB_ID(sensor_gps)};
+ uORB::PublicationMulti _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
+}
diff --git a/src/modules/commander/commander_params.yaml b/src/modules/commander/commander_params.yaml
index 2e130a7291..cf9c340f1f 100644
--- a/src/modules/commander/commander_params.yaml
+++ b/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
diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp
index a086a574df..163c53e484 100644
--- a/src/modules/commander/failsafe/failsafe.cpp
+++ b/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()));
diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h
index dc7974a830..2fccc955c8 100644
--- a/src/modules/commander/failsafe/failsafe.h
+++ b/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) _param_com_wind_max_act,
(ParamInt) _param_com_fltt_low_act,
(ParamInt) _param_com_pos_low_act,
- (ParamInt) _param_com_arm_odid
+ (ParamInt) _param_com_arm_odid,
+ (ParamInt) _param_com_gnssloss_act
);
};
diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp
index 5bed55df26..d5d9925eb4 100644
--- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp
+++ b/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);
diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp
index a02360917a..4c96805565 100644
--- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp
+++ b/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_pub{ORB_ID(sensor_gps)};
+ uORB::PublicationMulti _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) _sim_gps_used
+ (ParamInt) _sim_gps_used,
+ (ParamFloat) _param_gps1_offx,
+ (ParamFloat) _param_gps1_offy
)
};