From c319f87c0ec24b3368c2a215e382188675b5909a Mon Sep 17 00:00:00 2001
From: gguidone <gennaroguido2002@gmail.com>
Date: Thu, 2 Apr 2026 12:58:29 +0200
Subject: [PATCH] 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.
---
 src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp | 10 ++++++++++
 src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp |  5 ++++-
 2 files changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp
index 5bed55df26..bf6c28a101 100644
--- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp
+++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp
@@ -198,6 +198,16 @@ 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) {
+			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..0a55f5a9ed 100644
--- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp
+++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp
@@ -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
 	)
 };