* add AGP also for SimulatorMavlink

* add AGP failure injection
* add distance sensor failure injection

ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim

@@ -193,16 +193,8 @@ else
 	exit 1
 fi
 
-# TODO:
-
-# # NOTE: Only for rover_mecanum and spacecraft_2d. All other models have
-# # the magnetometer sensor in the model.sdf.
-# if param compare -s SENS_EN_MAGSIM 1
-# then
-# 	sensor_mag_sim start
-# fi
-# # NOTE: new gz has airspeed sensor, remove once added
-# if param compare -s SENS_EN_ARSPDSIM 1
-# then
-# 	sensor_airspeed_sim start
-# fi
+# Start sensor simulation manager for additional sensors not handled by gz
+if param compare -s SENS_EN_AGPSIM 1 || param compare -s SENS_EN_MAGSIM 1 || param compare -s SENS_EN_ARSPDSIM 1
+then
+	sensor_sim_manager start
+fi

ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim

@@ -5,6 +5,7 @@
 param set-default EKF2_GPS_DELAY 10
 param set-default EKF2_MULTI_IMU 3
 param set-default SENS_IMU_MODE 0
+param set-default SIM_GZ_EN 1
 
 simulator_tcp_port=$((4560+px4_instance))
 
@@ -25,3 +26,11 @@ else
 	echo "INFO  [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOSTNAME}"
 	simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}"
 fi
+
+echo "mvlinksim"
+
+# Start sensor simulation manager for additional sensors not handled by gz
+if param compare -s SENS_EN_AGPSIM 1 || param compare -s SENS_EN_MAGSIM 1 || param compare -s SENS_EN_ARSPDSIM 1
+then
+	sensor_sim_manager start
+fi

msg/versioned/VehicleCommand.msg

@@ -141,6 +141,7 @@ uint8 FAILURE_UNIT_SENSOR_VIO = 6
 uint8 FAILURE_UNIT_SENSOR_DISTANCE_SENSOR = 7
 uint8 FAILURE_UNIT_SENSOR_AIRSPEED = 8
 uint8 FAILURE_UNIT_SENSOR_GPS_ALT = 9
+uint8 FAILURE_UNIT_SENSOR_AGP = 10
 uint8 FAILURE_UNIT_SYSTEM_BATTERY = 100
 uint8 FAILURE_UNIT_SYSTEM_MOTOR = 101
 uint8 FAILURE_UNIT_SYSTEM_SERVO = 102

src/modules/simulation/failure_injection/failureInjection.cpp

@@ -423,6 +423,63 @@ void FailureInjection::check_failure_injections()
 				supported = true;
 				_vio_blocked = false;
 			}
+
+		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AGP) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				PX4_WARN("CMD_INJECT_FAILURE, agp off");
+				supported = true;
+				_agp_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, agp ok");
+				supported = true;
+				_agp_blocked = false;
+				_agp_stuck = false;
+				_agp_drift = false;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
+				PX4_WARN("CMD_INJECT_FAILURE, agp stuck");
+				supported = true;
+				_agp_stuck = true;
+				_agp_blocked = false;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_DRIFT) {
+				PX4_WARN("CMD_INJECT_FAILURE, agp drift");
+				supported = true;
+				_agp_drift = true;
+			}
+
+		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				PX4_WARN("CMD_INJECT_FAILURE, distance sensor off");
+				supported = true;
+				_distance_sensor_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, distance sensor ok");
+				supported = true;
+				_distance_sensor_blocked = false;
+				_distance_sensor_stuck = false;
+				_distance_sensor_wrong = false;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
+				PX4_WARN("CMD_INJECT_FAILURE, distance sensor stuck");
+				supported = true;
+				_distance_sensor_stuck = true;
+				_distance_sensor_blocked = false;
+				_distance_sensor_wrong = false;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
+				PX4_WARN("CMD_INJECT_FAILURE, distance sensor wrong (blocked at 0.5m)");
+				supported = true;
+				_distance_sensor_wrong = true;
+				_distance_sensor_blocked = false;
+				_distance_sensor_stuck = false;
+			}
 		}
 
 		if (handled) {

src/modules/simulation/failure_injection/failureInjection.hpp

@@ -53,6 +53,14 @@ public:
 
 	bool is_vio_blocked() { return _vio_blocked; }
 
+	bool is_agp_blocked() { return _agp_blocked; }
+	bool is_agp_stuck() { return _agp_stuck; }
+	bool is_agp_drift() { return _agp_drift; }
+
+	bool is_distance_sensor_blocked() { return _distance_sensor_blocked; }
+	bool is_distance_sensor_stuck() { return _distance_sensor_stuck; }
+	bool is_distance_sensor_wrong() { return _distance_sensor_wrong; }
+
 private:
 
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
@@ -89,5 +97,11 @@ private:
 	bool _airspeed_disconnected{false};
 	hrt_abstime _airspeed_blocked_timestamp{0};
 	bool _vio_blocked{false};
+	bool _agp_blocked{false};
+	bool _agp_stuck{false};
+	bool _agp_drift{false};
+	bool _distance_sensor_blocked{false};
+	bool _distance_sensor_stuck{false};
+	bool _distance_sensor_wrong{false};
 
 };

src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp

@@ -97,20 +97,28 @@ SensorSimManager::SensorSimManager() :
 		.enabled = false,
 	};
 
-	// IMU: 250 Hz
-	_imu_timing = {
-		.interval_us = 4000,
-		.next_update_time = now,
-		.offset_us = static_cast<hrt_abstime>(_uniform_dist(_gen) * 4000),
-		.enabled = true,  // Always enabled
-	};
-
 	// Distance sensor: 50 Hz
 	_distance_sensor_timing = {
 		.interval_us = 20000,
 		.next_update_time = now,
 		.offset_us = static_cast<hrt_abstime>(_uniform_dist(_gen) * 20000),
-		.enabled = true,  // Always enabled
+		.enabled = false,
+	};
+
+	// IMU: 250 Hz
+	_imu_timing = {
+		.interval_us = 4000,
+		.next_update_time = now,
+		.offset_us = static_cast<hrt_abstime>(_uniform_dist(_gen) * 4000),
+		.enabled = true,
+	};
+
+	// Failure Injection Poll: 10 Hz
+	_failure_update = {
+		.interval_us = 100000,
+		.next_update_time = now,
+		.offset_us = 0,
+		.enabled = true,
 	};
 
 	_gps_timing.next_update_time += _gps_timing.offset_us;
@@ -121,6 +129,8 @@ SensorSimManager::SensorSimManager() :
 	_imu_timing.next_update_time += _imu_timing.offset_us;
 	_distance_sensor_timing.next_update_time += _distance_sensor_timing.offset_us;
 
+	_param_sim_gz_en_handle = param_find("SIM_GZ_EN");
+
 }
 
 SensorSimManager::~SensorSimManager()
@@ -137,6 +147,10 @@ SensorSimManager::~SensorSimManager()
 
 bool SensorSimManager::init()
 {
+	if (_param_sim_gz_en_handle != PARAM_INVALID) {
+		param_get(_param_sim_gz_en_handle, &_sim_gz_en_value);
+	}
+
 	ScheduleOnInterval(2_ms);
 	return true;
 }
@@ -183,23 +197,28 @@ void SensorSimManager::Run()
 
 	perf_begin(_loop_perf);
 
-	if (_parameter_update_sub.updated()) {
-		parameter_update_s parameter_update;
-		_parameter_update_sub.copy(&parameter_update);
-		updateParams();
-
-		_gps_timing.enabled = (_param_sens_en_gpssim.get() != 0);
-		_baro_timing.enabled = (_param_sens_en_barosim.get() != 0);
-		_mag_timing.enabled = (_param_sens_en_magsim.get() != 0);
-		_airspeed_timing.enabled = (_param_sens_en_arspdsim.get() != 0);
-		_agp_timing.enabled = (_param_sens_en_agpsim.get() != 0);
-	}
-
 	const hrt_abstime now = hrt_absolute_time();
 
-	if (_gps_timing.enabled && now >= _gps_timing.next_update_time) {
-		_failure_injection.check_failure_injections(); // TODO, just here for now...
+	if (now >= _failure_update.next_update_time) {
+		_failure_injection.check_failure_injections();
+		_failure_update.next_update_time = now + _failure_update.interval_us;
 
+		if (_parameter_update_sub.updated()) {
+			parameter_update_s parameter_update;
+			_parameter_update_sub.copy(&parameter_update);
+			updateParams();
+
+			_gps_timing.enabled = (_param_sens_en_gpssim.get() != 0);
+			_baro_timing.enabled = (_param_sens_en_barosim.get() != 0);
+			_mag_timing.enabled = (_param_sens_en_magsim.get() != 0);
+			_airspeed_timing.enabled = (_param_sens_en_arspdsim.get() != 0);
+			_agp_timing.enabled = (_param_sens_en_agpsim.get() != 0);
+			_distance_sensor_timing.enabled = (_param_sens_en_distsim.get() != 0);
+			_imu_timing.enabled = (_sim_gz_en_value == 0);
+		}
+	}
+
+	if (_gps_timing.enabled && now >= _gps_timing.next_update_time) {
 		updateGPS();
 		_gps_timing.next_update_time = now + _gps_timing.interval_us;
 	}
@@ -247,11 +266,10 @@ void SensorSimManager::updateGPS()
 	vehicle_global_position_s gpos{};
 	_vehicle_global_position_sub.copy(&gpos);
 
-	// Check if data is recent (within 20ms), TODO
 	const hrt_abstime now = hrt_absolute_time();
 
 	if (lpos.timestamp > 0 && gpos.timestamp > 0 &&
-	    (now - lpos.timestamp) < 20000 && (now - gpos.timestamp) < 20000) {
+	    (now - lpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) {
 
 		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);
@@ -333,10 +351,9 @@ void SensorSimManager::updateBarometer()
 	vehicle_global_position_s gpos;
 
 	if (_vehicle_global_position_sub.copy(&gpos)) {
-		// Check if data is recent (within 20ms), TODO
 		const hrt_abstime now = hrt_absolute_time();
 
-		if (gpos.timestamp > 0 && (now - gpos.timestamp) < 20000) {
+		if (gpos.timestamp > 0 && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) {
 			const float dt = math::constrain((gpos.timestamp - _last_baro_update_time) * 1e-6f, 0.001f, 0.1f);
 
 			const float alt_msl = gpos.alt;
@@ -406,10 +423,9 @@ void SensorSimManager::updateMagnetometer()
 	vehicle_global_position_s gpos;
 
 	if (_vehicle_global_position_sub.copy(&gpos)) {
-		// Check if data is recent (within 20ms), TODO
 		const hrt_abstime now = hrt_absolute_time();
 
-		if (gpos.timestamp > 0 && (now - gpos.timestamp) < 20000 && gpos.eph < 1000) {
+		if (gpos.timestamp > 0 && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && gpos.eph < 1000) {
 			// magnetic field data returned by the geo library using the current GPS position
 			const float declination_rad = math::radians(get_mag_declination_degrees(gpos.lat, gpos.lon));
 			const float inclination_rad = math::radians(get_mag_inclination_degrees(gpos.lat, gpos.lon));
@@ -466,11 +482,12 @@ void SensorSimManager::updateAirspeed()
 	vehicle_attitude_s attitude{};
 	_vehicle_attitude_sub.copy(&attitude);
 
-	// Check if data is recent (within 20ms), TODO
+	// Check if groundtruth data is recent
 	const hrt_abstime now = hrt_absolute_time();
 
 	if (lpos.timestamp > 0 && gpos.timestamp > 0 && attitude.timestamp > 0 &&
-	    (now - lpos.timestamp) < 20000 && (now - gpos.timestamp) < 20000 && (now - attitude.timestamp) < 20000) {
+	    (now - lpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US
+	    && (now - attitude.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) {
 
 		Vector3f velocity_E{lpos.vx, lpos.vy, lpos.vz}; // Velocity in NED frame (similar to Earth frame)
 
@@ -514,6 +531,7 @@ void SensorSimManager::updateAirspeed()
 		const float airspeed_blockage_rampup_time = 1e6f; // 1 second in microseconds
 
 		float airspeed_blockage_scale = 1.f;
+
 		if (blocked_timestamp > 0) {
 			airspeed_blockage_scale = math::constrain(1.f - (now - blocked_timestamp) /
 						  airspeed_blockage_rampup_time, 1.f - blockage_fraction, 1.f);
@@ -521,7 +539,8 @@ void SensorSimManager::updateAirspeed()
 
 		differential_pressure_s differential_pressure{};
 		differential_pressure.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
-		differential_pressure.differential_pressure_pa = diff_pressure * 100.f * airspeed_blockage_scale; // hPa to Pa with blockage scaling
+		differential_pressure.differential_pressure_pa = diff_pressure * 100.f *
+				airspeed_blockage_scale; // hPa to Pa with blockage scaling
 		differential_pressure.temperature = temperature_local;
 		differential_pressure.timestamp = hrt_absolute_time();
 		_differential_pressure_pub.publish(differential_pressure);
@@ -537,26 +556,24 @@ void SensorSimManager::updateAGP()
 	vehicle_global_position_s gpos{};
 	_vehicle_global_position_sub.copy(&gpos);
 
-	// Check if data is recent (within 20ms), TODO
 	const hrt_abstime now = hrt_absolute_time();
 
-	if (gpos.timestamp > 0 && (now - gpos.timestamp) < 20000) {
+	if (gpos.timestamp > 0 && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) {
+
+		if (_failure_injection.is_agp_blocked()) {
+			perf_end(_agp_perf);
+			return;
+		}
 
 		const uint64_t current_time = gpos.timestamp;
 		const float dt = (current_time - _agp_time_last_update) * 1e-6f;
 		_agp_time_last_update = current_time;
 
-		// Handle failure modes
-		enum class FailureMode : int32_t {
-			Stuck = (1 << 0),
-			Drift = (1 << 1)
-		};
-
-		if (!(_sim_agp_fail.get() & static_cast<int32_t>(FailureMode::Stuck))) {
+		if (!_failure_injection.is_agp_stuck()) {
 			_agp_measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid);
 		}
 
-		if (_sim_agp_fail.get() & static_cast<int32_t>(FailureMode::Drift)) {
+		if (_failure_injection.is_agp_drift()) {
 			_agp_position_bias += matrix::Vector3f(1.5f, -5.f, 0.f) * dt;
 			_agp_measured_lla += _agp_position_bias;
 
@@ -605,12 +622,12 @@ void SensorSimManager::updateIMU()
 	vehicle_angular_velocity_s angular_velocity{};
 	_vehicle_angular_velocity_sub.copy(&angular_velocity);
 
-	// Check if data is recent (within 20ms)
 	const hrt_abstime now = hrt_absolute_time();
 
 	if (gpos.timestamp > 0 && lpos.timestamp > 0 && attitude.timestamp > 0 && angular_velocity.timestamp > 0 &&
-	    (now - gpos.timestamp) < 20000 && (now - lpos.timestamp) < 20000 &&
-	    (now - attitude.timestamp) < 20000 && (now - angular_velocity.timestamp) < 20000) {
+	    (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && (now - lpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US &&
+	    (now - attitude.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US
+	    && (now - angular_velocity.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) {
 
 		// The sensor signals reconstruction and noise levels are from [1]
 		// [1] Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight."
@@ -679,12 +696,15 @@ void SensorSimManager::updateDistanceSensor()
 	vehicle_attitude_s attitude{};
 	_vehicle_attitude_sub.copy(&attitude);
 
-	// Check if data is recent (within 20ms), TODO
 	const hrt_abstime now = hrt_absolute_time();
 
 	if (lpos.timestamp > 0 && attitude.timestamp > 0 &&
-	    (now - lpos.timestamp) < 20000 && (now - attitude.timestamp) < 20000 &&
-	    fabsf(_distance_snsr_override) < 10000) {
+	    (now - lpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && (now - attitude.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) {
+
+		if (_failure_injection.is_distance_sensor_blocked()) {
+			perf_end(_distance_sensor_perf);
+			return;
+		}
 
 		device::Device::DeviceId device_id;
 		device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
@@ -696,22 +716,30 @@ void SensorSimManager::updateDistanceSensor()
 		distance_sensor.device_id = device_id.devid;
 		distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
 		distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
-		distance_sensor.min_distance = _distance_snsr_min;
-		distance_sensor.max_distance = _distance_snsr_max;
+		distance_sensor.min_distance = _distance_snsr_min.get();
+		distance_sensor.max_distance = _distance_snsr_max.get();
 		distance_sensor.signal_quality = -1;
 
-		if (_distance_snsr_override >= 0.f) {
-			distance_sensor.current_distance = _distance_snsr_override;
+		float current_distance;
+
+		if (_failure_injection.is_distance_sensor_stuck()) {
+			current_distance = _last_distance_sensor_value;
+
+		} else if (_failure_injection.is_distance_sensor_wrong()) {
+			current_distance = 0.5f;
 
 		} else {
 			Quatf q{attitude.q};
-			distance_sensor.current_distance = -lpos.z / q.dcm_z()(2);
+			current_distance = -lpos.z / q.dcm_z()(2);
 
-			if (distance_sensor.current_distance > _distance_snsr_max) {
-				distance_sensor.current_distance = UINT16_MAX / 100.f;
+			if (current_distance > _distance_snsr_max.get()) {
+				current_distance = UINT16_MAX / 100.f;
 			}
+
+			_last_distance_sensor_value = current_distance;
 		}
 
+		distance_sensor.current_distance = current_distance;
 		distance_sensor.timestamp = hrt_absolute_time();
 		_distance_sensor_pub.publish(distance_sensor);
 	}

src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp

@@ -63,6 +63,7 @@
 #include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/lat_lon_alt/lat_lon_alt.hpp>
+#include <parameters/param.h>
 #include <random>
 
 using namespace time_literals;
@@ -116,6 +117,7 @@ private:
 	SensorTiming _agp_timing;
 	SensorTiming _imu_timing;
 	SensorTiming _distance_sensor_timing;
+	SensorTiming _failure_update;
 
 	// Subscriptions
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
@@ -164,13 +166,15 @@ private:
 		(ParamInt<px4::params::SENS_EN_MAGSIM>) _param_sens_en_magsim,
 		(ParamInt<px4::params::SENS_EN_ARSPDSIM>) _param_sens_en_arspdsim,
 		(ParamInt<px4::params::SENS_EN_AGPSIM>) _param_sens_en_agpsim,
+		(ParamInt<px4::params::SENS_EN_DISTSIM>) _param_sens_en_distsim,
 		(ParamInt<px4::params::SIM_GPS_USED>) _sim_gps_used,
 		(ParamFloat<px4::params::SIM_BARO_OFF_P>) _sim_baro_off_p,
 		(ParamFloat<px4::params::SIM_BARO_OFF_T>) _sim_baro_off_t,
 		(ParamFloat<px4::params::SIM_MAG_OFFSET_X>) _sim_mag_offset_x,
 		(ParamFloat<px4::params::SIM_MAG_OFFSET_Y>) _sim_mag_offset_y,
 		(ParamFloat<px4::params::SIM_MAG_OFFSET_Z>) _sim_mag_offset_z,
-		(ParamInt<px4::params::SIM_AGP_FAIL>) _sim_agp_fail
+		(ParamFloat<px4::params::SIH_DISTSNSR_MIN>) _distance_snsr_min,
+		(ParamFloat<px4::params::SIH_DISTSNSR_MAX>) _distance_snsr_max
 	)
 
 	hrt_abstime _last_baro_update_time{0};
@@ -190,9 +194,7 @@ private:
 	matrix::Vector3f _last_gyro{};
 	matrix::Vector3f _specific_force_E{};
 
-	float _distance_snsr_min{0.3f};
-	float _distance_snsr_max{50.0f};
-	float _distance_snsr_override{-1.0f};
+	float _last_distance_sensor_value{0.0f};
 
 	// Air constants
 	static constexpr float TEMPERATURE_MSL = 288.0f; // [K]
@@ -204,4 +206,10 @@ private:
 	// IMU constants
 	static constexpr float T1_C = 15.0f; // Temperature constant
 
+	static constexpr hrt_abstime GROUNDTRUTH_DATA_MAX_AGE_US = 12000;
+
+	// Parameter handles for cross-module access
+	param_t _param_sim_gz_en_handle{PARAM_INVALID};
+	int32_t _sim_gz_en_value{0};
+
 };

src/modules/simulation/sensor_sim_manager/parameters.c

@@ -75,6 +75,15 @@ PARAM_DEFINE_INT32(SENS_EN_ARSPDSIM, 0);
  */
 PARAM_DEFINE_INT32(SENS_EN_AGPSIM, 0);
 
+/**
+ * Enable simulated distance sensor
+ *
+ * @group Simulation
+ * @value 0 Disabled
+ * @value 1 Enabled
+ */
+PARAM_DEFINE_INT32(SENS_EN_DISTSIM, 0);
+
 /**
  * Number of GPS satellites used in simulation
  *
@@ -136,3 +145,27 @@ PARAM_DEFINE_FLOAT(SIM_MAG_OFFSET_Z, 0.0f);
  * @bit 1 Drift
  */
 PARAM_DEFINE_INT32(SIM_AGP_FAIL, 0);
+
+/**
+ * distance sensor minimum range
+ *
+ * @unit m
+ * @min 0.0
+ * @max 10.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MIN, 0.0f);
+
+/**
+ * distance sensor maximum range
+ *
+ * @unit m
+ * @min 0.0
+ * @max 1000.0
+ * @decimal 4
+ * @increment 0.01
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MAX, 100.0f);

src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

@@ -325,7 +325,8 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
 	}
 
 	// differential pressure
-	if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_failure_injection.is_airspeed_disconnected()) {
+	if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS
+	    && !_failure_injection.is_airspeed_disconnected()) {
 
 		const float blockage_fraction = 0.7; // defines max blockage (fully ramped)
 		const float airspeed_blockage_rampup_time = 1_s; // time it takes to go max blockage, linear ramp
@@ -333,6 +334,7 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
 		float airspeed_blockage_scale = 1.f;
 
 		hrt_abstime blocked_timestamp = _failure_injection.get_airspeed_blocked_timestamp();
+
 		if (blocked_timestamp > 0) {
 			airspeed_blockage_scale = math::constrain(1.f - (hrt_absolute_time() - blocked_timestamp) /
 						  airspeed_blockage_rampup_time, 1.f - blockage_fraction, 1.f);
@@ -1225,11 +1227,28 @@ void SimulatorMavlink::run()
 
 int SimulatorMavlink::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavlink)
 {
+	if (_failure_injection.is_distance_sensor_blocked()) {
+		return PX4_OK;
+	}
+
 	distance_sensor_s dist{};
 	dist.timestamp = hrt_absolute_time();
 	dist.min_distance = dist_mavlink->min_distance / 100.0f;
 	dist.max_distance = dist_mavlink->max_distance / 100.0f;
-	dist.current_distance = dist_mavlink->current_distance / 100.0f;
+
+	float current_distance = dist_mavlink->current_distance / 100.0f;
+
+	if (_failure_injection.is_distance_sensor_stuck()) {
+		current_distance = _last_distance_sensor_value;
+
+	} else if (_failure_injection.is_distance_sensor_wrong()) {
+		current_distance = 0.5f;
+
+	} else {
+		_last_distance_sensor_value = current_distance;
+	}
+
+	dist.current_distance = current_distance;
 	dist.type = dist_mavlink->type;
 	dist.variance = dist_mavlink->covariance * 1e-4f; // cm^2 to m^2
 

src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp

@@ -203,6 +203,7 @@ private:
 
 	uORB::PublicationMulti<distance_sensor_s>	*_dist_pubs[ORB_MULTI_MAX_INSTANCES] {};
 	uint32_t _dist_sensor_ids[ORB_MULTI_MAX_INSTANCES] {};
+	float _last_distance_sensor_value{0.0f};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 

src/modules/simulation/simulator_sih/sih_params.c

@@ -282,30 +282,6 @@ PARAM_DEFINE_FLOAT(SIH_LOC_LON0, 8.545594f);
  */
 PARAM_DEFINE_FLOAT(SIH_LOC_H0, 489.4f);
 
-/**
- * distance sensor minimum range
- *
- * @unit m
- * @min 0.0
- * @max 10.0
- * @decimal 4
- * @increment 0.01
- * @group Simulation In Hardware
- */
-PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MIN, 0.0f);
-
-/**
- * distance sensor maximum range
- *
- * @unit m
- * @min 0.0
- * @max 1000.0
- * @decimal 4
- * @increment 0.01
- * @group Simulation In Hardware
- */
-PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MAX, 100.0f);
-
 /**
  * if >= 0 the distance sensor measures will be overridden by this value
  *

src/systemcmds/failure/failure.cpp

@@ -58,6 +58,7 @@ static constexpr FailureUnit failure_units[] = {
 	{ "baro", vehicle_command_s::FAILURE_UNIT_SENSOR_BARO},
 	{ "gps", vehicle_command_s::FAILURE_UNIT_SENSOR_GPS},
 	{ "gps_alt", vehicle_command_s::FAILURE_UNIT_SENSOR_GPS_ALT},
+	{ "agp", vehicle_command_s::FAILURE_UNIT_SENSOR_AGP},
 	{ "optical_flow", vehicle_command_s::FAILURE_UNIT_SENSOR_OPTICAL_FLOW},
 	{ "vio", vehicle_command_s::FAILURE_UNIT_SENSOR_VIO},
 	{ "distance_sensor", vehicle_command_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR},
@@ -135,6 +136,7 @@ int inject_failure(const FailureUnit& unit, const FailureType& type, uint8_t ins
 	command.param3 = static_cast<float>(instance);
 	command.timestamp = hrt_absolute_time();
 	command_pub.publish(command);
+	printf("pub\n");
 
 	vehicle_command_ack_s ack;