remove airspeed publisher from airspeed-sim. only publishes differential pressure

* fix gps-stuck-failure to actually be stuck

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

@@ -193,14 +193,8 @@ else
 	exit 1
 fi
 
-# 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
+# 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_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
+	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,9 @@ else
 	echo "INFO  [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOSTNAME}"
 	simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}"
 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.sihsim

@@ -15,22 +15,7 @@ fi
 
 if simulator_sih start; then
 
-	if param compare -s SENS_EN_BAROSIM 1
-	then
-		sensor_baro_sim start
-	fi
-	if param compare -s SENS_EN_GPSSIM 1
-	then
-		sensor_gps_sim start
-	fi
-	if param compare -s SENS_EN_MAGSIM 1
-	then
-		sensor_mag_sim start
-	fi
-	if param compare -s SENS_EN_AGPSIM 1
-	then
-		sensor_agp_sim start
-	fi
+	sensor_sim_manager start
 
 else
 	echo "ERROR [init] simulator_sih failed to start"

ROMFS/px4fmu_common/init.d/rcS

@@ -336,10 +336,7 @@ else
 		if param compare SYS_HITL 2
 		then
 			simulator_sih start
-			sensor_baro_sim start
-			sensor_mag_sim start
-			sensor_gps_sim start
-			sensor_agp_sim start
+			sensor_sim_manager start
 		fi
 
 	else

boards/px4/sitl/default.px4board

@@ -55,7 +55,7 @@ CONFIG_COMMON_SIMULATION=y
 CONFIG_MODULES_SIMULATION_GZ_MSGS=y
 CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y
 CONFIG_MODULES_SIMULATION_GZ_PLUGINS=y
-CONFIG_MODULES_SIMULATION_SENSOR_AGP_SIM=y
+CONFIG_MODULES_SIMULATION_SENSOR_SIM_MANAGER=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y

msg/versioned/VehicleCommand.msg

@@ -140,6 +140,8 @@ uint8 FAILURE_UNIT_SENSOR_OPTICAL_FLOW = 5
 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
@@ -155,6 +157,7 @@ uint8 FAILURE_TYPE_WRONG = 4
 uint8 FAILURE_TYPE_SLOW = 5
 uint8 FAILURE_TYPE_DELAYED = 6
 uint8 FAILURE_TYPE_INTERMITTENT = 7
+uint8 FAILURE_TYPE_DRIFT = 8
 
 # Used as param1 in DO_CHANGE_SPEED command.
 uint8 SPEED_TYPE_AIRSPEED = 0

src/modules/simulation/Kconfig

@@ -5,10 +5,7 @@ menu "Simulation"
         depends on PLATFORM_POSIX
         select MODULES_SIMULATION_BATTERY_SIMULATOR
         select MODULES_SIMULATION_PWM_OUT_SIM
-        select MODULES_SIMULATION_SENSOR_AIRSPEED_SIM
-        select MODULES_SIMULATION_SENSOR_BARO_SIM
-        select MODULES_SIMULATION_SENSOR_GPS_SIM
-        select MODULES_SIMULATION_SENSOR_MAG_SIM
+        select MODULES_SIMULATION_SENSOR_SIM_MANAGER
         select MODULES_SIMULATION_SYSTEM_POWER_SIMULATOR
         select MODULES_SIMULATION_SIMULATOR_MAVLINK
         select MODULES_SIMULATION_SIMULATOR_SIH

src/modules/simulation/failure_injection/failureInjection.cpp

@@ -0,0 +1,517 @@
+
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <simulation/failure_injection/failureInjection.hpp>
+
+bool FailureInjection::handle_gps_failure(sensor_gps_s &gps)
+{
+	if (_gps_blocked) {
+		return false;
+	}
+
+	if (!_gps_stuck) {
+
+		if (_gps_wrong) {
+			gps.latitude_deg += 1.0;
+			gps.longitude_deg += 1.0;
+			gps.altitude_msl_m += 100.0;
+			gps.altitude_ellipsoid_m += 100.0;
+
+			gps.vel_m_s -= 1.f;
+			gps.vel_n_m_s += 5.f;
+			gps.vel_e_m_s -= 8.f;
+			gps.vel_d_m_s += 2.f;
+		}
+
+		if (_gps_drift) {
+			if (!_has_drift_ref) {
+
+				_mp.initReference(gps.latitude_deg, gps.longitude_deg);
+				_mp.project(gps.latitude_deg, gps.longitude_deg, _gps_drift_pos(0), _gps_drift_pos(1));
+				_has_drift_ref = true;
+			}
+
+			matrix::Vector2f vel_gps(gps.vel_n_m_s, gps.vel_e_m_s);
+			const float vel_norm = vel_gps.norm();
+
+			// directional drift by kRelativeGpsDrift
+			if (vel_norm > 1.f) {
+				vel_gps *= 1.f + kRelativeGpsDrift;
+
+			} else if (vel_norm > 0.01f) {
+				vel_gps *= 0.5f / vel_norm;
+
+			} else {
+				vel_gps(0) = 0.5f;
+			}
+
+			// perpendicular drift
+			matrix::Vector2f vel_normal = vel_gps.normalized();
+			vel_gps(0) += vel_norm * kRelativeGpsDrift * vel_normal(1);
+			vel_gps(1) -= vel_norm * kRelativeGpsDrift * vel_normal(0);
+
+			gps.vel_n_m_s = vel_gps(0);
+			gps.vel_e_m_s = vel_gps(1);
+
+			if (_last_gps_timestamp > 0) {
+				float dt = 1e-6f * (gps.timestamp - _last_gps_timestamp);
+				_gps_drift_pos += vel_gps * dt;
+				_mp.reproject(_gps_drift_pos(0), _gps_drift_pos(1), gps.latitude_deg, gps.longitude_deg);
+			}
+
+		}
+
+		_gps_prev = gps;
+
+	} else {
+		gps = _gps_prev;
+	}
+
+	_last_gps_timestamp = gps.timestamp;
+
+	return true;
+}
+
+bool FailureInjection::handle_gps_alt_failure(sensor_gps_s &gps)
+{
+	if (_gps_alt_blocked) {
+		return false;
+	}
+
+	if (!_gps_alt_stuck) {
+
+		if (_gps_alt_wrong) {
+			gps.altitude_msl_m += 100.0;
+			gps.altitude_ellipsoid_m += 100.0;
+		}
+
+		if (_gps_alt_drift) {
+			if (_alt_drift_t0 == 0) {
+				_alt_drift_t0 = gps.timestamp;
+			}
+
+			if (_gps_alt_offset < DBL_EPSILON) { _gps_alt_offset = gps.altitude_msl_m; }
+
+			static constexpr double kDriftPeriodT = 20.;
+			static constexpr double kDriftMagnitude = 5.;
+			gps.vel_d_m_s = kDriftMagnitude * sin(2 * M_PI / kDriftPeriodT * 1e-6 * (gps.timestamp - _alt_drift_t0));
+
+			if (_last_gps_timestamp > 0) {
+				_gps_alt_offset += -(double)(gps.vel_d_m_s * (gps.timestamp - _last_gps_timestamp) / 1.e6f);
+			}
+
+			gps.altitude_msl_m = _gps_alt_offset;
+		}
+
+		_gps_prev = gps;
+
+	} else {
+		gps = _gps_prev;
+	}
+
+	_last_gps_timestamp = gps.timestamp;
+
+	return true;
+
+}
+
+
+void FailureInjection::check_failure_injections()
+{
+	vehicle_command_s vehicle_command;
+
+	while (_vehicle_command_sub.update(&vehicle_command)) {
+		if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) {
+			continue;
+		}
+
+		bool handled = false;
+		bool supported = false;
+
+		const int failure_unit = static_cast<int>(vehicle_command.param1 + 0.5f);
+		const int failure_type = static_cast<int>(vehicle_command.param2 + 0.5f);
+		const int instance = static_cast<int>(vehicle_command.param3 + 0.5f);
+
+		if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				PX4_WARN("CMD_INJECT_FAILURE, GPS off");
+				supported = true;
+				_gps_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, GPS ok");
+				supported = true;
+				_gps_blocked = false;
+				_gps_stuck = false;
+				_gps_wrong = false;
+				_gps_drift = 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_type == vehicle_command_s::FAILURE_TYPE_DRIFT) {
+				PX4_INFO("CMD_INJECT_FAILURE, GPS drift");
+				supported = true;
+				_gps_drift = true;
+
+			}
+
+		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS_ALT) {
+
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				PX4_WARN("CMD_INJECT_FAILURE, GPS altitude off");
+				supported = true;
+				_gps_alt_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, GPS altitude ok");
+				supported = true;
+				_gps_alt_blocked = false;
+				_gps_alt_stuck = false;
+				_gps_alt_wrong = false;
+				_gps_alt_drift = false;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
+				supported = true;
+				_gps_alt_stuck = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
+				supported = true;
+				_gps_alt_wrong = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_DRIFT) {
+				PX4_INFO("CMD_INJECT_FAILURE, GPS altitude drift");
+				supported = true;
+				_gps_alt_drift = true;
+			}
+
+		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				supported = true;
+
+				// 0 to signal all
+				if (instance == 0) {
+					for (int i = 0; i < kAccelCountMax; i++) {
+						PX4_WARN("CMD_INJECT_FAILURE, accel %d off", i);
+						_accel_blocked[i] = true;
+						_accel_stuck[i] = false;
+					}
+
+				} else if (instance >= 1 && instance <= kAccelCountMax) {
+					PX4_WARN("CMD_INJECT_FAILURE, accel %d off", instance - 1);
+					_accel_blocked[instance - 1] = true;
+					_accel_stuck[instance - 1] = false;
+				}
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
+				supported = true;
+
+				// 0 to signal all
+				if (instance == 0) {
+					for (int i = 0; i < kAccelCountMax; i++) {
+						PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", i);
+						_accel_blocked[i] = false;
+						_accel_stuck[i] = true;
+					}
+
+				} else if (instance >= 1 && instance <= kAccelCountMax) {
+					PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", instance - 1);
+					_accel_blocked[instance - 1] = false;
+					_accel_stuck[instance - 1] = true;
+				}
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				supported = true;
+
+				// 0 to signal all
+				if (instance == 0) {
+					for (int i = 0; i < kAccelCountMax; i++) {
+						PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", i);
+						_accel_blocked[i] = false;
+						_accel_stuck[i] = false;
+					}
+
+				} else if (instance >= 1 && instance <= kAccelCountMax) {
+					PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", instance - 1);
+					_accel_blocked[instance - 1] = false;
+					_accel_stuck[instance - 1] = false;
+				}
+			}
+
+		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GYRO) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				supported = true;
+
+				// 0 to signal all
+				if (instance == 0) {
+					for (int i = 0; i < kGyroCountMax; i++) {
+						PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", i);
+						_gyro_blocked[i] = true;
+						_gyro_stuck[i] = false;
+					}
+
+				} else if (instance >= 1 && instance <= kGyroCountMax) {
+					PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", instance - 1);
+					_gyro_blocked[instance - 1] = true;
+					_gyro_stuck[instance - 1] = false;
+				}
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
+				supported = true;
+
+				// 0 to signal all
+				if (instance == 0) {
+					for (int i = 0; i < kGyroCountMax; i++) {
+						PX4_WARN("CMD_INJECT_FAILURE, gyro %d stuck", i);
+						_gyro_blocked[i] = false;
+						_gyro_stuck[i] = true;
+					}
+
+				} else if (instance >= 1 && instance <= kGyroCountMax) {
+					PX4_INFO("CMD_INJECT_FAILURE, gyro %d stuck", instance - 1);
+					_gyro_blocked[instance - 1] = false;
+					_gyro_stuck[instance - 1] = true;
+				}
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				supported = true;
+
+				// 0 to signal all
+				if (instance == 0) {
+					for (int i = 0; i < kGyroCountMax; i++) {
+						PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", i);
+						_gyro_blocked[i] = false;
+						_gyro_stuck[i] = false;
+					}
+
+				} else if (instance >= 1 && instance <= kGyroCountMax) {
+					PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", instance - 1);
+					_gyro_blocked[instance - 1] = false;
+					_gyro_stuck[instance - 1] = false;
+				}
+			}
+
+		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_MAG) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				supported = true;
+
+				// 0 to signal all
+				if (instance == 0) {
+					for (int i = 0; i < kMagCountMax; i++) {
+						PX4_WARN("CMD_INJECT_FAILURE, mag %d off", i);
+						_mag_blocked[i] = true;
+						_mag_stuck[i] = false;
+					}
+
+				} else if (instance >= 1 && instance <= kMagCountMax) {
+					PX4_WARN("CMD_INJECT_FAILURE, mag %d off", instance - 1);
+					_mag_blocked[instance - 1] = true;
+					_mag_stuck[instance - 1] = false;
+				}
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
+				supported = true;
+
+				// 0 to signal all
+				if (instance == 0) {
+					for (int i = 0; i < kMagCountMax; i++) {
+						PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", i);
+						_mag_blocked[i] = false;
+						_mag_stuck[i] = true;
+					}
+
+				} else if (instance >= 1 && instance <= kMagCountMax) {
+					PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", instance - 1);
+					_mag_blocked[instance - 1] = false;
+					_mag_stuck[instance - 1] = true;
+				}
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				supported = true;
+
+				// 0 to signal all
+				if (instance == 0) {
+					for (int i = 0; i < kMagCountMax; i++) {
+						PX4_INFO("CMD_INJECT_FAILURE, mag %d ok", i);
+						_mag_blocked[i] = false;
+						_mag_stuck[i] = false;
+					}
+
+				} else if (instance >= 1 && instance <= kMagCountMax) {
+					PX4_INFO("CMD_INJECT_FAILURE, mag %d ok", instance - 1);
+					_mag_blocked[instance - 1] = false;
+					_mag_stuck[instance - 1] = false;
+				}
+			}
+
+		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_BARO) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				PX4_WARN("CMD_INJECT_FAILURE, baro off");
+				supported = true;
+				_baro_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
+				PX4_WARN("CMD_INJECT_FAILURE, baro stuck");
+				supported = true;
+				_baro_stuck = true;
+				_baro_blocked = false;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, baro ok");
+				supported = true;
+				_baro_blocked = false;
+				_baro_stuck = false;
+			}
+
+		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				PX4_WARN("CMD_INJECT_FAILURE, airspeed off");
+				supported = true;
+				_airspeed_disconnected = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
+				PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate pitot blockage)");
+				supported = true;
+				_airspeed_blocked_timestamp = hrt_absolute_time();
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, airspeed ok");
+				supported = true;
+				_airspeed_disconnected = false;
+				_airspeed_blocked_timestamp = 0;
+			}
+
+		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_VIO) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				PX4_WARN("CMD_INJECT_FAILURE, vio off");
+				supported = true;
+				_vio_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, vio ok");
+				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) {
+			vehicle_command_ack_s ack{};
+			ack.command = vehicle_command.command;
+			ack.from_external = false;
+			ack.result = supported ?
+				     vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED :
+				     vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+			ack.timestamp = hrt_absolute_time();
+			_command_ack_pub.publish(ack);
+		}
+	}
+}

src/modules/simulation/failure_injection/failureInjection.hpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+*
+*   Copyright (c) 2019-2025 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+*    notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+*    notice, this list of conditions and the following disclaimer in
+*    the documentation and/or other materials provided with the
+*    distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+*    used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/bitmask.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/sensor_gps.h>
+#include <lib/geo/geo.h>
+#include <matrix/math.hpp>
+
+static constexpr uint8_t kAccelCountMax = 4;
+static constexpr uint8_t kGyroCountMax  = 4;
+static constexpr uint8_t kMagCountMax   = 4;
+
+static constexpr float kRelativeGpsDrift = 0.1f;
+
+class FailureInjection
+{
+
+public:
+	FailureInjection() = default;
+
+	void check_failure_injections();
+	bool handle_gps_failure(sensor_gps_s &gps);
+	bool handle_gps_alt_failure(sensor_gps_s &gps);
+
+	bool is_accel_blocked(const int instance = 0) { return _accel_blocked[instance]; }
+	bool is_accel_stuck(const int instance = 0) { return _accel_stuck[instance]; }
+
+	bool is_gyro_blocked(const int instance = 0) { return _gyro_blocked[instance]; }
+	bool is_gyro_stuck(const int instance = 0) { return _gyro_stuck[instance]; }
+
+	bool is_mag_blocked(const int instance = 0) { return _mag_blocked[instance]; }
+	bool is_mag_stuck(const int instance = 0) { return _mag_stuck[instance]; }
+
+	bool is_baro_blocked() { return _baro_blocked; }
+	bool is_baro_stuck() { return _baro_stuck; }
+
+	bool is_airspeed_disconnected() { return _airspeed_disconnected; }
+	hrt_abstime get_airspeed_blocked_timestamp() { return _airspeed_blocked_timestamp; }
+
+	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)};
+	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+	bool _gps_drift{false};
+	bool _gps_alt_drift{false};
+	MapProjection _mp;
+	bool _has_drift_ref{false};
+	matrix::Vector2f _gps_drift_pos;
+	double _gps_alt_offset{0};
+	hrt_abstime _last_gps_timestamp{0};
+	hrt_abstime _alt_drift_t0{0};
+
+	bool _gps_blocked{false};
+	bool _gps_alt_blocked{false};
+	bool _gps_stuck{false};
+	bool _gps_alt_stuck{false};
+	bool _gps_wrong{false};
+	bool _gps_alt_wrong{false};
+	sensor_gps_s _gps_prev{};
+
+	bool _accel_blocked[kAccelCountMax] {};
+	bool _accel_stuck[kAccelCountMax] {};
+	bool _gyro_blocked[kGyroCountMax] {};
+	bool _gyro_stuck[kGyroCountMax] {};
+
+	bool _mag_blocked[kMagCountMax] {};
+	bool _mag_stuck[kMagCountMax] {};
+	bool _baro_blocked{false};
+	bool _baro_stuck{false};
+	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_agp_sim/CMakeLists.txt

@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_module(
-	MODULE modules__simulation__sensor_agp_sim
-	MAIN sensor_agp_sim
-	COMPILE_FLAGS
-	SRCS
-		SensorAgpSim.cpp
-		SensorAgpSim.hpp
-	DEPENDS
-		px4_work_queue
-	)

src/modules/simulation/sensor_agp_sim/Kconfig

@@ -1,5 +0,0 @@
-menuconfig MODULES_SIMULATION_SENSOR_AGP_SIM
-	bool "sensor_agp_sim"
-	default n
-	---help---
-		Enable support for sensor_agp_sim

src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp

@@ -1,202 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SensorAgpSim.hpp"
-
-#include <drivers/drv_sensor.h>
-#include <lib/drivers/device/Device.hpp>
-#include <lib/geo/geo.h>
-
-using namespace matrix;
-
-SensorAgpSim::SensorAgpSim() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
-{
-}
-
-SensorAgpSim::~SensorAgpSim()
-{
-	perf_free(_loop_perf);
-}
-
-bool SensorAgpSim::init()
-{
-	ScheduleOnInterval(500_ms); // 2 Hz
-	return true;
-}
-
-float SensorAgpSim::generate_wgn()
-{
-	// generate white Gaussian noise sample with std=1
-	// from BlockRandGauss.hpp
-	static float V1, V2, S;
-	static bool phase = true;
-	float X;
-
-	if (phase) {
-		do {
-			float U1 = (float)rand() / (float)RAND_MAX;
-			float U2 = (float)rand() / (float)RAND_MAX;
-			V1 = 2.0f * U1 - 1.0f;
-			V2 = 2.0f * U2 - 1.0f;
-			S = V1 * V1 + V2 * V2;
-		} while (S >= 1.0f || fabsf(S) < 1e-8f);
-
-		X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
-
-	} else {
-		X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
-	}
-
-	phase = !phase;
-	return X;
-}
-
-void SensorAgpSim::Run()
-{
-	if (should_exit()) {
-		ScheduleClear();
-		exit_and_cleanup();
-		return;
-	}
-
-	perf_begin(_loop_perf);
-
-	// Check if parameters have changed
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		updateParams();
-	}
-
-	if (_vehicle_global_position_sub.updated()) {
-
-		vehicle_global_position_s gpos{};
-		_vehicle_global_position_sub.copy(&gpos);
-
-		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{};
-
-		sample.timestamp_sample = gpos.timestamp_sample;
-		sample.lat = latitude;
-		sample.lon = longitude;
-		sample.alt = altitude;
-		sample.lat_lon_valid = true;
-		sample.alt_ellipsoid = altitude;
-		sample.alt_valid = true;
-		sample.eph = 20.f;
-		sample.epv = 5.f;
-
-		sample.timestamp = hrt_absolute_time();
-		_aux_global_position_pub.publish(sample);
-	}
-
-	perf_end(_loop_perf);
-}
-
-int SensorAgpSim::task_spawn(int argc, char *argv[])
-{
-	SensorAgpSim *instance = new SensorAgpSim();
-
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
-
-		if (instance->init()) {
-			return PX4_OK;
-		}
-
-	} else {
-		PX4_ERR("alloc failed");
-	}
-
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
-
-	return PX4_ERROR;
-}
-
-int SensorAgpSim::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
-}
-
-int SensorAgpSim::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-Module to simulate auxiliary global position measurements with optional failure modes for SIH simulation.
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("sensor_agp_sim", "system");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
-
-extern "C" __EXPORT int sensor_agp_sim_main(int argc, char *argv[])
-{
-	return SensorAgpSim::main(argc, argv);
-}

src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp

@@ -1,93 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#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>
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_global_position.h>
-
-using namespace time_literals;
-
-class SensorAgpSim : public ModuleBase<SensorAgpSim>, public ModuleParams, public px4::ScheduledWorkItem
-{
-public:
-	SensorAgpSim();
-	~SensorAgpSim() override;
-
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
-
-	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)};
-
-	uORB::PublicationMulti<vehicle_global_position_s> _aux_global_position_pub{ORB_ID(aux_global_position)};
-
-	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::SIM_AGP_FAIL>) _param_sim_agp_fail
-	)
-};

src/modules/simulation/sensor_agp_sim/parameters.c

@@ -1,57 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-/**
- * Simulate Aux Global Position (AGP)
- *
- * @reboot_required true
- * @min 0
- * @max 1
- * @group Sensors
- * @value 0 Disabled
- * @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);

src/modules/simulation/sensor_airspeed_sim/CMakeLists.txt

@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_module(
-	MODULE modules__simulation__sensor_airspeed_sim
-	MAIN sensor_airspeed_sim
-	COMPILE_FLAGS
-	SRCS
-		SensorAirspeedSim.cpp
-		SensorAirspeedSim.hpp
-	DEPENDS
-		px4_work_queue
-	)

src/modules/simulation/sensor_airspeed_sim/Kconfig

@@ -1,5 +0,0 @@
-menuconfig MODULES_SIMULATION_SENSOR_AIRSPEED_SIM
-	bool "sensor_airspeed_sim"
-	default n
-	---help---
-		Enable support for sensor_airspeed_sim

src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp

@@ -1,208 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SensorAirspeedSim.hpp"
-
-#include <drivers/drv_sensor.h>
-#include <lib/drivers/device/Device.hpp>
-#include <lib/geo/geo.h>
-
-using namespace matrix;
-
-SensorAirspeedSim::SensorAirspeedSim() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
-{
-}
-
-SensorAirspeedSim::~SensorAirspeedSim()
-{
-	perf_free(_loop_perf);
-}
-
-bool SensorAirspeedSim::init()
-{
-	ScheduleOnInterval(125_ms); // 8 Hz
-	return true;
-}
-
-float SensorAirspeedSim::generate_wgn()
-{
-	// generate white Gaussian noise sample with std=1
-
-	// algorithm 1:
-	// float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
-	// return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
-	// algorithm 2: from BlockRandGauss.hpp
-	static float V1, V2, S;
-	static bool phase = true;
-	float X;
-
-	if (phase) {
-		do {
-			float U1 = (float)rand() / (float)RAND_MAX;
-			float U2 = (float)rand() / (float)RAND_MAX;
-			V1 = 2.0f * U1 - 1.0f;
-			V2 = 2.0f * U2 - 1.0f;
-			S = V1 * V1 + V2 * V2;
-		} while (S >= 1.0f || fabsf(S) < 1e-8f);
-
-		X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
-
-	} else {
-		X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
-	}
-
-	phase = !phase;
-	return X;
-}
-
-void SensorAirspeedSim::Run()
-{
-	if (should_exit()) {
-		ScheduleClear();
-		exit_and_cleanup();
-		return;
-	}
-
-	perf_begin(_loop_perf);
-
-	// Check if parameters have changed
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		updateParams();
-	}
-
-	if (_sim_failure.get() == 0) {
-		if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated()
-		    && _vehicle_attitude_sub.updated()) {
-
-			vehicle_local_position_s lpos{};
-			_vehicle_local_position_sub.copy(&lpos);
-
-			vehicle_global_position_s gpos{};
-			_vehicle_global_position_sub.copy(&gpos);
-
-			vehicle_attitude_s attitude{};
-			_vehicle_attitude_sub.copy(&attitude);
-
-			Vector3f local_velocity = Vector3f{lpos.vx, lpos.vy, lpos.vz};
-			Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * local_velocity;
-
-			// device id
-			device::Device::DeviceId device_id;
-			device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
-			device_id.devid_s.bus = 0;
-			device_id.devid_s.address = 0;
-			device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM;
-
-			const float alt_amsl = gpos.alt;
-			const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl;
-			const float density_ratio = powf(TEMPERATURE_MSL / temperature_local, 4.256f);
-			const float air_density = AIR_DENSITY_MSL / density_ratio;
-
-			// calculate differential pressure + noise in hPa
-			const float diff_pressure_noise = (float)generate_wgn() * 0.01f;
-			float diff_pressure = sign(body_velocity(0)) * 0.005f * air_density  * body_velocity(0) * body_velocity(
-						      0) + diff_pressure_noise;
-
-
-			differential_pressure_s differential_pressure{};
-			// report.timestamp_sample = time;
-			differential_pressure.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
-			differential_pressure.differential_pressure_pa = (double)diff_pressure * 100.0; // hPa to Pa;
-			differential_pressure.temperature = temperature_local + ABSOLUTE_ZERO_C; // K to C
-			differential_pressure.timestamp = hrt_absolute_time();
-			_differential_pressure_pub.publish(differential_pressure);
-
-		}
-	}
-
-	perf_end(_loop_perf);
-}
-
-int SensorAirspeedSim::task_spawn(int argc, char *argv[])
-{
-	SensorAirspeedSim *instance = new SensorAirspeedSim();
-
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
-
-		if (instance->init()) {
-			return PX4_OK;
-		}
-
-	} else {
-		PX4_ERR("alloc failed");
-	}
-
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
-
-	return PX4_ERROR;
-}
-
-int SensorAirspeedSim::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
-}
-
-int SensorAirspeedSim::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("sensor_arispeed_sim", "system");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
-
-extern "C" __EXPORT int sensor_airspeed_sim_main(int argc, char *argv[])
-{
-	return SensorAirspeedSim::main(argc, argv);
-}

src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp

@@ -1,96 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <lib/perf/perf_counter.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-
-using namespace time_literals;
-
-static constexpr float ABSOLUTE_ZERO_C = -273.15; // absolute 0 temperature [C]
-static constexpr float TEMPERATURE_MSL = 288.15; // temperature at MSL [K] (15 [C])
-static constexpr float PRESSURE_MSL = 101325.0; // pressure at MSL [Pa]
-static constexpr float LAPSE_RATE = 0.0065; // reduction in temperature with altitude for troposphere [K/m]
-static constexpr float AIR_DENSITY_MSL = 1.225; // air density at MSL [kg/m^3]
-
-class SensorAirspeedSim : public ModuleBase<SensorAirspeedSim>, public ModuleParams, public px4::ScheduledWorkItem
-{
-public:
-	SensorAirspeedSim();
-	~SensorAirspeedSim() override;
-
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
-
-	bool init();
-
-private:
-	void Run() override;
-
-	// generate white Gaussian noise sample with std=1
-	static float generate_wgn();
-
-	// generate white Gaussian noise sample as a 3D vector with specified std
-	matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz) { return matrix::Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); }
-
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
-	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)};
-
-	uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
-
-	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::SIM_ARSPD_FAIL>) _sim_failure
-	)
-};

src/modules/simulation/sensor_airspeed_sim/parameters.c

@@ -1,56 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * Enable simulated airspeed sensor instance
- *
- * @reboot_required true
- * @min 0
- * @max 1
- * @group Sensors
- * @value 0 Disabled
- * @value 1 Enabled
-  */
-PARAM_DEFINE_INT32(SENS_EN_ARSPDSIM, 0);
-
-/**
- * Dynamically simulate failure of airspeed sensor instance
- *
- * @reboot_required true
- * @min 0
- * @max 1
- * @group Sensors
- * @value 0 Disabled
- * @value 1 Enabled
-  */
-PARAM_DEFINE_INT32(SIM_ARSPD_FAIL, 0);

src/modules/simulation/sensor_baro_sim/CMakeLists.txt

@@ -1,44 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_module(
-	MODULE modules__simulation__sensor_baro_sim
-	MAIN sensor_baro_sim
-	COMPILE_FLAGS
-	SRCS
-		SensorBaroSim.cpp
-		SensorBaroSim.hpp
-	DEPENDS
-		geo
-		px4_work_queue
-	)

src/modules/simulation/sensor_baro_sim/Kconfig

@@ -1,5 +0,0 @@
-menuconfig MODULES_SIMULATION_SENSOR_BARO_SIM
-	bool "sensor_baro_sim"
-	default n
-	---help---
-		Enable support for sensor_baro_sim

src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp

@@ -1,232 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SensorBaroSim.hpp"
-
-#include <drivers/drv_sensor.h>
-
-using namespace matrix;
-
-SensorBaroSim::SensorBaroSim() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
-{
-	srand(1234); // initialize the random seed once before calling generate_wgn()
-}
-
-SensorBaroSim::~SensorBaroSim()
-{
-	perf_free(_loop_perf);
-}
-
-bool SensorBaroSim::init()
-{
-	ScheduleOnInterval(50_ms); // 20 Hz
-	return true;
-}
-
-float SensorBaroSim::generate_wgn()
-{
-	// generate white Gaussian noise sample with std=1
-
-	// algorithm 1:
-	// float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
-	// return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
-	// algorithm 2: from BlockRandGauss.hpp
-	static float V1, V2, S;
-	static bool phase = true;
-	float X;
-
-	if (phase) {
-		do {
-			float U1 = (float)rand() / (float)RAND_MAX;
-			float U2 = (float)rand() / (float)RAND_MAX;
-			V1 = 2.0f * U1 - 1.0f;
-			V2 = 2.0f * U2 - 1.0f;
-			S = V1 * V1 + V2 * V2;
-		} while (S >= 1.0f || fabsf(S) < 1e-8f);
-
-		X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
-
-	} else {
-		X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
-	}
-
-	phase = !phase;
-	return X;
-}
-
-void SensorBaroSim::Run()
-{
-	if (should_exit()) {
-		ScheduleClear();
-		exit_and_cleanup();
-		return;
-	}
-
-	perf_begin(_loop_perf);
-
-	// Check if parameters have changed
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		updateParams();
-	}
-
-	if (_vehicle_global_position_sub.updated()) {
-		vehicle_global_position_s gpos;
-
-		if (_vehicle_global_position_sub.copy(&gpos)) {
-
-			const float dt = math::constrain((gpos.timestamp - _last_update_time) * 1e-6f, 0.001f, 0.1f);
-
-			const float alt_msl = gpos.alt;
-
-			// calculate abs_pressure using an ISA model for the tropsphere (valid up to 11km above MSL)
-			const float lapse_rate = 0.0065f; // reduction in temperature with altitude (Kelvin/m)
-			const float temperature_msl = 288.0f; // temperature at MSL (Kelvin)
-
-			const float temperature_local = temperature_msl - lapse_rate * alt_msl;
-			const float pressure_ratio = powf(temperature_msl / temperature_local, 5.256f);
-			const float pressure_msl = 101325.0f; // pressure at MSL
-			const float absolute_pressure = pressure_msl / pressure_ratio;
-
-			// generate Gaussian noise sequence using polar form of Box-Muller transformation
-			double y1;
-			{
-				double x1;
-				double x2;
-				double w;
-
-				if (!_baro_rnd_use_last) {
-					do {
-						x1 = 2. * (double)generate_wgn() - 1.;
-						x2 = 2. * (double)generate_wgn() - 1.;
-						w = x1 * x1 + x2 * x2;
-					} while (w >= 1.0);
-
-					w = sqrt((-2.0 * log(w)) / w);
-					// calculate two values - the second value can be used next time because it is uncorrelated
-					y1 = x1 * w;
-					_baro_rnd_y2 = x2 * w;
-					_baro_rnd_use_last = true;
-
-				} else {
-					// no need to repeat the calculation - use the second value from last update
-					y1 = _baro_rnd_y2;
-					_baro_rnd_use_last = false;
-				}
-			}
-
-			// Apply noise and drift
-			const float abs_pressure_noise = 1.f * (float)y1;  // 1 Pa RMS noise
-			_baro_drift_pa += _baro_drift_pa_per_sec * dt;
-			const float absolute_pressure_noisy = absolute_pressure + abs_pressure_noise + _baro_drift_pa;
-
-			// convert to hPa
-			float pressure = absolute_pressure_noisy + _sim_baro_off_p.get();
-
-			// calculate temperature in Celsius
-			float temperature = temperature_local - 273.0f + _sim_baro_off_t.get();
-
-			// publish
-			sensor_baro_s sensor_baro{};
-			sensor_baro.timestamp_sample = gpos.timestamp;
-			sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
-			sensor_baro.pressure = pressure;
-			sensor_baro.temperature = temperature;
-			sensor_baro.timestamp = hrt_absolute_time();
-			_sensor_baro_pub.publish(sensor_baro);
-
-
-			_last_update_time = gpos.timestamp;
-		}
-	}
-
-	perf_end(_loop_perf);
-}
-
-int SensorBaroSim::task_spawn(int argc, char *argv[])
-{
-	SensorBaroSim *instance = new SensorBaroSim();
-
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
-
-		if (instance->init()) {
-			return PX4_OK;
-		}
-
-	} else {
-		PX4_ERR("alloc failed");
-	}
-
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
-
-	return PX4_ERROR;
-}
-
-int SensorBaroSim::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
-}
-
-int SensorBaroSim::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("sensor_baro_sim", "system");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
-
-extern "C" __EXPORT int sensor_baro_sim_main(int argc, char *argv[])
-{
-	return SensorBaroSim::main(argc, argv);
-}

src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp

@@ -1,92 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <lib/perf/perf_counter.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <uORB/Publication.hpp>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_baro.h>
-#include <uORB/topics/vehicle_global_position.h>
-
-using namespace time_literals;
-
-class SensorBaroSim : public ModuleBase<SensorBaroSim>, public ModuleParams, public px4::ScheduledWorkItem
-{
-public:
-	SensorBaroSim();
-	~SensorBaroSim() override;
-
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
-
-	bool init();
-
-private:
-	void Run() override;
-
-	// generate white Gaussian noise sample with std=1
-	static float generate_wgn();
-
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
-
-	bool _baro_rnd_use_last{false};
-	double _baro_rnd_y2{0.0};
-	float _baro_drift_pa_per_sec{0.0};
-	float _baro_drift_pa{0.0};
-
-	hrt_abstime _last_update_time{0};
-
-	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
-
-	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::SIM_BARO_OFF_P>) _sim_baro_off_p,
-		(ParamFloat<px4::params::SIM_BARO_OFF_T>) _sim_baro_off_t
-	)
-};

src/modules/simulation/sensor_baro_sim/parameters.c

@@ -1,59 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * Enable simulated barometer sensor instance
- *
- * @reboot_required true
- * @min 0
- * @max 1
- * @group Sensors
- * @value 0 Disabled
- * @value 1 Enabled
-  */
-PARAM_DEFINE_INT32(SENS_EN_BAROSIM, 0);
-
-/**
- * simulated barometer pressure offset
- *
- * @group Simulator
- */
-PARAM_DEFINE_FLOAT(SIM_BARO_OFF_P, 0.0f);
-
-/**
- * simulated barometer temperature offset
- *
- * @group Simulator
- * @unit celcius
- */
-PARAM_DEFINE_FLOAT(SIM_BARO_OFF_T, 0.0f);

src/modules/simulation/sensor_gps_sim/CMakeLists.txt

@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_module(
-	MODULE modules__simulation__sensor_gps_sim
-	MAIN sensor_gps_sim
-	COMPILE_FLAGS
-	SRCS
-		SensorGpsSim.cpp
-		SensorGpsSim.hpp
-	DEPENDS
-		px4_work_queue
-	)

src/modules/simulation/sensor_gps_sim/Kconfig

@@ -1,5 +0,0 @@
-menuconfig MODULES_SIMULATION_SENSOR_GPS_SIM
-	bool "sensor_gps_sim"
-	default n
-	---help---
-		Enable support for sensor_gps_sim

src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp

@@ -1,236 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SensorGpsSim.hpp"
-
-#include <drivers/drv_sensor.h>
-#include <lib/drivers/device/Device.hpp>
-#include <lib/geo/geo.h>
-
-using namespace matrix;
-
-SensorGpsSim::SensorGpsSim() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
-{
-}
-
-SensorGpsSim::~SensorGpsSim()
-{
-	perf_free(_loop_perf);
-}
-
-bool SensorGpsSim::init()
-{
-	ScheduleOnInterval(125_ms); // 8 Hz
-	return true;
-}
-
-float SensorGpsSim::generate_wgn()
-{
-	// generate white Gaussian noise sample with std=1
-
-	// algorithm 1:
-	// float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
-	// return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
-	// algorithm 2: from BlockRandGauss.hpp
-	static float V1, V2, S;
-	static bool phase = true;
-	float X;
-
-	if (phase) {
-		do {
-			float U1 = (float)rand() / (float)RAND_MAX;
-			float U2 = (float)rand() / (float)RAND_MAX;
-			V1 = 2.0f * U1 - 1.0f;
-			V2 = 2.0f * U2 - 1.0f;
-			S = V1 * V1 + V2 * V2;
-		} while (S >= 1.0f || fabsf(S) < 1e-8f);
-
-		X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
-
-	} else {
-		X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
-	}
-
-	phase = !phase;
-	return X;
-}
-
-void SensorGpsSim::Run()
-{
-	if (should_exit()) {
-		ScheduleClear();
-		exit_and_cleanup();
-		return;
-	}
-
-	perf_begin(_loop_perf);
-
-	// Check if parameters have changed
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		updateParams();
-	}
-
-	if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated()) {
-
-		vehicle_local_position_s lpos{};
-		_vehicle_local_position_sub.copy(&lpos);
-
-		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));
-
-		Vector3f gps_vel = Vector3f{lpos.vx, lpos.vy, lpos.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f);
-
-		// device id
-		device::Device::DeviceId device_id;
-		device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
-		device_id.devid_s.bus = 0;
-		device_id.devid_s.address = 0;
-		device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
-
-		sensor_gps_s sensor_gps{};
-
-		if (_sim_gps_used.get() >= 4) {
-			// fix
-			sensor_gps.fix_type = 3; // 3D fix
-			sensor_gps.s_variance_m_s = 0.4f;
-			sensor_gps.c_variance_rad = 0.1f;
-			sensor_gps.eph = 0.9f;
-			sensor_gps.epv = 1.78f;
-			sensor_gps.hdop = 0.7f;
-			sensor_gps.vdop = 1.1f;
-
-		} else {
-			// no fix
-			sensor_gps.fix_type = 0; // No fix
-			sensor_gps.s_variance_m_s = 100.f;
-			sensor_gps.c_variance_rad = 100.f;
-			sensor_gps.eph = 100.f;
-			sensor_gps.epv = 100.f;
-			sensor_gps.hdop = 100.f;
-			sensor_gps.vdop = 100.f;
-		}
-
-		sensor_gps.timestamp_sample = gpos.timestamp_sample;
-		sensor_gps.time_utc_usec = 0;
-		sensor_gps.device_id = device_id.devid;
-		sensor_gps.latitude_deg = latitude; // Latitude in degrees
-		sensor_gps.longitude_deg = longitude; // Longitude in degrees
-		sensor_gps.altitude_msl_m = altitude; // Altitude in meters above MSL
-		sensor_gps.altitude_ellipsoid_m = altitude;
-		sensor_gps.noise_per_ms = 0;
-		sensor_gps.jamming_indicator = 0;
-		sensor_gps.vel_m_s = sqrtf(gps_vel(0) * gps_vel(0) + gps_vel(1) * gps_vel(1)); // GPS ground speed, (metres/sec)
-		sensor_gps.vel_n_m_s = gps_vel(0);
-		sensor_gps.vel_e_m_s = gps_vel(1);
-		sensor_gps.vel_d_m_s = gps_vel(2);
-		sensor_gps.cog_rad = atan2(gps_vel(1),
-					   gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
-		sensor_gps.timestamp_time_relative = 0;
-		sensor_gps.heading = NAN;
-		sensor_gps.heading_offset = NAN;
-		sensor_gps.heading_accuracy = 0;
-		sensor_gps.automatic_gain_control = 0;
-		sensor_gps.jamming_state = 0;
-		sensor_gps.spoofing_state = 0;
-		sensor_gps.vel_ned_valid = true;
-		sensor_gps.satellites_used = _sim_gps_used.get();
-
-		sensor_gps.timestamp = hrt_absolute_time();
-		_sensor_gps_pub.publish(sensor_gps);
-	}
-
-	perf_end(_loop_perf);
-}
-
-int SensorGpsSim::task_spawn(int argc, char *argv[])
-{
-	SensorGpsSim *instance = new SensorGpsSim();
-
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
-
-		if (instance->init()) {
-			return PX4_OK;
-		}
-
-	} else {
-		PX4_ERR("alloc failed");
-	}
-
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
-
-	return PX4_ERROR;
-}
-
-int SensorGpsSim::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
-}
-
-int SensorGpsSim::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("sensor_gps_sim", "system");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
-
-extern "C" __EXPORT int sensor_gps_sim_main(int argc, char *argv[])
-{
-	return SensorGpsSim::main(argc, argv);
-}

src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp

@@ -1,88 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <lib/perf/perf_counter.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_gps.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-
-using namespace time_literals;
-
-class SensorGpsSim : public ModuleBase<SensorGpsSim>, public ModuleParams, public px4::ScheduledWorkItem
-{
-public:
-	SensorGpsSim();
-	~SensorGpsSim() override;
-
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
-
-	bool init();
-
-private:
-	void Run() override;
-
-	// generate white Gaussian noise sample with std=1
-	static float generate_wgn();
-
-	// generate white Gaussian noise sample as a 3D vector with specified std
-	matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz) { return matrix::Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); }
-
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
-	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)};
-
-	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
-
-	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::SIM_GPS_USED>) _sim_gps_used
-	)
-};

src/modules/simulation/sensor_gps_sim/parameters.c

@@ -1,52 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-/**
- * Enable simulated GPS sinstance
- *
- * @reboot_required true
- * @min 0
- * @max 1
- * @group Sensors
- * @value 0 Disabled
- * @value 1 Enabled
-  */
-PARAM_DEFINE_INT32(SENS_EN_GPSSIM, 0);
-
-/**
- * simulated GPS number of satellites used
- *
- * @min 0
- * @max  50
- * @group Simulator
- */
-PARAM_DEFINE_INT32(SIM_GPS_USED, 10);

src/modules/simulation/sensor_mag_sim/Kconfig

@@ -1,5 +0,0 @@
-menuconfig MODULES_SIMULATION_SENSOR_MAG_SIM
-	bool "sensor_mag_sim"
-	default n
-	---help---
-		Enable support for sensor_mag_sim

src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp

@@ -1,196 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "SensorMagSim.hpp"
-
-#include <drivers/drv_sensor.h>
-#include <lib/world_magnetic_model/geo_mag_declination.h>
-
-using namespace matrix;
-
-SensorMagSim::SensorMagSim() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
-{
-	_px4_mag.set_device_type(DRV_MAG_DEVTYPE_MAGSIM);
-}
-
-SensorMagSim::~SensorMagSim()
-{
-	perf_free(_loop_perf);
-}
-
-bool SensorMagSim::init()
-{
-	ScheduleOnInterval(20_ms); // 50 Hz
-	return true;
-}
-
-float SensorMagSim::generate_wgn()
-{
-	// generate white Gaussian noise sample with std=1
-
-	// algorithm 1:
-	// float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
-	// return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
-	// algorithm 2: from BlockRandGauss.hpp
-	static float V1, V2, S;
-	static bool phase = true;
-	float X;
-
-	if (phase) {
-		do {
-			float U1 = (float)rand() / (float)RAND_MAX;
-			float U2 = (float)rand() / (float)RAND_MAX;
-			V1 = 2.0f * U1 - 1.0f;
-			V2 = 2.0f * U2 - 1.0f;
-			S = V1 * V1 + V2 * V2;
-		} while (S >= 1.0f || fabsf(S) < 1e-8f);
-
-		X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
-
-	} else {
-		X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
-	}
-
-	phase = !phase;
-	return X;
-}
-
-void SensorMagSim::Run()
-{
-	if (should_exit()) {
-		ScheduleClear();
-		exit_and_cleanup();
-		return;
-	}
-
-	perf_begin(_loop_perf);
-
-	// Check if parameters have changed
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		updateParams();
-	}
-
-	if (_vehicle_global_position_sub.updated()) {
-		vehicle_global_position_s gpos;
-
-		if (_vehicle_global_position_sub.copy(&gpos)) {
-			if (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));
-				const float field_strength_gauss = get_mag_strength_gauss(gpos.lat, gpos.lon);
-
-				_mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0);
-
-				_mag_earth_available = true;
-			}
-		}
-	}
-
-	if (_mag_earth_available) {
-		vehicle_attitude_s attitude;
-
-		if (_vehicle_attitude_sub.update(&attitude)) {
-			Vector3f expected_field = Dcmf{Quatf{attitude.q}} .transpose() * _mag_earth_pred;
-
-			expected_field += noiseGauss3f(0.02f, 0.02f, 0.03f);
-
-			_px4_mag.update(attitude.timestamp,
-					expected_field(0) + _sim_mag_offset_x.get(),
-					expected_field(1) + _sim_mag_offset_y.get(),
-					expected_field(2) + _sim_mag_offset_z.get());
-		}
-	}
-
-	perf_end(_loop_perf);
-}
-
-int SensorMagSim::task_spawn(int argc, char *argv[])
-{
-	SensorMagSim *instance = new SensorMagSim();
-
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
-
-		if (instance->init()) {
-			return PX4_OK;
-		}
-
-	} else {
-		PX4_ERR("alloc failed");
-	}
-
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
-
-	return PX4_ERROR;
-}
-
-int SensorMagSim::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
-}
-
-int SensorMagSim::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_WARN("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("sensor_mag_sim", "system");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
-}
-
-extern "C" __EXPORT int sensor_mag_sim_main(int argc, char *argv[])
-{
-	return SensorMagSim::main(argc, argv);
-}

src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp

@@ -1,94 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
-#include <lib/perf/perf_counter.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_global_position.h>
-
-using namespace time_literals;
-
-class SensorMagSim : public ModuleBase<SensorMagSim>, public ModuleParams, public px4::ScheduledWorkItem
-{
-public:
-	SensorMagSim();
-	~SensorMagSim() override;
-
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
-
-	bool init();
-
-private:
-	void Run() override;
-
-	// generate white Gaussian noise sample with std=1
-	static float generate_wgn();
-
-	// generate white Gaussian noise sample as a 3D vector with specified std
-	matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz) { return matrix::Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); }
-
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude_groundtruth)};
-	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
-
-	PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 3, TYPE: SIMULATION
-
-	bool _mag_earth_available{false};
-
-	matrix::Vector3f _mag_earth_pred{};
-
-	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
-
-	DEFINE_PARAMETERS(
-		(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
-	)
-};

src/modules/simulation/sensor_sim_manager/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+# Copyright (c) 2025 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -32,14 +32,17 @@
 ############################################################################
 
 px4_add_module(
-	MODULE modules__simulation__senosr_mag_sim
-	MAIN sensor_mag_sim
+	MODULE modules__simulation__sensor_sim_manager
+	MAIN sensor_sim_manager
 	COMPILE_FLAGS
+	INCLUDES
+		${CMAKE_CURRENT_SOURCE_DIR}/../failure_injection
 	SRCS
-		SensorMagSim.cpp
-		SensorMagSim.hpp
+		SensorSimManager.cpp
+		SensorSimManager.hpp
+		${CMAKE_CURRENT_SOURCE_DIR}/../failure_injection/failureInjection.cpp
 	DEPENDS
-		drivers_magnetometer
 		geo
+		mathlib
 		px4_work_queue
-	)
+)

src/modules/simulation/sensor_sim_manager/Kconfig

@@ -0,0 +1,5 @@
+menuconfig MODULES_SIMULATION_SENSOR_SIM_MANAGER
+	bool "sensor_sim_manager"
+	default n
+	---help---
+		Enable support for sensor_sim_manager - Unified sensor simulation manager for SIH

src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp

@@ -0,0 +1,743 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "SensorSimManager.hpp"
+
+#include <drivers/drv_sensor.h>
+#include <lib/drivers/device/Device.hpp>
+#include <lib/geo/geo.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/matrix/matrix/math.hpp>
+
+using matrix::sign;
+
+using namespace matrix;
+
+SensorSimManager::SensorSimManager() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+{
+	static constexpr float kT1C = 15.0f; // Temperature constant
+	_px4_accel.set_temperature(kT1C);
+	_px4_gyro.set_temperature(kT1C);
+
+	srand(1234);
+
+	// Initialize sensor timings with base intervals and random offsets
+	const hrt_abstime now = hrt_absolute_time();
+
+	// GPS: 8 Hz
+	_gps_timing = {
+		.interval_us = 125000,
+		.next_update_time = now,
+		.offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 125000),
+		.enabled = false,
+	};
+
+	// Barometer: 20 Hz
+	_baro_timing = {
+		.interval_us = 50000,
+		.next_update_time = now,
+		.offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 50000),
+		.enabled = false,
+	};
+
+	// Magnetometer: 50 Hz
+	_mag_timing = {
+		.interval_us = 20000,
+		.next_update_time = now,
+		.offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 20000),
+		.enabled = false,
+	};
+
+	// Airspeed: 8 Hz
+	_airspeed_timing = {
+		.interval_us = 125000,
+		.next_update_time = now,
+		.offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 125000),
+		.enabled = false,
+	};
+
+	// AGP: 2 Hz
+	_agp_timing = {
+		.interval_us = 500000,
+		.next_update_time = now,
+		.offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 500000),
+		.enabled = false,
+	};
+
+	// Distance sensor: 50 Hz
+	_distance_sensor_timing = {
+		.interval_us = 20000,
+		.next_update_time = now,
+		.offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 20000),
+		.enabled = false,
+	};
+
+	// IMU: 250 Hz
+	_imu_timing = {
+		.interval_us = 4000,
+		.next_update_time = now,
+		.offset_us = static_cast<hrt_abstime>((float(rand()) / RAND_MAX) * 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;
+	_baro_timing.next_update_time += _baro_timing.offset_us;
+	_mag_timing.next_update_time += _mag_timing.offset_us;
+	_airspeed_timing.next_update_time += _airspeed_timing.offset_us;
+	_agp_timing.next_update_time += _agp_timing.offset_us;
+	_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()
+{
+	perf_free(_loop_perf);
+	perf_free(_gps_perf);
+	perf_free(_baro_perf);
+	perf_free(_mag_perf);
+	perf_free(_airspeed_perf);
+	perf_free(_agp_perf);
+	perf_free(_imu_perf);
+	perf_free(_distance_sensor_perf);
+}
+
+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;
+}
+
+float SensorSimManager::generate_wgn()
+{
+	// Generate white Gaussian noise sample with std=1
+	// Using Box-Muller transform
+	static float V1, V2, S;
+	static bool phase = true;
+	float X;
+
+	if (phase) {
+		do {
+			const float U1 = (float)rand() / (float)RAND_MAX;
+			const float U2 = (float)rand() / (float)RAND_MAX;
+			V1 = 2.0f * U1 - 1.0f;
+			V2 = 2.0f * U2 - 1.0f;
+			S = V1 * V1 + V2 * V2;
+		} while (S >= 1.0f || fabsf(S) < 1e-8f);
+
+		X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
+
+	} else {
+		X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
+	}
+
+	phase = !phase;
+	return X;
+}
+
+matrix::Vector3f SensorSimManager::noiseGauss3f(float stdx, float stdy, float stdz)
+{
+	return matrix::Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz);
+}
+
+void SensorSimManager::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
+	}
+
+	perf_begin(_loop_perf);
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	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;
+	}
+
+	if (_baro_timing.enabled && now >= _baro_timing.next_update_time) {
+		updateBarometer();
+		_baro_timing.next_update_time = now + _baro_timing.interval_us;
+	}
+
+	if (_mag_timing.enabled && now >= _mag_timing.next_update_time) {
+		updateMagnetometer();
+		_mag_timing.next_update_time = now + _mag_timing.interval_us;
+	}
+
+	if (_airspeed_timing.enabled && now >= _airspeed_timing.next_update_time) {
+		updateAirspeed();
+		_airspeed_timing.next_update_time = now + _airspeed_timing.interval_us;
+	}
+
+	if (_agp_timing.enabled && now >= _agp_timing.next_update_time) {
+		updateAGP();
+		_agp_timing.next_update_time = now + _agp_timing.interval_us;
+	}
+
+	if (_imu_timing.enabled && now >= _imu_timing.next_update_time) {
+		updateIMU();
+		_imu_timing.next_update_time = now + _imu_timing.interval_us;
+	}
+
+	if (_distance_sensor_timing.enabled && now >= _distance_sensor_timing.next_update_time) {
+		updateDistanceSensor();
+		_distance_sensor_timing.next_update_time = now + _distance_sensor_timing.interval_us;
+	}
+
+	perf_end(_loop_perf);
+}
+
+void SensorSimManager::updateGPS()
+{
+	perf_begin(_gps_perf);
+
+	vehicle_local_position_s lpos{};
+	_vehicle_local_position_sub.copy(&lpos);
+
+	vehicle_global_position_s gpos{};
+	_vehicle_global_position_sub.copy(&gpos);
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	if (lpos.timestamp > 0 && gpos.timestamp > 0 &&
+	    (now - lpos.timestamp) < kGroundtruthDataMaxAgeUs && (now - gpos.timestamp) < kGroundtruthDataMaxAgeUs) {
+
+		const double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
+		const double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
+		const double altitude = (double)(gpos.alt + (generate_wgn() * 0.5f));
+
+		const Vector3f gps_vel = Vector3f{lpos.vx, lpos.vy, lpos.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f);
+
+		// device id
+		device::Device::DeviceId device_id;
+		device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
+		device_id.devid_s.bus = 0;
+		device_id.devid_s.address = 0;
+		device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
+
+		sensor_gps_s sensor_gps{};
+
+		if (_sim_gps_used.get() >= 4) {
+			// fix
+			sensor_gps.fix_type = 3; // 3D fix
+			sensor_gps.s_variance_m_s = 0.4f;
+			sensor_gps.c_variance_rad = 0.1f;
+			sensor_gps.eph = 0.9f;
+			sensor_gps.epv = 1.78f;
+			sensor_gps.hdop = 0.7f;
+			sensor_gps.vdop = 1.1f;
+
+		} else {
+			// no fix
+			sensor_gps.fix_type = 0; // No fix
+			sensor_gps.s_variance_m_s = 100.f;
+			sensor_gps.c_variance_rad = 100.f;
+			sensor_gps.eph = 100.f;
+			sensor_gps.epv = 100.f;
+			sensor_gps.hdop = 100.f;
+			sensor_gps.vdop = 100.f;
+		}
+
+		sensor_gps.timestamp_sample = gpos.timestamp_sample;
+		sensor_gps.time_utc_usec = 0;
+		sensor_gps.device_id = device_id.devid;
+		sensor_gps.latitude_deg = latitude;
+		sensor_gps.longitude_deg = longitude;
+		sensor_gps.altitude_msl_m = altitude;
+		sensor_gps.altitude_ellipsoid_m = altitude;
+		sensor_gps.noise_per_ms = 0;
+		sensor_gps.jamming_indicator = 0;
+		sensor_gps.vel_m_s = gps_vel.xy().norm();
+		sensor_gps.vel_n_m_s = gps_vel(0);
+		sensor_gps.vel_e_m_s = gps_vel(1);
+		sensor_gps.vel_d_m_s = gps_vel(2);
+		sensor_gps.cog_rad = atan2(gps_vel(1),
+					   gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
+		sensor_gps.timestamp_time_relative = 0;
+		sensor_gps.heading = NAN;
+		sensor_gps.heading_offset = NAN;
+		sensor_gps.heading_accuracy = 0;
+		sensor_gps.automatic_gain_control = 0;
+		sensor_gps.jamming_state = 0;
+		sensor_gps.spoofing_state = 0;
+		sensor_gps.vel_ned_valid = true;
+		sensor_gps.satellites_used = _sim_gps_used.get();
+
+		sensor_gps.timestamp = hrt_absolute_time();
+
+		if (_failure_injection.handle_gps_failure(sensor_gps) && _failure_injection.handle_gps_alt_failure(sensor_gps)) {
+			_sensor_gps_pub.publish(sensor_gps);
+		}
+	}
+
+	perf_end(_gps_perf);
+}
+
+void SensorSimManager::updateBarometer()
+{
+	perf_begin(_baro_perf);
+
+	vehicle_global_position_s gpos;
+
+	if (_vehicle_global_position_sub.copy(&gpos)) {
+		const hrt_abstime now = hrt_absolute_time();
+
+		if (gpos.timestamp > 0 && (now - gpos.timestamp) < kGroundtruthDataMaxAgeUs) {
+			const float dt = math::constrain((gpos.timestamp - _last_baro_update_time) * 1e-6f, 0.001f, 0.1f);
+
+			const float temperature_local = kTemperatureMsl - kLapseRate * gpos.alt;
+			const float pressure_ratio = powf(kTemperatureMsl / temperature_local, 5.256f);
+			const float absolute_pressure = kPressureMsl / pressure_ratio;
+
+			if (!_failure_injection.is_baro_stuck()) {
+				const float abs_pressure_noise = generate_wgn();  // 1 Pa RMS noise
+				_baro_drift_pa += _baro_drift_pa_per_sec * dt;
+				const float absolute_pressure_noisy = absolute_pressure + abs_pressure_noise + _baro_drift_pa;
+
+				_last_baro_pressure = absolute_pressure_noisy + _sim_baro_off_p.get();
+
+				_last_baro_temperature = temperature_local - kAbsoluteZeroC + _sim_baro_off_t.get();
+			}
+
+			if (!_failure_injection.is_baro_blocked()) {
+				sensor_baro_s sensor_baro{};
+				sensor_baro.timestamp_sample = gpos.timestamp;
+				sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
+				sensor_baro.pressure = _last_baro_pressure;
+				sensor_baro.temperature = _last_baro_temperature;
+				sensor_baro.timestamp = hrt_absolute_time();
+				_sensor_baro_pub.publish(sensor_baro);
+			}
+
+			_last_baro_update_time = gpos.timestamp;
+		}
+	}
+
+	perf_end(_baro_perf);
+}
+
+void SensorSimManager::updateMagnetometer()
+{
+	perf_begin(_mag_perf);
+
+	vehicle_global_position_s gpos;
+
+	if (_vehicle_global_position_sub.copy(&gpos)) {
+		const hrt_abstime now = hrt_absolute_time();
+
+		if (gpos.timestamp > 0 && (now - gpos.timestamp) < kGroundtruthDataMaxAgeUs && 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));
+			const float field_strength_gauss = get_mag_strength_gauss(gpos.lat, gpos.lon);
+
+			_mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0);
+
+			_mag_earth_available = true;
+		}
+	}
+
+	if (_mag_earth_available) {
+		vehicle_attitude_s attitude;
+
+		if (_vehicle_attitude_sub.copy(&attitude)) {
+			if (_failure_injection.is_mag_stuck(0)) {
+				_px4_mag.update(attitude.timestamp, _last_mag(0), _last_mag(1), _last_mag(2));
+
+			} else if (!_failure_injection.is_mag_blocked(0)) {
+				Vector3f expected_field = Dcmf{Quatf{attitude.q}} .transpose() * _mag_earth_pred;
+
+				expected_field += noiseGauss3f(0.02f, 0.02f, 0.03f);
+
+				const Vector3f mag_output{
+					expected_field(0) + _sim_mag_offset_x.get(),
+					expected_field(1) + _sim_mag_offset_y.get(),
+					expected_field(2) + _sim_mag_offset_z.get()
+				};
+
+				_px4_mag.update(attitude.timestamp, mag_output(0), mag_output(1), mag_output(2));
+				_last_mag = mag_output;
+			}
+		}
+	}
+
+	perf_end(_mag_perf);
+}
+
+void SensorSimManager::updateAirspeed()
+{
+	perf_begin(_airspeed_perf);
+
+	if (_failure_injection.is_airspeed_disconnected()) {
+		perf_end(_airspeed_perf);
+		return;
+	}
+
+	vehicle_local_position_s lpos{};
+	_vehicle_local_position_sub.copy(&lpos);
+
+	vehicle_global_position_s gpos{};
+	_vehicle_global_position_sub.copy(&gpos);
+
+	vehicle_attitude_s attitude{};
+	_vehicle_attitude_sub.copy(&attitude);
+
+	// 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) < kGroundtruthDataMaxAgeUs && (now - gpos.timestamp) < kGroundtruthDataMaxAgeUs
+	    && (now - attitude.timestamp) < kGroundtruthDataMaxAgeUs) {
+
+		const hrt_abstime blocked_timestamp = _failure_injection.get_airspeed_blocked_timestamp();
+		const float temperature_local = kTemperatureMsl - kLapseRate * gpos.alt;
+		const float density_ratio = powf(kTemperatureMsl / temperature_local, 4.256f);
+		const float air_density = kAirDensityMsl / density_ratio;
+		const Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * Vector3f{lpos.vx, lpos.vy, lpos.vz};
+
+		// calculate differential pressure + noise in hPa
+		const float diff_pressure_noise = (float)generate_wgn() * 0.01f;
+		const float diff_pressure = sign(body_velocity(0)) * 0.005f * air_density  * body_velocity(0) * body_velocity(
+					      0) + diff_pressure_noise;
+
+		const float blockage_fraction = 0.7f; // defines max blockage (fully ramped)
+		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);
+		}
+
+		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.temperature = temperature_local + kAbsoluteZeroC; // K to C
+		differential_pressure.timestamp = hrt_absolute_time();
+		_differential_pressure_pub.publish(differential_pressure);
+	}
+
+	perf_end(_airspeed_perf);
+}
+
+void SensorSimManager::updateAGP()
+{
+	perf_begin(_agp_perf);
+
+	vehicle_global_position_s gpos{};
+	_vehicle_global_position_sub.copy(&gpos);
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	if (gpos.timestamp > 0 && (now - gpos.timestamp) < kGroundtruthDataMaxAgeUs) {
+
+		if (_failure_injection.is_agp_blocked()) {
+			perf_end(_agp_perf);
+			return;
+		}
+
+		const float dt = (gpos.timestamp - _agp_time_last_update) * 1e-6f;
+		_agp_time_last_update = gpos.timestamp;
+
+		if (!_failure_injection.is_agp_stuck()) {
+			_agp_measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid);
+		}
+
+		if (_failure_injection.is_agp_drift()) {
+			_agp_position_bias += matrix::Vector3f(1.5f, -5.f, 0.f) * dt;
+			_agp_measured_lla += _agp_position_bias;
+
+		} else {
+			_agp_position_bias.zero();
+		}
+
+		const double latitude = _agp_measured_lla.latitude_deg() + math::degrees((double)generate_wgn() * 2.0 /
+					CONSTANTS_RADIUS_OF_EARTH);
+		const double longitude = _agp_measured_lla.longitude_deg() + math::degrees((double)generate_wgn() * 2.0 /
+					 CONSTANTS_RADIUS_OF_EARTH);
+		const double altitude = (double)(_agp_measured_lla.altitude() + (generate_wgn() * 0.5f));
+
+		vehicle_global_position_s agp_sample{};
+
+		agp_sample.timestamp_sample = gpos.timestamp_sample;
+		agp_sample.lat = latitude;
+		agp_sample.lon = longitude;
+		agp_sample.alt = altitude;
+		agp_sample.lat_lon_valid = true;
+		agp_sample.alt_ellipsoid = altitude;
+		agp_sample.alt_valid = true;
+		agp_sample.eph = 20.f;
+		agp_sample.epv = 5.f;
+
+		agp_sample.timestamp = hrt_absolute_time();
+		_aux_global_position_pub.publish(agp_sample);
+	}
+
+	perf_end(_agp_perf);
+}
+
+void SensorSimManager::updateIMU()
+{
+	perf_begin(_imu_perf);
+
+	vehicle_global_position_s gpos{};
+	_vehicle_global_position_sub.copy(&gpos);
+
+	vehicle_local_position_s lpos{};
+	_vehicle_local_position_sub.copy(&lpos);
+
+	vehicle_attitude_s attitude{};
+	_vehicle_attitude_sub.copy(&attitude);
+
+	vehicle_angular_velocity_s angular_velocity{};
+	_vehicle_angular_velocity_sub.copy(&angular_velocity);
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	if (gpos.timestamp > 0 && lpos.timestamp > 0 && attitude.timestamp > 0 && angular_velocity.timestamp > 0 &&
+	    (now - gpos.timestamp) < kGroundtruthDataMaxAgeUs && (now - lpos.timestamp) < kGroundtruthDataMaxAgeUs &&
+	    (now - attitude.timestamp) < kGroundtruthDataMaxAgeUs
+	    && (now - angular_velocity.timestamp) < kGroundtruthDataMaxAgeUs) {
+
+		// The sensor signals reconstruction and noise levels are from [1]
+		// [1] Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight."
+		//     In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018.
+
+		const Dcmf R_E2B(Quatf{attitude.q}.inversed());
+
+		const Vector3f acceleration_N{lpos.ax, lpos.ay, lpos.az}; // acceleration in NED frame
+		const Vector3f specific_force_N = acceleration_N - Vector3f{0.0f, 0.0f, 9.80665f};
+		const Vector3f specific_force_B = R_E2B * specific_force_N;
+
+		Vector3f accel_noise;
+		Vector3f gyro_noise;
+
+		// Use velocity magnitude to determine noise level (higher noise when moving)
+		const Vector3f velocity{lpos.vx, lpos.vy, lpos.vz};
+
+		if (velocity.norm() > 0.1f) {
+			accel_noise = noiseGauss3f(0.5f, 1.7f, 1.4f);
+			gyro_noise = noiseGauss3f(0.14f, 0.07f, 0.03f);
+
+		} else {
+			accel_noise = noiseGauss3f(0.1f, 0.1f, 0.1f);
+			gyro_noise = noiseGauss3f(0.01f, 0.01f, 0.01f);
+		}
+
+		const Vector3f accel = specific_force_B + accel_noise;
+
+		// Angular velocity with Earth rotation and noise
+		const Vector3f w_B{angular_velocity.xyz[0], angular_velocity.xyz[1], angular_velocity.xyz[2]};
+		const Vector3f earth_spin_rate_B = R_E2B * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE);
+		const Vector3f gyro = w_B + earth_spin_rate_B + gyro_noise;
+
+		if (_failure_injection.is_accel_stuck()) {
+			_px4_accel.update(now, _last_accel(0), _last_accel(1), _last_accel(2));
+
+		} else if (!_failure_injection.is_accel_blocked()) {
+			_px4_accel.update(now, accel(0), accel(1), accel(2));
+		}
+
+		if (_failure_injection.is_gyro_stuck()) {
+			_px4_gyro.update(now, _last_gyro(0), _last_gyro(1), _last_gyro(2));
+
+		} else if (!_failure_injection.is_gyro_blocked()) {
+			_px4_gyro.update(now, gyro(0), gyro(1), gyro(2));
+		}
+
+		_last_accel = accel;
+		_last_gyro = gyro;
+	}
+
+	perf_end(_imu_perf);
+}
+
+void SensorSimManager::updateDistanceSensor()
+{
+	perf_begin(_distance_sensor_perf);
+
+	vehicle_local_position_s lpos{};
+	_vehicle_local_position_sub.copy(&lpos);
+
+	vehicle_attitude_s attitude{};
+	_vehicle_attitude_sub.copy(&attitude);
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	if (lpos.timestamp > 0 && attitude.timestamp > 0 &&
+	    (now - lpos.timestamp) < kGroundtruthDataMaxAgeUs && (now - attitude.timestamp) < kGroundtruthDataMaxAgeUs) {
+
+		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;
+		device_id.devid_s.bus = 0;
+		device_id.devid_s.address = 0;
+		device_id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM;
+
+		distance_sensor_s distance_sensor{};
+		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.get();
+		distance_sensor.max_distance = _distance_snsr_max.get();
+		distance_sensor.signal_quality = -1;
+
+		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};
+			current_distance = -lpos.z / q.dcm_z()(2);
+
+			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);
+	}
+
+	perf_end(_distance_sensor_perf);
+}
+
+int SensorSimManager::task_spawn(int argc, char *argv[])
+{
+	SensorSimManager *instance = new SensorSimManager();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int SensorSimManager::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int SensorSimManager::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Unified sensor simulation manager for SIH (Simulation in Hardware).
+Simulates GPS, barometer, magnetometer, airspeed, and AGP sensors with realistic noise and timing.
+Integrates failure injection capabilities for robust testing.
+
+### Examples
+To start the sensor simulation manager:
+$ sensor_sim_manager start
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("sensor_sim_manager", "simulation");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int sensor_sim_manager_main(int argc, char *argv[])
+{
+	return SensorSimManager::main(argc, argv);
+}

src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp

@@ -0,0 +1,200 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/sensor_mag.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <simulation/failure_injection/failureInjection.hpp>
+#include <drivers/drv_sensor.h>
+#include <lib/drivers/device/Device.hpp>
+#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <lib/world_magnetic_model/geo_mag_declination.h>
+#include <lib/geo/geo.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/lat_lon_alt/lat_lon_alt.hpp>
+#include <parameters/param.h>
+#include <px4_platform_common/px4_config.h>
+
+using namespace time_literals;
+
+class SensorSimManager : public ModuleBase<SensorSimManager>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	SensorSimManager();
+	~SensorSimManager() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+private:
+	void Run() override;
+
+	// Sensor simulation methods
+	void updateGPS();
+	void updateBarometer();
+	void updateMagnetometer();
+	void updateAirspeed();
+	void updateAGP();
+	void updateIMU();
+	void updateDistanceSensor();
+
+	// Utility methods
+	static float generate_wgn();
+	matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz);
+
+	// Sensor timing structure
+	struct SensorTiming {
+		hrt_abstime interval_us;
+		hrt_abstime next_update_time;
+		hrt_abstime offset_us;
+		bool enabled;
+	};
+
+	// Sensor timings with random offsets
+	SensorTiming _gps_timing;
+	SensorTiming _baro_timing;
+	SensorTiming _mag_timing;
+	SensorTiming _airspeed_timing;
+	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};
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude_groundtruth)};
+	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity_groundtruth)};
+
+	// Publications
+	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
+	uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
+	uORB::PublicationMulti<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
+	uORB::PublicationMulti<vehicle_global_position_s> _aux_global_position_pub{ORB_ID(aux_global_position)};
+
+	PX4Accelerometer _px4_accel{1310988, ROTATION_NONE}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	PX4Gyroscope     _px4_gyro{1310988, ROTATION_NONE};  // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
+
+	FailureInjection _failure_injection{};
+
+	matrix::Vector3f _last_mag{};
+	float _last_baro_pressure{0.0f};
+	float _last_baro_temperature{0.0f};
+
+	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t _gps_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gps")};
+	perf_counter_t _baro_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": baro")};
+	perf_counter_t _mag_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": mag")};
+	perf_counter_t _airspeed_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": airspeed")};
+	perf_counter_t _agp_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": agp")};
+	perf_counter_t _imu_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": imu")};
+	perf_counter_t _distance_sensor_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": distance")};
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SENS_EN_GPSSIM>) _param_sens_en_gpssim,
+		(ParamInt<px4::params::SENS_EN_BAROSIM>) _param_sens_en_barosim,
+		(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,
+		(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};
+	float _baro_drift_pa{0.0f};
+	float _baro_drift_pa_per_sec{0.1f};
+
+	matrix::Vector3f _mag_earth_pred{};
+	bool _mag_earth_available{false};
+
+	LatLonAlt _agp_measured_lla{};
+	matrix::Vector3f _agp_position_bias{};
+	uint64_t _agp_time_last_update{0};
+
+	matrix::Vector3f _last_accel{};
+	matrix::Vector3f _last_gyro{};
+
+	float _last_distance_sensor_value{0.0f};
+
+	// Air constants
+	static constexpr float kAbsoluteZeroC = -273.15f; // [K]
+	static constexpr float kTemperatureMsl = 288.15f; // [K]
+	static constexpr float kPressureMsl = 101325.0f; // [Pa]
+	static constexpr float kLapseRate = 0.0065f; // [K/m]
+	static constexpr float kAirDensityMsl = 1.225f; // [kg/m^3]
+
+	static constexpr hrt_abstime kGroundtruthDataMaxAgeUs = 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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,37 +31,141 @@
  *
  ****************************************************************************/
 /**
- * Enable simulated magnetometer sensor instance
+ * Enable simulated GPS sensor
  *
- * @reboot_required true
- * @min 0
- * @max 1
- * @group Sensors
+ * @group Simulation
  * @value 0 Disabled
  * @value 1 Enabled
-  */
+ */
+PARAM_DEFINE_INT32(SENS_EN_GPSSIM, 1);
+
+/**
+ * Enable simulated barometer sensor
+ *
+ * @group Simulation
+ * @value 0 Disabled
+ * @value 1 Enabled
+ */
+PARAM_DEFINE_INT32(SENS_EN_BAROSIM, 0);
+
+/**
+ * Enable simulated magnetometer sensor
+ *
+ * @group Simulation
+ * @value 0 Disabled
+ * @value 1 Enabled
+ */
 PARAM_DEFINE_INT32(SENS_EN_MAGSIM, 0);
 
+/**
+ * Enable simulated airspeed sensor
+ *
+ * @group Simulation
+ * @value 0 Disabled
+ * @value 1 Enabled
+ */
+PARAM_DEFINE_INT32(SENS_EN_ARSPDSIM, 0);
+
+/**
+ * Enable simulated AGP sensor
+ *
+ * @group Simulation
+ * @value 0 Disabled
+ * @value 1 Enabled
+ */
+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
+ *
+ * @group Simulator
+ * @min 0
+ * @max 50
+ */
+PARAM_DEFINE_INT32(SIM_GPS_USED, 10);
+
+/**
+ * simulated barometer pressure offset
+ *
+ * @group Simulator
+ */
+PARAM_DEFINE_FLOAT(SIM_BARO_OFF_P, 0.0f);
+
+/**
+ * simulated barometer temperature offset
+ *
+ * @group Simulator
+ * @unit celcius
+ */
+PARAM_DEFINE_FLOAT(SIM_BARO_OFF_T, 0.0f);
+
 /**
  * simulated magnetometer X offset
  *
- * @unit gauss
  * @group Simulator
+ * @unit gauss
  */
 PARAM_DEFINE_FLOAT(SIM_MAG_OFFSET_X, 0.0f);
 
 /**
  * simulated magnetometer Y offset
  *
- * @unit gauss
  * @group Simulator
+ * @unit gauss
  */
 PARAM_DEFINE_FLOAT(SIM_MAG_OFFSET_Y, 0.0f);
 
 /**
  * simulated magnetometer Z offset
  *
- * @unit gauss
  * @group Simulator
+ * @unit gauss
  */
-PARAM_DEFINE_FLOAT(SIM_MAG_OFFSET_Z,  0.0f);
+PARAM_DEFINE_FLOAT(SIM_MAG_OFFSET_Z, 0.0f);
+
+/**
+ * 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);
+
+/**
+ * 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/CMakeLists.txt

@@ -41,9 +41,11 @@ px4_add_module(
 	INCLUDES
 		${CMAKE_BINARY_DIR}/mavlink
 		${CMAKE_BINARY_DIR}/mavlink/${CONFIG_MAVLINK_DIALECT}
+		${CMAKE_CURRENT_SOURCE_DIR}/../failure_injection
 	SRCS
 		SimulatorMavlink.cpp
 		SimulatorMavlink.hpp
+		${CMAKE_CURRENT_SOURCE_DIR}/../failure_injection/failureInjection.cpp
 	DEPENDS
 		mavlink_c_generate
 		conversion

src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

@@ -197,8 +197,8 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
 
 	// accel
 	if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) {
-		if (sensors.id >= ACCEL_COUNT_MAX) {
-			PX4_ERR("Number of simulated accelerometer %d out of range. Max: %d", sensors.id, ACCEL_COUNT_MAX);
+		if (sensors.id >= kAccelCountMax) {
+			PX4_ERR("Number of simulated accelerometer %d out of range. Max: %d", sensors.id, kAccelCountMax);
 			return;
 		}
 
@@ -210,10 +210,10 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
 			_px4_accel[sensors.id].set_scale(ACCEL_FIFO_SCALE);
 			_px4_accel[sensors.id].set_range(ACCEL_FIFO_RANGE);
 
-			if (_accel_stuck[sensors.id]) {
+			if (_failure_injection.is_accel_stuck(sensors.id)) {
 				_px4_accel[sensors.id].updateFIFO(_last_accel_fifo);
 
-			} else if (!_accel_blocked[sensors.id]) {
+			} else if (!_failure_injection.is_accel_blocked(sensors.id)) {
 				_px4_accel[sensors.id].set_temperature(_sensors_temperature);
 
 				_last_accel_fifo.samples = 1;
@@ -227,10 +227,11 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
 			}
 
 		} else {
-			if (_accel_stuck[sensors.id]) {
+
+			if (_failure_injection.is_accel_stuck(sensors.id)) {
 				_px4_accel[sensors.id].update(time, _last_accel[sensors.id](0), _last_accel[sensors.id](1), _last_accel[sensors.id](2));
 
-			} else if (!_accel_blocked[sensors.id]) {
+			} else if (!_failure_injection.is_accel_blocked(sensors.id)) {
 				_px4_accel[sensors.id].set_temperature(_sensors_temperature);
 				_px4_accel[sensors.id].update(time, sensors.xacc, sensors.yacc, sensors.zacc);
 				_last_accel[sensors.id] = matrix::Vector3f{sensors.xacc, sensors.yacc, sensors.zacc};
@@ -240,8 +241,8 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
 
 	// gyro
 	if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) {
-		if (sensors.id >= GYRO_COUNT_MAX) {
-			PX4_ERR("Number of simulated gyroscope %d out of range. Max: %d", sensors.id, GYRO_COUNT_MAX);
+		if (sensors.id >= kGyroCountMax) {
+			PX4_ERR("Number of simulated gyroscope %d out of range. Max: %d", sensors.id, kGyroCountMax);
 			return;
 		}
 
@@ -253,10 +254,10 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
 			_px4_gyro[sensors.id].set_scale(GYRO_FIFO_SCALE);
 			_px4_gyro[sensors.id].set_range(GYRO_FIFO_RANGE);
 
-			if (_gyro_stuck[sensors.id]) {
+			if (_failure_injection.is_gyro_stuck(sensors.id)) {
 				_px4_gyro[sensors.id].updateFIFO(_last_gyro_fifo);
 
-			} else if (!_gyro_blocked[sensors.id]) {
+			} else if (!_failure_injection.is_gyro_blocked(sensors.id)) {
 				_px4_gyro[sensors.id].set_temperature(_sensors_temperature);
 
 				_last_gyro_fifo.samples = 1;
@@ -270,10 +271,10 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
 			}
 
 		} else {
-			if (_gyro_stuck[sensors.id]) {
+			if (_failure_injection.is_gyro_stuck(sensors.id)) {
 				_px4_gyro[sensors.id].update(time, _last_gyro[sensors.id](0), _last_gyro[sensors.id](1), _last_gyro[sensors.id](2));
 
-			} else if (!_gyro_blocked[sensors.id]) {
+			} else if (!_failure_injection.is_gyro_blocked(sensors.id)) {
 				_px4_gyro[sensors.id].set_temperature(_sensors_temperature);
 				_px4_gyro[sensors.id].update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
 				_last_gyro[sensors.id] = matrix::Vector3f{sensors.xgyro, sensors.ygyro, sensors.zgyro};
@@ -283,27 +284,25 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
 
 	// magnetometer
 	if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG) {
-		if (sensors.id >= MAG_COUNT_MAX) {
-			PX4_ERR("Number of simulated magnetometer %d out of range. Max: %d", sensors.id, MAG_COUNT_MAX);
+		if (sensors.id >= kMagCountMax) {
+			PX4_ERR("Number of simulated magnetometer %d out of range. Max: %d", sensors.id, kMagCountMax);
 			return;
 		}
 
-		if (_mag_stuck[sensors.id]) {
-			_px4_mag[sensors.id].update(time, _last_magx[sensors.id], _last_magy[sensors.id], _last_magz[sensors.id]);
+		if (_failure_injection.is_mag_stuck(sensors.id)) {
+			_px4_mag[sensors.id].update(time, _last_mag[sensors.id](0), _last_mag[sensors.id](1), _last_mag[sensors.id](2));
 
-		} else if (!_mag_blocked[sensors.id]) {
+		} else if (!_failure_injection.is_mag_blocked(sensors.id)) {
 			_px4_mag[sensors.id].set_temperature(_sensors_temperature);
 			_px4_mag[sensors.id].update(time, sensors.xmag, sensors.ymag, sensors.zmag);
-			_last_magx[sensors.id] = sensors.xmag;
-			_last_magy[sensors.id] = sensors.ymag;
-			_last_magz[sensors.id] = sensors.zmag;
+			_last_mag[sensors.id] = matrix::Vector3f{sensors.xmag, sensors.ymag, sensors.zmag};
 		}
 	}
 
 	// baro
-	if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_baro_blocked) {
+	if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_failure_injection.is_baro_blocked()) {
 
-		if (!_baro_stuck) {
+		if (!_failure_injection.is_baro_stuck()) {
 			_last_baro_pressure = sensors.abs_pressure * 100.f; // hPa to Pa
 			_last_baro_temperature = sensors.temperature;
 		}
@@ -326,15 +325,18 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil
 	}
 
 	// differential pressure
-	if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_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
 
 		float airspeed_blockage_scale = 1.f;
 
-		if (_airspeed_blocked_timestamp > 0) {
-			airspeed_blockage_scale = math::constrain(1.f - (hrt_absolute_time() - _airspeed_blocked_timestamp) /
+		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);
 		}
 
@@ -410,70 +412,49 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg)
 	mavlink_hil_gps_t hil_gps;
 	mavlink_msg_hil_gps_decode(msg, &hil_gps);
 
-	if (!_gps_blocked) {
-		sensor_gps_s gps{};
+	sensor_gps_s gps{};
 
-		if (!_gps_stuck) {
-			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;
+	gps.timestamp = hrt_absolute_time();
+	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;
+	gps.fix_type = hil_gps.fix_type;
 
-			gps.s_variance_m_s = 0.25f;
-			gps.c_variance_rad = 0.5f;
-			gps.fix_type = hil_gps.fix_type;
+	gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
+	gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
 
-			gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
-			gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
+	gps.hdop = 0; // TODO
+	gps.vdop = 0; // TODO
 
-			gps.hdop = 0; // TODO
-			gps.vdop = 0; // TODO
+	gps.noise_per_ms = 0;
+	gps.automatic_gain_control = 0;
+	gps.jamming_indicator = 0;
+	gps.jamming_state = 0;
+	gps.spoofing_state = 0;
 
-			gps.noise_per_ms = 0;
-			gps.automatic_gain_control = 0;
-			gps.jamming_indicator = 0;
-			gps.jamming_state = 0;
-			gps.spoofing_state = 0;
+	gps.vel_m_s = (float)(hil_gps.vel) / 100.0f;
+	gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f;
+	gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f;
+	gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f;
 
-			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;
 
-			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;
+	gps.timestamp_time_relative = 0;
+	gps.time_utc_usec = hil_gps.time_usec;
 
-			gps.timestamp_time_relative = 0;
-			gps.time_utc_usec = hil_gps.time_usec;
+	gps.satellites_used = hil_gps.satellites_visible;
 
-			gps.satellites_used = hil_gps.satellites_visible;
+	gps.heading = NAN;
+	gps.heading_offset = NAN;
 
-			gps.heading = NAN;
-			gps.heading_offset = NAN;
 
-			_gps_prev = gps;
-
-		} else {
-			gps = _gps_prev;
-		}
-
-		gps.timestamp = hrt_absolute_time();
+	if (_failure_injection.handle_gps_failure(gps) && _failure_injection.handle_gps_alt_failure(gps)) {
 
 		// New publishers will be created based on the HIL_GPS ID's being different or not
 		for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) {
@@ -848,7 +829,7 @@ void SimulatorMavlink::handle_message_odometry(const mavlink_message_t *msg)
 	case MAV_ESTIMATOR_TYPE_NAIVE:
 	case MAV_ESTIMATOR_TYPE_VISION:
 	case MAV_ESTIMATOR_TYPE_VIO:
-		if (!_vio_blocked) {
+		if (!_failure_injection.is_vio_blocked()) {
 			odom.timestamp = hrt_absolute_time();
 			_visual_odometry_pub.publish(odom);
 		}
@@ -1056,7 +1037,7 @@ void SimulatorMavlink::send()
 		if (fds_actuator_outputs[0].revents & POLLIN) {
 			// Got new data to read, update all topics.
 			parameters_update(false);
-			check_failure_injections();
+			_failure_injection.check_failure_injections();
 			_vehicle_status_sub.update(&_vehicle_status);
 			_battery_status_sub.update(&_battery_status);
 
@@ -1244,286 +1225,30 @@ void SimulatorMavlink::run()
 	}
 }
 
-void SimulatorMavlink::check_failure_injections()
-{
-	vehicle_command_s vehicle_command;
-
-	while (_vehicle_command_sub.update(&vehicle_command)) {
-		if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) {
-			continue;
-		}
-
-		bool handled = false;
-		bool supported = false;
-
-		const int failure_unit = static_cast<int>(vehicle_command.param1 + 0.5f);
-		const int failure_type = static_cast<int>(vehicle_command.param2 + 0.5f);
-		const int instance = static_cast<int>(vehicle_command.param3 + 0.5f);
-
-		if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GPS) {
-			handled = true;
-
-			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
-				PX4_WARN("CMD_INJECT_FAILURE, GPS off");
-				supported = true;
-				_gps_blocked = true;
-
-			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
-				PX4_INFO("CMD_INJECT_FAILURE, GPS ok");
-				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) {
-			handled = true;
-
-			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
-				supported = true;
-
-				// 0 to signal all
-				if (instance == 0) {
-					for (int i = 0; i < ACCEL_COUNT_MAX; i++) {
-						PX4_WARN("CMD_INJECT_FAILURE, accel %d off", i);
-						_accel_blocked[i] = true;
-						_accel_stuck[i] = false;
-					}
-
-				} else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) {
-					PX4_WARN("CMD_INJECT_FAILURE, accel %d off", instance - 1);
-					_accel_blocked[instance - 1] = true;
-					_accel_stuck[instance - 1] = false;
-				}
-
-			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
-				supported = true;
-
-				// 0 to signal all
-				if (instance == 0) {
-					for (int i = 0; i < ACCEL_COUNT_MAX; i++) {
-						PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", i);
-						_accel_blocked[i] = false;
-						_accel_stuck[i] = true;
-					}
-
-				} else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) {
-					PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", instance - 1);
-					_accel_blocked[instance - 1] = false;
-					_accel_stuck[instance - 1] = true;
-				}
-
-			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
-				supported = true;
-
-				// 0 to signal all
-				if (instance == 0) {
-					for (int i = 0; i < ACCEL_COUNT_MAX; i++) {
-						PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", i);
-						_accel_blocked[i] = false;
-						_accel_stuck[i] = false;
-					}
-
-				} else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) {
-					PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", instance - 1);
-					_accel_blocked[instance - 1] = false;
-					_accel_stuck[instance - 1] = false;
-				}
-			}
-
-		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GYRO) {
-			handled = true;
-
-			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
-				supported = true;
-
-				// 0 to signal all
-				if (instance == 0) {
-					for (int i = 0; i < GYRO_COUNT_MAX; i++) {
-						PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", i);
-						_gyro_blocked[i] = true;
-						_gyro_stuck[i] = false;
-					}
-
-				} else if (instance >= 1 && instance <= GYRO_COUNT_MAX) {
-					PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", instance - 1);
-					_gyro_blocked[instance - 1] = true;
-					_gyro_stuck[instance - 1] = false;
-				}
-
-			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
-				supported = true;
-
-				// 0 to signal all
-				if (instance == 0) {
-					for (int i = 0; i < GYRO_COUNT_MAX; i++) {
-						PX4_WARN("CMD_INJECT_FAILURE, gyro %d stuck", i);
-						_gyro_blocked[i] = false;
-						_gyro_stuck[i] = true;
-					}
-
-				} else if (instance >= 1 && instance <= GYRO_COUNT_MAX) {
-					PX4_INFO("CMD_INJECT_FAILURE, gyro %d stuck", instance - 1);
-					_gyro_blocked[instance - 1] = false;
-					_gyro_stuck[instance - 1] = true;
-				}
-
-			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
-				supported = true;
-
-				// 0 to signal all
-				if (instance == 0) {
-					for (int i = 0; i < GYRO_COUNT_MAX; i++) {
-						PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", i);
-						_gyro_blocked[i] = false;
-						_gyro_stuck[i] = false;
-					}
-
-				} else if (instance >= 1 && instance <= GYRO_COUNT_MAX) {
-					PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", instance - 1);
-					_gyro_blocked[instance - 1] = false;
-					_gyro_stuck[instance - 1] = false;
-				}
-			}
-
-		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_MAG) {
-			handled = true;
-
-			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
-				supported = true;
-
-				// 0 to signal all
-				if (instance == 0) {
-					for (int i = 0; i < MAG_COUNT_MAX; i++) {
-						PX4_WARN("CMD_INJECT_FAILURE, mag %d off", i);
-						_mag_blocked[i] = true;
-						_mag_stuck[i] = false;
-					}
-
-				} else if (instance >= 1 && instance <= MAG_COUNT_MAX) {
-					PX4_WARN("CMD_INJECT_FAILURE, mag %d off", instance - 1);
-					_mag_blocked[instance - 1] = true;
-					_mag_stuck[instance - 1] = false;
-				}
-
-			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
-				supported = true;
-
-				// 0 to signal all
-				if (instance == 0) {
-					for (int i = 0; i < MAG_COUNT_MAX; i++) {
-						PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", i);
-						_mag_blocked[i] = false;
-						_mag_stuck[i] = true;
-					}
-
-				} else if (instance >= 1 && instance <= MAG_COUNT_MAX) {
-					PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", instance - 1);
-					_mag_blocked[instance - 1] = false;
-					_mag_stuck[instance - 1] = true;
-				}
-
-			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
-				supported = true;
-
-				// 0 to signal all
-				if (instance == 0) {
-					for (int i = 0; i < MAG_COUNT_MAX; i++) {
-						PX4_WARN("CMD_INJECT_FAILURE, mag %d ok", i);
-						_mag_blocked[i] = false;
-						_mag_stuck[i] = false;
-					}
-
-				} else if (instance >= 1 && instance <= MAG_COUNT_MAX) {
-					PX4_WARN("CMD_INJECT_FAILURE, mag %d ok", instance - 1);
-					_mag_blocked[instance - 1] = false;
-					_mag_stuck[instance - 1] = false;
-				}
-			}
-
-		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_BARO) {
-			handled = true;
-
-			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
-				PX4_WARN("CMD_INJECT_FAILURE, baro off");
-				supported = true;
-				_baro_blocked = true;
-
-			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
-				PX4_WARN("CMD_INJECT_FAILURE, baro stuck");
-				supported = true;
-				_baro_stuck = true;
-				_baro_blocked = false;
-
-			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
-				PX4_INFO("CMD_INJECT_FAILURE, baro ok");
-				supported = true;
-				_baro_blocked = false;
-			}
-
-		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_AIRSPEED) {
-			handled = true;
-
-			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
-				PX4_WARN("CMD_INJECT_FAILURE, airspeed off");
-				supported = true;
-				_airspeed_disconnected = true;
-
-			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
-				PX4_WARN("CMD_INJECT_FAILURE, airspeed wrong (simulate pitot blockage)");
-				supported = true;
-				_airspeed_blocked_timestamp = hrt_absolute_time();
-
-			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
-				PX4_INFO("CMD_INJECT_FAILURE, airspeed ok");
-				supported = true;
-				_airspeed_disconnected = false;
-				_airspeed_blocked_timestamp = 0;
-			}
-
-		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_VIO) {
-			handled = true;
-
-			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
-				PX4_WARN("CMD_INJECT_FAILURE, vio off");
-				supported = true;
-				_vio_blocked = true;
-
-			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
-				PX4_INFO("CMD_INJECT_FAILURE, vio ok");
-				supported = true;
-				_vio_blocked = false;
-			}
-		}
-
-		if (handled) {
-			vehicle_command_ack_s ack{};
-			ack.command = vehicle_command.command;
-			ack.from_external = false;
-			ack.result = supported ?
-				     vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED :
-				     vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
-			ack.timestamp = hrt_absolute_time();
-			_command_ack_pub.publish(ack);
-		}
-	}
-}
-
 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

@@ -77,6 +77,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/rpm.h>
+#include <simulation/failure_injection/failureInjection.hpp>
 
 #include <random>
 
@@ -157,29 +158,28 @@ private:
 	}
 
 
-	void check_failure_injections();
 
 	int publish_distance_topic(const mavlink_distance_sensor_t *dist);
 
 	static SimulatorMavlink *_instance;
 
 	// simulated sensor instances
-	static constexpr uint8_t ACCEL_COUNT_MAX = 3;
-	PX4Accelerometer _px4_accel[ACCEL_COUNT_MAX] {
+	static constexpr uint8_t kAccelCountMax = 3;
+	PX4Accelerometer _px4_accel[kAccelCountMax] {
 		{1310988, ROTATION_NONE}, // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
 		{1310996, ROTATION_NONE}, // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
 		{1311004, ROTATION_NONE}, // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
 	};
 
-	static constexpr uint8_t GYRO_COUNT_MAX = 3;
-	PX4Gyroscope _px4_gyro[GYRO_COUNT_MAX] {
+	static constexpr uint8_t kGyroCountMax = 3;
+	PX4Gyroscope _px4_gyro[kGyroCountMax] {
 		{1310988, ROTATION_NONE}, // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
 		{1310996, ROTATION_NONE}, // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION
 		{1311004, ROTATION_NONE}, // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
 	};
 
-	static constexpr uint8_t MAG_COUNT_MAX = 2;
-	PX4Magnetometer _px4_mag[MAG_COUNT_MAX] {
+	static constexpr uint8_t kMagCountMax = 2;
+	PX4Magnetometer _px4_mag[kMagCountMax] {
 		{197388, ROTATION_NONE},
 		{197644, ROTATION_NONE},
 	};
@@ -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};
 
@@ -277,37 +278,19 @@ private:
 	vehicle_status_s _vehicle_status{};
 	battery_status_s _battery_status{};
 
-	bool _accel_blocked[ACCEL_COUNT_MAX] {};
-	bool _accel_stuck[ACCEL_COUNT_MAX] {};
 	sensor_accel_fifo_s _last_accel_fifo{};
-	matrix::Vector3f _last_accel[GYRO_COUNT_MAX] {};
+	matrix::Vector3f _last_accel[kGyroCountMax] {};
 
-	bool _gyro_blocked[GYRO_COUNT_MAX] {};
-	bool _gyro_stuck[GYRO_COUNT_MAX] {};
 	sensor_gyro_fifo_s _last_gyro_fifo{};
-	matrix::Vector3f _last_gyro[GYRO_COUNT_MAX] {};
+	matrix::Vector3f _last_gyro[kGyroCountMax] {};
 
-	bool _baro_blocked{false};
-	bool _baro_stuck{false};
-
-	bool _mag_blocked[MAG_COUNT_MAX] {};
-	bool _mag_stuck[MAG_COUNT_MAX] {};
-
-	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};
-	bool _vio_blocked{false};
-
-	float _last_magx[MAG_COUNT_MAX] {};
-	float _last_magy[MAG_COUNT_MAX] {};
-	float _last_magz[MAG_COUNT_MAX] {};
+	matrix::Vector3f _last_mag[kMagCountMax] {};
 
 	float _last_baro_pressure{0.0f};
 	float _last_baro_temperature{0.0f};
 
+	FailureInjection _failure_injection{};
+
 	int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)

src/modules/simulation/simulator_sih/Kconfig

@@ -2,9 +2,7 @@ menuconfig MODULES_SIMULATION_SIMULATOR_SIH
 	bool "simulator_sih"
 	default n
 	select MODULES_SIMULATION_PWM_OUT_SIM
-	select MODULES_SIMULATION_SENSOR_BARO_SIM
-	select MODULES_SIMULATION_SENSOR_GPS_SIM
-	select MODULES_SIMULATION_SENSOR_MAG_SIM
+	select MODULES_SIMULATION_SENSOR_SIM_MANAGER
 	---help---
 		Enable support for simulator_sih
 

src/modules/simulation/simulator_sih/sih.cpp

@@ -65,16 +65,11 @@ Sih::~Sih()
 
 void Sih::run()
 {
-	_px4_accel.set_temperature(T1_C);
-	_px4_gyro.set_temperature(T1_C);
-
 	init_variables();
 	parameters_updated();
 
 	const hrt_abstime task_start = hrt_absolute_time();
 	_last_run = task_start;
-	_airspeed_time = task_start;
-	_dist_snsr_time = task_start;
 	_vehicle = static_cast<VehicleType>(constrain(_sih_vtype.get(),
 					    static_cast<int32_t>(VehicleType::First),
 					    static_cast<int32_t>(VehicleType::Last)));
@@ -215,23 +210,6 @@ void Sih::sensor_step()
 
 	equations_of_motion(dt);
 
-	reconstruct_sensors_signals(now);
-
-	if ((_vehicle == VehicleType::FixedWing
-	     || _vehicle == VehicleType::TailsitterVTOL
-	     || _vehicle == VehicleType::StandardVTOL)
-	    && now - _airspeed_time >= 50_ms) {
-		_airspeed_time = now;
-		send_airspeed(now);
-	}
-
-	// distance sensor published at 50 Hz
-	if (now - _dist_snsr_time >= 20_ms
-	    && fabs(_distance_snsr_override) < 10000) {
-		_dist_snsr_time = now;
-		send_dist_snsr(now);
-	}
-
 	publish_ground_truth(now);
 
 	perf_end(_loop_perf);
@@ -277,10 +255,6 @@ void Sih::parameters_updated()
 	// guards against too small determinants
 	_Im1 = 100.0f * inv(static_cast<typeof _I>(100.0f * _I));
 
-	_distance_snsr_min = _sih_distance_snsr_min.get();
-	_distance_snsr_max = _sih_distance_snsr_max.get();
-	_distance_snsr_override = _sih_distance_snsr_override.get();
-
 	_T_TAU = _sih_thrust_tau.get();
 }
 
@@ -608,85 +582,8 @@ Dcmf Sih::computeRotEcefToNed(const LatLonAlt &lla)
 	return Dcmf(val);
 }
 
-void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_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."
-	//     In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018.
 
-	// IMU
-	const Dcmf R_E2B(_q_E.inversed());
-	Vector3f accel_noise;
-	Vector3f gyro_noise;
 
-	if (_T_B.longerThan(FLT_EPSILON)) {
-		accel_noise = noiseGauss3f(0.5f, 1.7f, 1.4f);
-		gyro_noise = noiseGauss3f(0.14f, 0.07f, 0.03f);
-
-	} else {
-		// Lower noise when not armed
-		accel_noise = noiseGauss3f(0.1f, 0.1f, 0.1f);
-		gyro_noise = noiseGauss3f(0.01f, 0.01f, 0.01f);
-	}
-
-	Vector3f specific_force_B = R_E2B * _specific_force_E;
-	Vector3f accel = specific_force_B + accel_noise;
-
-	const Vector3f earth_spin_rate_B = R_E2B * Vector3f(0.f, 0.f, CONSTANTS_EARTH_SPIN_RATE);
-	Vector3f gyro = _w_B + earth_spin_rate_B + gyro_noise;
-
-	// update IMU every iteration
-	_px4_accel.update(time_now_us, accel(0), accel(1), accel(2));
-	_px4_gyro.update(time_now_us, gyro(0), gyro(1), gyro(2));
-}
-
-void Sih::send_airspeed(const hrt_abstime &time_now_us)
-{
-	// TODO: send differential pressure instead?
-	airspeed_s airspeed{};
-	airspeed.timestamp_sample = time_now_us;
-
-	// regardless of vehicle type, body frame, etc this holds as long as wind=0
-	airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_E.norm() + generate_wgn() * 0.2f);
-	airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO);
-	airspeed.confidence = 0.7f;
-	airspeed.timestamp = hrt_absolute_time();
-	_airspeed_pub.publish(airspeed);
-}
-
-void Sih::send_dist_snsr(const hrt_abstime &time_now_us)
-{
-	device::Device::DeviceId device_id;
-	device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
-	device_id.devid_s.bus = 0;
-	device_id.devid_s.address = 0;
-	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM;
-
-	distance_sensor_s distance_sensor{};
-	// distance_sensor.timestamp_sample = time_now_us;
-	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.signal_quality = -1;
-
-	if (_distance_snsr_override >= 0.f) {
-		distance_sensor.current_distance = _distance_snsr_override;
-
-	} else {
-		distance_sensor.current_distance = -_lpos(2) / _q.dcm_z()(2);
-
-		if (distance_sensor.current_distance > _distance_snsr_max) {
-			// this is based on lightware lw20 behaviour
-			distance_sensor.current_distance = UINT16_MAX / 100.f;
-
-		}
-	}
-
-	distance_sensor.timestamp = hrt_absolute_time();
-	_distance_snsr_pub.publish(distance_sensor);
-}
 
 void Sih::publish_ground_truth(const hrt_abstime &time_now_us)
 {
@@ -763,39 +660,6 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us)
 	}
 }
 
-float Sih::generate_wgn()   // generate white Gaussian noise sample with std=1
-{
-	// algorithm 1:
-	// float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
-	// return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);
-	// algorithm 2: from BlockRandGauss.hpp
-	static float V1, V2, S;
-	static bool phase = true;
-	float X;
-
-	if (phase) {
-		do {
-			float U1 = (float)rand() / (float)RAND_MAX;
-			float U2 = (float)rand() / (float)RAND_MAX;
-			V1 = 2.0f * U1 - 1.0f;
-			V2 = 2.0f * U2 - 1.0f;
-			S = V1 * V1 + V2 * V2;
-		} while (S >= 1.0f || fabsf(S) < 1e-8f);
-
-		X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S));
-
-	} else {
-		X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S));
-	}
-
-	phase = !phase;
-	return X;
-}
-
-Vector3f Sih::noiseGauss3f(float stdx, float stdy, float stdz)
-{
-	return Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz);
-}
 
 int Sih::print_status()
 {

src/modules/simulation/simulator_sih/sih.hpp

@@ -63,8 +63,6 @@
 #include <conversion/rotation.h>    // math::radians,
 #include <lib/atmosphere/atmosphere.h>        // to get the physical constants
 #include <drivers/drv_hrt.h>        // to get the real time
-#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/geo/geo.h>
 #include <lib/lat_lon_alt/lat_lon_alt.hpp>
 #include <lib/perf/perf_counter.h>
@@ -72,9 +70,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/airspeed.h>
 #include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -110,20 +106,9 @@ public:
 	/** @see ModuleBase::run() */
 	void run() override;
 
-	static float generate_wgn();    // generate white Gaussian noise sample
-
-	// generate white Gaussian noise sample as a 3D vector with specified std
-	static matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz);
-
 private:
 	void parameters_updated();
 
-	// simulated sensors
-	PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
-	PX4Gyroscope     _px4_gyro{1310988};  // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
-	uORB::Publication<distance_sensor_s>  _distance_snsr_pub{ORB_ID(distance_sensor)};
-	uORB::Publication<airspeed_s>         _airspeed_pub{ORB_ID(airspeed)};
-
 	// groundtruth
 	uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
 	uORB::Publication<vehicle_attitude_s>         _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
@@ -135,11 +120,7 @@ private:
 
 	// hard constants
 	static constexpr uint16_t NUM_ACTUATORS_MAX = 9;
-	static constexpr float T1_C = 15.0f;                        // ground temperature in Celsius
-	static constexpr float T1_K = T1_C - atmosphere::kAbsoluteNullCelsius;   // ground temperature in Kelvin
-	static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f;    // temperature gradient in degrees per metre
 	// Aerodynamic coefficients
-	static constexpr float RHO = 1.225f; 		// air density at sea level [kg/m^3]
 	static constexpr float SPAN = 0.86f; 	// wing span [m]
 	static constexpr float MAC = 0.21f; 	// wing mean aerodynamic chord [m]
 	static constexpr float RP = 0.1f; 	// radius of the propeller [m]
@@ -156,10 +137,6 @@ private:
 	// apply the equations of motion of a rigid body and integrate one step
 	void equations_of_motion(const float dt);
 
-	// reconstruct the noisy sensor signals
-	void reconstruct_sensors_signals(const hrt_abstime &time_now_us);
-	void send_airspeed(const hrt_abstime &time_now_us);
-	void send_dist_snsr(const hrt_abstime &time_now_us);
 	void publish_ground_truth(const hrt_abstime &time_now_us);
 	void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust);
 	void generate_ts_aerodynamics();
@@ -194,8 +171,6 @@ private:
 
 	hrt_abstime _last_run{0};
 	hrt_abstime _last_actuator_output_time{0};
-	hrt_abstime _airspeed_time{0};
-	hrt_abstime _dist_snsr_time{0};
 
 	bool        _grounded{true};// whether the vehicle is on the ground
 
@@ -275,7 +250,6 @@ private:
 	matrix::Matrix3f _I;    // vehicle inertia matrix
 	matrix::Matrix3f _Im1;  // inverse of the inertia matrix
 
-	float _distance_snsr_min, _distance_snsr_max, _distance_snsr_override;
 
 	// parameters defined in sih_params.c
 	DEFINE_PARAMETERS(
@@ -297,9 +271,6 @@ private:
 		(ParamFloat<px4::params::SIH_LOC_LAT0>) _sih_lat0,
 		(ParamFloat<px4::params::SIH_LOC_LON0>) _sih_lon0,
 		(ParamFloat<px4::params::SIH_LOC_H0>) _sih_h0,
-		(ParamFloat<px4::params::SIH_DISTSNSR_MIN>) _sih_distance_snsr_min,
-		(ParamFloat<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max,
-		(ParamFloat<px4::params::SIH_DISTSNSR_OVR>) _sih_distance_snsr_override,
 		(ParamFloat<px4::params::SIH_T_TAU>) _sih_thrust_tau,
 		(ParamInt<px4::params::SIH_VEHICLE_TYPE>) _sih_vtype
 	)

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

@@ -57,6 +57,8 @@ static constexpr FailureUnit failure_units[] = {
 	{ "mag", vehicle_command_s::FAILURE_UNIT_SENSOR_MAG},
 	{ "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},
@@ -83,6 +85,7 @@ static constexpr FailureType failure_types[] = {
 	{ "slow", vehicle_command_s::FAILURE_TYPE_SLOW},
 	{ "delayed", vehicle_command_s::FAILURE_TYPE_DELAYED},
 	{ "intermittent", vehicle_command_s::FAILURE_TYPE_INTERMITTENT},
+	{ "drift", vehicle_command_s::FAILURE_TYPE_DRIFT},
 };
 
 static void print_usage()
@@ -133,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;