simulation: implement required failure hooks

src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp

@@ -86,6 +86,57 @@ float SensorBaroSim::generate_wgn()
 	return X;
 }
 
+void SensorBaroSim::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);
+
+		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;
+				_baro_stuck = false;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_STUCK) {
+				PX4_WARN("CMD_INJECT_FAILURE, baro stuck");
+				supported = true;
+				_baro_blocked = false;
+				_baro_stuck = true;
+
+			} 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;
+			}
+		}
+
+		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);
+		}
+	}
+}
+
 void SensorBaroSim::Run()
 {
 	if (should_exit()) {
@@ -105,6 +156,27 @@ void SensorBaroSim::Run()
 		updateParams();
 	}
 
+	check_failure_injections();
+
+	if (_baro_blocked) {
+		perf_end(_loop_perf);
+		return;
+	}
+
+	if (_baro_stuck) {
+		// Publish stuck (last known) values
+		sensor_baro_s sensor_baro{};
+		sensor_baro.timestamp_sample = hrt_absolute_time();
+		sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
+		sensor_baro.pressure = _last_pressure;
+		sensor_baro.temperature = _last_temperature;
+		sensor_baro.timestamp = hrt_absolute_time();
+		_sensor_baro_pub.publish(sensor_baro);
+
+		perf_end(_loop_perf);
+		return;
+	}
+
 	if (_vehicle_global_position_sub.updated()) {
 		vehicle_global_position_s gpos;
 
@@ -161,6 +233,10 @@ void SensorBaroSim::Run()
 			// calculate temperature in Celsius
 			float temperature = temperature_local - 273.0f + _sim_baro_off_t.get();
 
+			// Store values for stuck mode
+			_last_pressure = pressure;
+			_last_temperature = temperature;
+
 			// publish
 			sensor_baro_s sensor_baro{};
 			sensor_baro.timestamp_sample = gpos.timestamp;

src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp

@@ -44,6 +44,8 @@
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_global_position.h>
 
 using namespace time_literals;
@@ -67,17 +69,25 @@ public:
 
 private:
 	void Run() override;
+	void check_failure_injections();
 
 	// 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)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 
+	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+	bool _baro_blocked{false};
+	bool _baro_stuck{false};
 	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};
+	float _last_pressure{0.0f};
+	float _last_temperature{0.0f};
 
 	hrt_abstime _last_update_time{0};
 

src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp

@@ -87,6 +87,49 @@ float SensorGpsSim::generate_wgn()
 	return X;
 }
 
+void SensorGpsSim::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);
+
+		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;
+			}
+		}
+
+		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);
+		}
+	}
+}
+
 void SensorGpsSim::Run()
 {
 	if (should_exit()) {
@@ -106,6 +149,13 @@ void SensorGpsSim::Run()
 		updateParams();
 	}
 
+	check_failure_injections();
+
+	if (_gps_blocked) {
+		perf_end(_loop_perf);
+		return;
+	}
+
 	if (_vehicle_local_position_groundtruth_sub.updated() && _vehicle_global_position_groundtruth_sub.updated()) {
 
 		vehicle_local_position_s vehicle_local_position{};

src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp

@@ -43,6 +43,8 @@
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_gps.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>
 
@@ -67,6 +69,7 @@ public:
 
 private:
 	void Run() override;
+	void check_failure_injections();
 
 	// generate white Gaussian noise sample with std=1
 	static float generate_wgn();
@@ -77,8 +80,12 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _vehicle_global_position_groundtruth_sub{ORB_ID(vehicle_global_position_groundtruth)};
 	uORB::Subscription _vehicle_local_position_groundtruth_sub{ORB_ID(vehicle_local_position_groundtruth)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 
 	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
+	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+	bool _gps_blocked{false};
 
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 

src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp

@@ -87,6 +87,49 @@ float SensorMagSim::generate_wgn()
 	return X;
 }
 
+void SensorMagSim::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);
+
+		if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_MAG) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				PX4_WARN("CMD_INJECT_FAILURE, mag off");
+				supported = true;
+				_mag_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, mag ok");
+				supported = true;
+				_mag_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);
+		}
+	}
+}
+
 void SensorMagSim::Run()
 {
 	if (should_exit()) {
@@ -106,6 +149,13 @@ void SensorMagSim::Run()
 		updateParams();
 	}
 
+	check_failure_injections();
+
+	if (_mag_blocked) {
+		perf_end(_loop_perf);
+		return;
+	}
+
 	if (_vehicle_global_position_sub.updated()) {
 		vehicle_global_position_s gpos;
 

src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp

@@ -44,6 +44,8 @@
 #include <uORB/SubscriptionInterval.hpp>
 #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>
 
 using namespace time_literals;
@@ -67,6 +69,7 @@ public:
 
 private:
 	void Run() override;
+	void check_failure_injections();
 
 	// generate white Gaussian noise sample with std=1
 	static float generate_wgn();
@@ -77,9 +80,14 @@ private:
 	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)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+
+	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 3, TYPE: SIMULATION
 
+	bool _mag_blocked{false};
+
 	bool _mag_earth_available{false};
 
 	matrix::Vector3f _mag_earth_pred{};

src/modules/simulation/simulator_sih/sih.cpp

@@ -190,6 +190,91 @@ void Sih::realtime_loop()
 	px4_sem_destroy(&_data_semaphore);
 }
 
+void Sih::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);
+
+		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_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, airspeed ok");
+				supported = true;
+				_airspeed_blocked = false;
+			}
+
+		} 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;
+			}
+
+		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				PX4_WARN("CMD_INJECT_FAILURE, accel off");
+				supported = true;
+				_accel_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, accel ok");
+				supported = true;
+				_accel_blocked = false;
+			}
+
+		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_GYRO) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				PX4_WARN("CMD_INJECT_FAILURE, gyro off");
+				supported = true;
+				_gyro_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, gyro ok");
+				supported = true;
+				_gyro_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);
+		}
+	}
+}
+
 void Sih::sensor_step()
 {
 	// check for parameter updates
@@ -203,6 +288,8 @@ void Sih::sensor_step()
 		parameters_updated();
 	}
 
+	check_failure_injections();
+
 	perf_begin(_loop_perf);
 
 	const hrt_abstime now = hrt_absolute_time();
@@ -636,12 +723,21 @@ void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us)
 	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));
+	if (!_accel_blocked) {
+		_px4_accel.update(time_now_us, accel(0), accel(1), accel(2));
+	}
+
+	if (!_gyro_blocked) {
+		_px4_gyro.update(time_now_us, gyro(0), gyro(1), gyro(2));
+	}
 }
 
 void Sih::send_airspeed(const hrt_abstime &time_now_us)
 {
+	if (_airspeed_blocked) {
+		return;
+	}
+
 	// TODO: send differential pressure instead?
 	airspeed_s airspeed{};
 	airspeed.timestamp_sample = time_now_us;
@@ -656,6 +752,10 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us)
 
 void Sih::send_dist_snsr(const hrt_abstime &time_now_us)
 {
+	if (_distance_sensor_blocked) {
+		return;
+	}
+
 	device::Device::DeviceId device_id;
 	device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
 	device_id.devid_s.bus = 0;

src/modules/simulation/simulator_sih/sih.hpp

@@ -77,6 +77,8 @@
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/vehicle_angular_velocity.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>
 
@@ -117,6 +119,7 @@ public:
 
 private:
 	void parameters_updated();
+	void check_failure_injections();
 
 	// simulated sensors
 	PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
@@ -132,6 +135,14 @@ private:
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+
+	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+	bool _airspeed_blocked{false};
+	bool _distance_sensor_blocked{false};
+	bool _accel_blocked{false};
+	bool _gyro_blocked{false};
 
 	// hard constants
 	static constexpr uint16_t NUM_ACTUATORS_MAX = 9;