fix(sensor_airspeed_sim): match SimulatorMavlink rounding

- wrong and off equal to simulator_mavlink implementation
     - but wrong is over timescale of 10s rather than 1s by default to
       make it harder for innovation check to flag (closer to icing)
 - stuck will just keep the last value before injecting the failure

src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp

@@ -108,9 +108,12 @@ void SensorAirspeedSim::Run()
 		updateParams();
 	}
 
-	if (_sim_failure.get() == 0) {
-		if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated()
-		    && _vehicle_attitude_sub.updated()) {
+	if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated()
+	    && _vehicle_attitude_sub.updated()) {
+
+		check_failure_injection();
+
+		if (!_airspeed_disconnected) {
 
 			vehicle_local_position_s lpos{};
 			_vehicle_local_position_sub.copy(&lpos);
@@ -136,26 +139,95 @@ void SensorAirspeedSim::Run()
 			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;
+			if (!_airspeed_stuck) {
+				// 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;
 
+				// simulate pitot blockage: ramp down differential pressure over time
+				const float blockage_fraction = 0.7f; // max blockage (fully ramped)
+				const float airspeed_blockage_rampup_time = 30_s;
+
+				float airspeed_blockage_scale = 1.f;
+
+				if (_airspeed_blocked_timestamp > 0) {
+					airspeed_blockage_scale = math::constrain(1.f - (hrt_absolute_time() - _airspeed_blocked_timestamp) /
+								  airspeed_blockage_rampup_time, 1.f - blockage_fraction, 1.f);
+				}
+
+				_last_differential_pressure_pa = diff_pressure * 100.0f * airspeed_blockage_scale; // hPa to Pa
+			}
 
 			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.differential_pressure_pa = _last_differential_pressure_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);
 }
 
+void SensorAirspeedSim::check_failure_injection()
+{
+	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>(std::lround(vehicle_command.param1));
+		const int failure_type = static_cast<int>(std::lround(vehicle_command.param2));
+
+		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_STUCK) {
+				PX4_WARN("CMD_INJECT_FAILURE, airspeed stuck");
+				supported = true;
+				_airspeed_stuck = 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_stuck = false;
+				_airspeed_blocked_timestamp = 0;
+			}
+		}
+
+		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 SensorAirspeedSim::task_spawn(int argc, char *argv[])
 {
 	SensorAirspeedSim *instance = new SensorAirspeedSim();

src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp

@@ -34,16 +34,21 @@
 #pragma once
 
 #include <lib/perf/perf_counter.h>
+#include <math.h>
+#include <mathlib/mathlib.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/differential_pressure.h>
+#include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 
@@ -74,6 +79,8 @@ public:
 
 	bool init();
 
+	void check_failure_injection();
+
 private:
 	void Run() override;
 
@@ -87,12 +94,15 @@ private:
 	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::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 
 	uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
+	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+	bool _airspeed_disconnected{false};
+	bool _airspeed_stuck{false};
+	hrt_abstime _airspeed_blocked_timestamp{0};
+	float _last_differential_pressure_pa{0.f};
 
 	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.yaml

@@ -13,14 +13,3 @@ parameters:
       reboot_required: true
       min: 0
       max: 1
-    SIM_ARSPD_FAIL:
-      description:
-        short: Dynamically simulate failure of airspeed sensor instance
-      type: enum
-      values:
-        0: Disabled
-        1: Enabled
-      default: 0
-      reboot_required: true
-      min: 0
-      max: 1