From 3165a77e26023d083b2b2540ce60ca6978f2154e Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Fri, 6 Dec 2024 15:04:19 +0100
Subject: [PATCH] SIH-AGP: add sensor failure simulation

---
 .../sensor_agp_sim/SensorAgpSim.cpp           | 28 +++++++++++++++----
 .../sensor_agp_sim/SensorAgpSim.hpp           | 13 ++++++++-
 .../simulation/sensor_agp_sim/parameters.c    | 14 ++++++++++
 3 files changed, 49 insertions(+), 6 deletions(-)

diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp
index 4b9327978f..4de5d46c2d 100644
--- a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp
+++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp
@@ -107,9 +107,27 @@ void SensorAgpSim::Run()
 		vehicle_global_position_s gpos{};
 		_vehicle_global_position_sub.copy(&gpos);
 
-		double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
-		double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
-		double altitude = (double)(gpos.alt + (generate_wgn() * 0.5f));
+		const uint64_t now = gpos.timestamp;
+		const float dt = (now - _time_last_update) * 1e-6f;
+		_time_last_update = now;
+
+		if (!(_param_sim_agp_fail.get() & static_cast<int32_t>(FailureMode::Stuck))) {
+			_measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid);
+		}
+
+		if (_param_sim_agp_fail.get() & static_cast<int32_t>(FailureMode::Drift)) {
+			_position_bias += Vector3f(1.5f, -5.f, 0.f) * dt;
+			_measured_lla += _position_bias;
+
+		} else {
+			_position_bias.zero();
+		}
+
+		const double latitude = _measured_lla.latitude_deg() + math::degrees((double)generate_wgn() * 2.0 /
+					CONSTANTS_RADIUS_OF_EARTH);
+		const double longitude = _measured_lla.longitude_deg() + math::degrees((double)generate_wgn() * 2.0 /
+					 CONSTANTS_RADIUS_OF_EARTH);
+		const double altitude = (double)(_measured_lla.altitude() + (generate_wgn() * 0.5f));
 
 		vehicle_global_position_s sample{};
 
@@ -120,8 +138,8 @@ void SensorAgpSim::Run()
 		sample.lat_lon_valid = true;
 		sample.alt_ellipsoid = altitude;
 		sample.alt_valid = true;
-		sample.eph = 0.9f;
-		sample.epv = 1.78f;
+		sample.eph = 20.f;
+		sample.epv = 5.f;
 
 		sample.timestamp = hrt_absolute_time();
 		_aux_global_position_pub.publish(sample);
diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp
index 6ef7d3aa9a..9700780adb 100644
--- a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp
+++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp
@@ -33,6 +33,7 @@
 
 #pragma once
 
+#include <lib/lat_lon_alt/lat_lon_alt.hpp>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
@@ -64,11 +65,21 @@ public:
 	bool init();
 
 private:
+	enum class FailureMode : int32_t {
+		Stuck = (1 << 0),
+		Drift = (1 << 1)
+	};
+
 	void Run() override;
 
 	// generate white Gaussian noise sample with std=1
 	static float generate_wgn();
 
+	LatLonAlt _measured_lla{};
+	matrix::Vector3f _position_bias{};
+
+	uint64_t _time_last_update{};
+
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
 
@@ -77,6 +88,6 @@ private:
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
 	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::SIM_GPS_USED>) _sim_gps_used
+		(ParamInt<px4::params::SIM_AGP_FAIL>) _param_sim_agp_fail
 	)
 };
diff --git a/src/modules/simulation/sensor_agp_sim/parameters.c b/src/modules/simulation/sensor_agp_sim/parameters.c
index 6ee86e740b..6690255f09 100644
--- a/src/modules/simulation/sensor_agp_sim/parameters.c
+++ b/src/modules/simulation/sensor_agp_sim/parameters.c
@@ -41,3 +41,17 @@
  * @value 1 Enabled
   */
 PARAM_DEFINE_INT32(SENS_EN_AGPSIM, 0);
+
+/**
+ * AGP failure mode
+ *
+ * Stuck: freeze the measurement to the current location
+ * Drift: add a linearly growing bias to the sensor data
+ *
+ * @group Simulator
+ * @min 0
+ * @max 3
+ * @bit 0 Stuck
+ * @bit 1 Drift
+ */
+PARAM_DEFINE_INT32(SIM_AGP_FAIL, 0);