fix(gz_bridge): support GPS failure injection (#28398)

* fix(gz_bridge): support GPS failure injection

Modern Gazebo publishes sensor_gps directly from GZBridge, bypassing the shared failure-injection processing. Route each NavSat sample through process_gnss using the actual uORB publication instance so off, stuck, wrong, and recovery work for the addressed receiver.

Add functional regression coverage for the real GZBridge NavSat callback and sensor_gps publication path, plus recovery coverage for the shared GNSS processor.

Fixes #22296

Assisted-by: Codex:gpt-5
Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu>

* docs: gps failure injection is available on Gazebo

The gz_bridge now applies the shared GNSS failure state to the
simulator's NavSat data, so the table entry and the SIM_GZ_EN_GPS
workaround note are out of date.

Assisted-by: Claude:claude-fable-5
Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu>

---------

Signed-off-by: Saibernard Yogendran <bernie97@seas.upenn.edu>

docs/en/debug/failure_injection.md

@@ -21,7 +21,7 @@ A `—` means the module still accepts the command, but no consumer applies it i
 | `mag`             | `off`, `stuck`          | `off`, `stuck`          | `off`, `stuck`                               | `off`, `stuck`          |
 | `baro`            | `off`, `stuck`          | `off`, `stuck`          | `off`, `stuck`                               | `off`, `stuck`          |
 | `distance_sensor` | `off`, `stuck`          | `off`, `stuck`          | `off`, `stuck`                               | `off`, `stuck`          |
-| `gps`             | —                       | `off`, `stuck`, `wrong` | `off`, `stuck`, `wrong`                      | `off`, `stuck`, `wrong` |
+| `gps`             | `off`, `stuck`, `wrong` | `off`, `stuck`, `wrong` | `off`, `stuck`, `wrong`                      | `off`, `stuck`, `wrong` |
 | `airspeed`        | `off`, `stuck`, `wrong` | —                       | `off`, `wrong`                               | —                       |
 | `vio`             | —                       | —                       | `off`                                        | —                       |
 | `battery`         | `off`, `wrong`          | `off`, `wrong`          | `off`, `wrong`                               | `off`, `wrong`          |
@@ -35,8 +35,7 @@ A `—` means the module still accepts the command, but no consumer applies it i
 
 ::: info
 
-- `gps off | stuck | wrong` on Gazebo (Gz): only available if [SIM_GZ_EN_GPS](../advanced_config/parameter_reference.md#SIM_GZ_EN_GPS) is set to `0` to use the injectable simulated-GPS module.
-  By default Gazebo publishes GPS from the simulator's own GNSS sensor (`SIM_GZ_EN_GPS` = 1), which is not injectable.
+- `gps off | stuck | wrong` on Gazebo (Gz): applied to the GNSS data from the simulator's own sensor as well as to the simulated-GPS module ([SIM_GZ_EN_GPS](../advanced_config/parameter_reference.md#SIM_GZ_EN_GPS) `0`).
 - `airspeed off | stuck | wrong` on Gazebo (Gz): only injectable when airspeed is provided by the simulated-airspeed module ([SENS_EN_ARSPDSIM](../advanced_config/parameter_reference.md#SENS_EN_ARSPDSIM)); worlds that model an airspeed sensor directly are not injected.
 - `battery wrong` reports the remaining charge just below the [SYS_FAIL_BAT_LVL](../advanced_config/parameter_reference.md#SYS_FAIL_BAT_LVL) warning threshold to trigger the battery failsafe; `off` stops publishing the battery status entirely.
 - `traffic off` suppresses incoming reports and marks the ADS-B/FLARM link unhealthy.

src/modules/failure_injection_manager/FailureInjectionConfigTest.cpp

@@ -422,6 +422,31 @@ TEST(FailureInjectionConfig, ProcessGnssStuckReplaysLastGoodSample)
 	EXPECT_EQ(moved.timestamp_sample, 1900u);
 }
 
+TEST(FailureInjectionConfig, ProcessGnssRecoveryUsesLiveSample)
+{
+	Config config;
+	Stuck<sensor_gps_s> stuck;
+
+	sensor_gps_s initial = clean_gps();
+	EXPECT_TRUE(process_gnss(config, 0, initial, stuck));
+
+	config.set(make_config(GPS, 0x1, STUCK));
+	sensor_gps_s frozen = clean_gps();
+	frozen.latitude_deg = 48.0;
+	EXPECT_TRUE(process_gnss(config, 0, frozen, stuck));
+	EXPECT_DOUBLE_EQ(frozen.latitude_deg, initial.latitude_deg);
+
+	config.set(make_config(GPS, 0x1, OFF));
+	sensor_gps_s suppressed = clean_gps();
+	EXPECT_FALSE(process_gnss(config, 0, suppressed, stuck));
+
+	config.set(failure_injection_s{});
+	sensor_gps_s recovered = clean_gps();
+	recovered.latitude_deg = 48.0;
+	EXPECT_TRUE(process_gnss(config, 0, recovered, stuck));
+	EXPECT_DOUBLE_EQ(recovered.latitude_deg, 48.0);
+}
+
 // ===========================================================================
 // process_esc(): ESC Off / Wrong on the multi-instance esc_status
 // ===========================================================================

src/modules/simulation/gz_bridge/CMakeLists.txt

@@ -91,6 +91,11 @@ if (gz-transport_FOUND)
 	target_include_directories(modules__simulation__gz_bridge PUBLIC px4_gz_msgs)
 	target_link_libraries(modules__simulation__gz_bridge PUBLIC px4_gz_msgs)
 
+	px4_add_functional_gtest(
+		SRC GZBridgeTest.cpp
+		LINKLIBS modules__simulation__gz_bridge ${GZ_TRANSPORT_TARGET} geo
+	)
+
 	include(ExternalProject)
 	ExternalProject_Add(gz
 		SOURCE_DIR ${PX4_SOURCE_DIR}/Tools/simulation/gz

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -675,6 +675,7 @@ void GZBridge::addGpsNoise(double &latitude, double &longitude, double &altitude
 void GZBridge::navSatCallback(const gz::msgs::NavSat &msg)
 {
 	const uint64_t timestamp = hrt_absolute_time();
+	_failure_config.update();
 
 	// initialize gps position
 	if (!_pos_ref.isInitialized()) {
@@ -758,7 +759,9 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &msg)
 	sensor_gps.vel_ned_valid = true;
 	sensor_gps.satellites_used = _sim_gps_used.get();
 
-	_sensor_gps_pub.publish(sensor_gps);
+	if (failure_injection::process_gnss(_failure_config, _sensor_gps_pub.get_instance(), sensor_gps, _gps_stuck)) {
+		_sensor_gps_pub.publish(sensor_gps);
+	}
 }
 
 void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg)

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -50,6 +50,7 @@
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 #include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
 #include <lib/drivers/barometer/PX4Barometer.hpp>
+#include <lib/failure_injection/FailureInjection.hpp>
 #include <lib/geo/geo.h>
 #include <systemlib/system_time_source.h>
 
@@ -108,6 +109,7 @@ public:
 	int print_status() override;
 
 private:
+	friend class GZBridgeTestPeer;
 
 	void Run() override;
 
@@ -161,6 +163,8 @@ private:
 	uORB::PublicationMulti<vehicle_odometry_s>    _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 	uORB::PublicationMulti<sensor_optical_flow_s> _optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
+	failure_injection::Config _failure_config;
+	failure_injection::Stuck<sensor_gps_s> _gps_stuck;
 
 	GZMixingInterfaceESC   _mixing_interface_esc{_node};
 	GZMixingInterfaceServo _mixing_interface_servo{_node};

src/modules/simulation/gz_bridge/GZBridgeTest.cpp

@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ *   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 "GZBridge.hpp"
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/failure_injection.h>
+#include <uORB/topics/sensor_gps.h>
+
+class GZBridgeTestPeer
+{
+public:
+	static void navSatCallback(GZBridge &bridge, const gz::msgs::NavSat &message)
+	{
+		bridge.navSatCallback(message);
+	}
+};
+
+namespace
+{
+
+gz::msgs::NavSat navSat(double latitude_deg, double longitude_deg)
+{
+	gz::msgs::NavSat message;
+	message.set_latitude_deg(latitude_deg);
+	message.set_longitude_deg(longitude_deg);
+	message.set_altitude(500.0);
+	message.set_velocity_north(1.0);
+	message.set_velocity_east(2.0);
+	message.set_velocity_up(0.5);
+	return message;
+}
+
+failure_injection_s gpsFailure(uint8_t failure_type)
+{
+	failure_injection_s config{};
+	config.timestamp = hrt_absolute_time();
+
+	if (failure_type != failure_injection_s::FAILURE_TYPE_OK) {
+		config.count = 1;
+		config.unit[0] = failure_injection_s::FAILURE_UNIT_SENSOR_GPS;
+		config.instance_mask[0] = 1;
+		config.failure_type[0] = failure_type;
+	}
+
+	return config;
+}
+
+} // namespace
+
+TEST(GZBridge, AppliesGpsFailureInjectionAtNavSatPublication)
+{
+	GZBridge bridge{"default", "x500"};
+	uORB::Publication<failure_injection_s> failure_pub{ORB_ID(failure_injection)};
+	uORB::Subscription gps_sub{ORB_ID(sensor_gps)};
+
+	// Initialize the map reference.
+	GZBridgeTestPeer::navSatCallback(bridge, navSat(47.397742, 8.545594));
+	GZBridgeTestPeer::navSatCallback(bridge, navSat(47.397742, 8.545594));
+
+	sensor_gps_s baseline{};
+	ASSERT_TRUE(gps_sub.update(&baseline));
+	ASSERT_EQ(baseline.fix_type, sensor_gps_s::FIX_TYPE_3D);
+
+	ASSERT_TRUE(failure_pub.publish(gpsFailure(failure_injection_s::FAILURE_TYPE_STUCK)));
+	GZBridgeTestPeer::navSatCallback(bridge, navSat(47.407742, 8.555594));
+
+	sensor_gps_s stuck{};
+	ASSERT_TRUE(gps_sub.update(&stuck));
+	EXPECT_DOUBLE_EQ(stuck.latitude_deg, baseline.latitude_deg);
+	EXPECT_DOUBLE_EQ(stuck.longitude_deg, baseline.longitude_deg);
+
+	ASSERT_TRUE(failure_pub.publish(gpsFailure(failure_injection_s::FAILURE_TYPE_WRONG)));
+	GZBridgeTestPeer::navSatCallback(bridge, navSat(47.407742, 8.555594));
+
+	sensor_gps_s wrong{};
+	ASSERT_TRUE(gps_sub.update(&wrong));
+	EXPECT_EQ(wrong.fix_type, sensor_gps_s::FIX_TYPE_2D);
+
+	ASSERT_TRUE(failure_pub.publish(gpsFailure(failure_injection_s::FAILURE_TYPE_OFF)));
+	GZBridgeTestPeer::navSatCallback(bridge, navSat(47.407742, 8.555594));
+	EXPECT_FALSE(gps_sub.updated());
+
+	ASSERT_TRUE(failure_pub.publish(gpsFailure(failure_injection_s::FAILURE_TYPE_OK)));
+	GZBridgeTestPeer::navSatCallback(bridge, navSat(47.407742, 8.555594));
+
+	sensor_gps_s recovered{};
+	ASSERT_TRUE(gps_sub.update(&recovered));
+	EXPECT_EQ(recovered.fix_type, sensor_gps_s::FIX_TYPE_3D);
+	EXPECT_NE(recovered.latitude_deg, baseline.latitude_deg);
+	EXPECT_NE(recovered.longitude_deg, baseline.longitude_deg);
+}