From 8346b53921b7fd3a5abc163aab42b0f271a98f6f Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Tue, 15 Jul 2025 09:11:10 +0200
Subject: [PATCH] mavlink sim: add GNSS failure "wrong" type

---
 .../simulator_mavlink/SimulatorMavlink.cpp    | 38 +++++++++++++++----
 .../simulator_mavlink/SimulatorMavlink.hpp    |  1 +
 2 files changed, 31 insertions(+), 8 deletions(-)

diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
index a9a8269e9f..342cfcd4e7 100644
--- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
+++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
@@ -414,10 +414,18 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg)
 		sensor_gps_s gps{};
 
 		if (!_gps_stuck) {
-			gps.latitude_deg = hil_gps.lat / 1e7;
-			gps.longitude_deg = hil_gps.lon / 1e7;
-			gps.altitude_msl_m = hil_gps.alt / 1e3;
-			gps.altitude_ellipsoid_m = hil_gps.alt / 1e3;
+			if (!_gps_wrong) {
+				gps.latitude_deg = hil_gps.lat / 1e7;
+				gps.longitude_deg = hil_gps.lon / 1e7;
+				gps.altitude_msl_m = hil_gps.alt / 1e3;
+				gps.altitude_ellipsoid_m = hil_gps.alt / 1e3;
+
+			} else {
+				gps.latitude_deg = hil_gps.lat / 1e7 + 1.0;
+				gps.longitude_deg = hil_gps.lon / 1e7 + 1.0;
+				gps.altitude_msl_m = hil_gps.alt / 1e3 + 100.0;
+				gps.altitude_ellipsoid_m = hil_gps.alt / 1e3 - 100.0;
+			}
 
 			gps.s_variance_m_s = 0.25f;
 			gps.c_variance_rad = 0.5f;
@@ -435,10 +443,19 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg)
 			gps.jamming_state = 0;
 			gps.spoofing_state = 0;
 
-			gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s
-			gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s
-			gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s
-			gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s
+			if (!_gps_wrong) {
+				gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s
+				gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s
+				gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s
+				gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s
+
+			} else {
+				gps.vel_m_s = (float)(hil_gps.vel) / 100.0f - 1.f; // cm/s -> m/s
+				gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f + 5.f; // cm/s -> m/s
+				gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f - 8.f; // cm/s -> m/s
+				gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f + 2.f; // cm/s -> m/s
+			}
+
 			gps.cog_rad = ((hil_gps.cog == 65535) ? NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad
 			gps.vel_ned_valid = true;
 
@@ -1256,10 +1273,15 @@ void SimulatorMavlink::check_failure_injections()
 				supported = true;
 				_gps_blocked = false;
 				_gps_stuck = false;
+				_gps_wrong = false;
 
 			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
 				supported = true;
 				_gps_stuck = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
+				supported = true;
+				_gps_wrong = true;
 			}
 
 		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) {
diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
index 9ef81fdd55..bf8c07bbf4 100644
--- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
+++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
@@ -295,6 +295,7 @@ private:
 
 	bool _gps_blocked{false};
 	bool _gps_stuck{false};
+	bool _gps_wrong{false};
 	sensor_gps_s _gps_prev{};
 	bool _airspeed_disconnected{false};
 	hrt_abstime _airspeed_blocked_timestamp{0};