new sensor failures

airspeed, baro, and mag off.

agp has no failures, and other failure types like stuck etc not
implemented at all.

doubting this implementation, lots of repetitions and
almost-repetitions. maybe we should do it more similar to
SimulatorMavlink.cpp? Where simulator_sih handles the sensor failures
itself, with one big if mess, but the rest of it nice and lean.

failing tests for quadx:
 - 'Continue on baro stuck during mission (baro height mode)': failed
 - 'Continue on baro stuck during mission (GPS height mode)': failed
 - 'Fly straight Multicopter Mission': failed
 - 'Offboard takeoff and land': failed
 - 'Offboard position control': failed

src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp

@@ -154,6 +154,50 @@ void SensorAirspeedSim::Run()
 	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>(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;
+				_arsp_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, Airspeed ok");
+				supported = true;
+				_arsp_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 SensorAirspeedSim::task_spawn(int argc, char *argv[])
 {
 	SensorAirspeedSim *instance = new SensorAirspeedSim();

src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp

@@ -46,6 +46,8 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 
 using namespace time_literals;
 
@@ -72,6 +74,8 @@ public:
 
 	bool init();
 
+	void check_failure_injection();
+
 private:
 	void Run() override;
 
@@ -85,8 +89,13 @@ 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 _arsp_blocked{false};
 
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 

src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp

@@ -106,9 +106,12 @@ void SensorBaroSim::Run()
 	}
 
 	if (_vehicle_global_position_sub.updated()) {
+
+		check_failure_injection();
+
 		vehicle_global_position_s gpos;
 
-		if (_vehicle_global_position_sub.copy(&gpos)) {
+		if (_vehicle_global_position_sub.copy(&gpos) && !_baro_blocked) {
 
 			const float dt = math::constrain((gpos.timestamp - _last_update_time) * 1e-6f, 0.001f, 0.1f);
 
@@ -178,6 +181,50 @@ void SensorBaroSim::Run()
 	perf_end(_loop_perf);
 }
 
+
+void SensorBaroSim::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>(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;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, BARO ok");
+				supported = true;
+				_baro_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 SensorBaroSim::task_spawn(int argc, char *argv[])
 {
 	SensorBaroSim *instance = new SensorBaroSim();

src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp

@@ -45,6 +45,8 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 
 using namespace time_literals;
 
@@ -65,6 +67,8 @@ public:
 
 	bool init();
 
+	void check_failure_injection();
+
 private:
 	void Run() override;
 
@@ -73,15 +77,19 @@ private:
 
 	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)};
+
 
 	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};
+	bool _baro_blocked{false};
 
 	hrt_abstime _last_update_time{0};
 
 	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
+	uORB::Publication<vehicle_command_ack_s>        _command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 

src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp

@@ -107,19 +107,24 @@ void SensorMagSim::Run()
 	}
 
 	if (_vehicle_global_position_sub.updated()) {
-		vehicle_global_position_s gpos;
 
-		if (_vehicle_global_position_sub.copy(&gpos)) {
-			if (gpos.eph < 1000) {
+		check_failure_injection();
 
-				// 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);
+		if (!_mag_blocked) {
+			vehicle_global_position_s gpos;
 
-				_mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0);
+			if (_vehicle_global_position_sub.copy(&gpos)) {
+				if (gpos.eph < 1000) {
 
-				_mag_earth_available = true;
+					// 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;
+				}
 			}
 		}
 	}
@@ -142,6 +147,51 @@ void SensorMagSim::Run()
 	perf_end(_loop_perf);
 }
 
+
+void SensorMagSim::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>(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);
+		}
+	}
+}
+
+
 int SensorMagSim::task_spawn(int argc, char *argv[])
 {
 	SensorMagSim *instance = new SensorMagSim();

src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp

@@ -45,6 +45,8 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 
 using namespace time_literals;
 
@@ -65,6 +67,8 @@ public:
 
 	bool init();
 
+	void check_failure_injection();
+
 private:
 	void Run() override;
 
@@ -77,10 +81,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)};
+
+	PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+	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_earth_available{false};
+	bool _mag_blocked{false};
 
 	matrix::Vector3f _mag_earth_pred{};
 

src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

@@ -1246,6 +1246,8 @@ void SimulatorMavlink::run()
 
 void SimulatorMavlink::check_failure_injections()
 {
+	PX4_INFO("Entering SimulatorMavlink::check_failure_injections");
+
 	vehicle_command_s vehicle_command;
 
 	while (_vehicle_command_sub.update(&vehicle_command)) {