simulator: use failure injection to block GPS

This uses the new failure injection system using vehicle_commands
instead of specific parameters. For now it implements GPS only.

src/modules/commander/Commander.cpp

@@ -1115,6 +1115,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
 	case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW:
+	case vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 

src/modules/simulator/simulator.h

@@ -72,6 +72,8 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 
 #include <random>
 
@@ -145,7 +147,8 @@ private:
 		_instance = nullptr;
 	}
 
-	// class methods
+
+	void check_failure_injections();
 
 	int publish_flow_topic(const mavlink_hil_optical_flow_t *flow);
 	int publish_odometry_topic(const mavlink_message_t *odom_mavlink);
@@ -176,6 +179,8 @@ private:
 	uORB::Publication<vehicle_odometry_s>		_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 	uORB::Publication<vehicle_odometry_s>		_mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
 
+	uORB::PublicationQueued<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
+
 	uORB::PublicationMulti<distance_sensor_s>	*_dist_pubs[RANGE_FINDER_MAX_SENSORS] {};
 	uint8_t _dist_sensor_ids[RANGE_FINDER_MAX_SENSORS] {};
 
@@ -235,6 +240,7 @@ private:
 	actuator_outputs_s _actuator_outputs{};
 
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 
 	// hil map_ref data
 	struct map_projection_reference_s _hil_local_proj_ref {};
@@ -246,20 +252,20 @@ private:
 	float _hil_ref_alt{0.0f};
 	uint64_t _hil_ref_timestamp{0};
 
-	// uORB data containers
 	vehicle_status_s _vehicle_status{};
 
+	bool _accel_blocked{false};
+	bool _gyro_blocked{false};
+	bool _baro_blocked{false};
+	bool _mag_blocked{false};
+	bool _gps_blocked{false};
+	bool _airspeed_blocked{false};
+
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 	px4::atomic<bool> _has_initialized {false};
 #endif
 
 	DEFINE_PARAMETERS(
-		(ParamBool<px4::params::SIM_GPS_BLOCK>) _param_sim_gps_block,
-		(ParamBool<px4::params::SIM_ACCEL_BLOCK>) _param_sim_accel_block,
-		(ParamBool<px4::params::SIM_GYRO_BLOCK>) _param_sim_gyro_block,
-		(ParamBool<px4::params::SIM_BARO_BLOCK>) _param_sim_baro_block,
-		(ParamBool<px4::params::SIM_MAG_BLOCK>) _param_sim_mag_block,
-		(ParamBool<px4::params::SIM_DPRES_BLOCK>) _param_sim_dpres_block,
 		(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
 		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
 		(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id

src/modules/simulator/simulator_mavlink.cpp

@@ -234,31 +234,31 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
 	}
 
 	// accel
-	if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL && !_param_sim_accel_block.get()) {
+	if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL && !_accel_blocked) {
 		_px4_accel_0.update(time, sensors.xacc, sensors.yacc, sensors.zacc);
 		_px4_accel_1.update(time, sensors.xacc, sensors.yacc, sensors.zacc);
 	}
 
 	// gyro
-	if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO && !_param_sim_gyro_block.get()) {
+	if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO && !_gyro_blocked) {
 		_px4_gyro_0.update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
 		_px4_gyro_1.update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
 	}
 
 	// magnetometer
-	if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_param_sim_mag_block.get()) {
+	if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_mag_blocked) {
 		_px4_mag_0.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
 		_px4_mag_1.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
 	}
 
 	// baro
-	if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_param_sim_baro_block.get()) {
+	if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_baro_blocked) {
 		_px4_baro_0.update(time, sensors.abs_pressure);
 		_px4_baro_1.update(time, sensors.abs_pressure);
 	}
 
 	// differential pressure
-	if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_param_sim_dpres_block.get()) {
+	if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_airspeed_blocked) {
 		differential_pressure_s report{};
 		report.timestamp = time;
 		report.temperature = temperature;
@@ -322,7 +322,7 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
 	mavlink_hil_gps_t hil_gps;
 	mavlink_msg_hil_gps_decode(msg, &hil_gps);
 
-	if (!_param_sim_gps_block.get()) {
+	if (!_gps_blocked) {
 		vehicle_gps_position_s gps{};
 
 		gps.timestamp = hrt_absolute_time();
@@ -618,6 +618,7 @@ void Simulator::send()
 		if (fds_actuator_outputs[0].revents & POLLIN) {
 			// Got new data to read, update all topics.
 			parameters_update(false);
+			check_failure_injections();
 			_vehicle_status_sub.update(&_vehicle_status);
 			send_controls();
 			// Wait for other modules, such as logger or ekf2
@@ -924,6 +925,106 @@ int openUart(const char *uart_name, int baud)
 }
 #endif
 
+void Simulator::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) {
+				supported = true;
+				_gps_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				supported = true;
+				_gps_blocked = false;
+			}
+
+		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				supported = true;
+				_accel_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_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) {
+				supported = true;
+				_gyro_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				supported = true;
+				_gyro_blocked = 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;
+				_mag_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				supported = true;
+				_mag_blocked = false;
+			}
+
+		} else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_BARO) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				supported = true;
+				_baro_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_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) {
+				supported = true;
+				_airspeed_blocked = true;
+
+			} else if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				supported = true;
+				_airspeed_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_RESULT_ACCEPTED :
+				     vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
+			_command_ack_pub.publish(ack);
+		}
+	}
+}
+
 int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink)
 {
 	optical_flow_s flow = {};

src/modules/simulator/simulator_params.c

@@ -38,63 +38,3 @@
  *
  * @author Mohamed Abdelkader <mohamedashraf123@gmail.com>
  */
-
-/**
- * Simulator block GPS data.
- *
- * Enable to block the publication of any incoming simulation GPS data.
- *
- * @boolean
- * @group SITL
- */
-PARAM_DEFINE_INT32(SIM_GPS_BLOCK, 0);
-
-/**
- * Simulator block accelerometer data.
- *
- * Enable to block the publication of any incoming simulation accelerometer data.
- *
- * @boolean
- * @group SITL
- */
-PARAM_DEFINE_INT32(SIM_ACCEL_BLOCK, 0);
-
-/**
- * Simulator block gyroscope data.
- *
- * Enable to block the publication of any incoming simulation gyroscope data.
- *
- * @boolean
- * @group SITL
- */
-PARAM_DEFINE_INT32(SIM_GYRO_BLOCK, 0);
-
-/**
- * Simulator block magnetometer data.
- *
- * Enable to block the publication of any incoming simulation magnetometer data.
- *
- * @boolean
- * @group SITL
- */
-PARAM_DEFINE_INT32(SIM_MAG_BLOCK, 0);
-
-/**
- * Simulator block barometer data.
- *
- * Enable to block the publication of any incoming simulation barometer data.
- *
- * @boolean
- * @group SITL
- */
-PARAM_DEFINE_INT32(SIM_BARO_BLOCK, 0);
-
-/**
- * Simulator block differential pressure data.
- *
- * Enable to block the publication of any incoming simulation differential pressure data.
- *
- * @boolean
- * @group SITL
- */
-PARAM_DEFINE_INT32(SIM_DPRES_BLOCK, 0);