SIH-AGP: add sensor failure simulation

PX4 · Dec 6, 2024 · 1782751 · 1782751
1 parent b4ddaad
commit 1782751
Show file tree

Hide file tree

Showing 3 changed files with 46 additions and 6 deletions.
diff --git 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::Constant))) {
+			_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();
+		}
+
+		double latitude = _measured_lla.latitude_deg() + math::degrees((double)generate_wgn() * 2.0 /
+				  CONSTANTS_RADIUS_OF_EARTH);
+		double longitude = _measured_lla.longitude_deg() + math::degrees((double)generate_wgn() * 2.0 /
+				   CONSTANTS_RADIUS_OF_EARTH);
+		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
@@ -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 @@ class SensorAgpSim : public ModuleBase<SensorAgpSim>, public ModuleParams, publi
 	bool init();
 
 private:
+	enum class FailureMode : int32_t {
+		Constant = (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 @@ class SensorAgpSim : public ModuleBase<SensorAgpSim>, public ModuleParams, publi
 	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
@@ -41,3 +41,14 @@
  * @value 1 Enabled
   */
 PARAM_DEFINE_INT32(SENS_EN_AGPSIM, 0);
+
+/**
+ * AGP failure mode
+ *
+ * @group Simulator
+ * @min 0
+ * @max 3
+ * @bit 0 Constant
+ * @bit 1 Drift
+ */
+PARAM_DEFINE_INT32(SIM_AGP_FAIL, 0);