mavlink: handle failure injection commands

This adds handling of MAVLink failure injection commands. An
additional parameter is added as a guard to prevent triggering any
failures by accident.

msg/vehicle_command.msg

@@ -67,6 +67,7 @@ uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245		# Request storage of different param
 uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246	# Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_MISSION_START = 300			# start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|
 uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400		# Arms / Disarms a component |1 to arm, 0 to disarm|
+uint16 VEHICLE_CMD_INJECT_FAILURE = 420			# Inject artificial failure for testing purposes
 uint16 VEHICLE_CMD_START_RX_PAIR = 500			# Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|
 uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530            # Set camera capture mode (photo, video, etc.)
 uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531                # Set camera zoom
@@ -107,6 +108,31 @@ uint8 VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS = 1      # Continuous zoom up/down unt
 uint8 VEHICLE_CAMERA_ZOOM_TYPE_RANGE = 2           # Zoom value as proportion of full camera range
 uint8 VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH = 3    # Zoom to a focal length
 
+uint8 FAILURE_UNIT_SENSOR_GYRO = 0
+uint8 FAILURE_UNIT_SENSOR_ACCEL = 1
+uint8 FAILURE_UNIT_SENSOR_MAG = 2
+uint8 FAILURE_UNIT_SENSOR_BARO = 3
+uint8 FAILURE_UNIT_SENSOR_GPS = 4
+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_SYSTEM_BATTERY = 100
+uint8 FAILURE_UNIT_SYSTEM_MOTOR = 101
+uint8 FAILURE_UNIT_SYSTEM_SERVO = 102
+uint8 FAILURE_UNIT_SYSTEM_AVOIDANCE = 103
+uint8 FAILURE_UNIT_SYSTEM_RC_SIGNAL = 104
+uint8 FAILURE_UNIT_SYSTEM_MAVLINK_SIGNAL = 105
+
+uint8 FAILURE_TYPE_OK = 0
+uint8 FAILURE_TYPE_OFF = 1
+uint8 FAILURE_TYPE_STUCK = 2
+uint8 FAILURE_TYPE_GARBAGE = 3
+uint8 FAILURE_TYPE_WRONG = 4
+uint8 FAILURE_TYPE_SLOW = 5
+uint8 FAILURE_TYPE_DELAYED = 6
+uint8 FAILURE_TYPE_INTERMITTENT = 7
+
 uint8 ORB_QUEUE_LENGTH = 3
 
 float32 param1			# Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.

src/lib/systemlib/system_params.c

@@ -255,3 +255,16 @@ PARAM_DEFINE_INT32(SYS_HAS_BARO, 1);
  * @group System
  */
 PARAM_DEFINE_INT32(SYS_BL_UPDATE, 0);
+
+/**
+ * Enable failure injection
+ *
+ * If enabled allows MAVLink INJECT_FAILURE commands.
+ *
+ * WARNING: the failures can easily cause crashes and are to be used with caution!
+ *
+ * @boolean
+ *
+ * @group System
+ */
+PARAM_DEFINE_INT32(SYS_FAILURE_EN, 0);

src/modules/mavlink/mavlink_main.h

@@ -504,6 +504,8 @@ public:
 	bool forward_heartbeats_enabled() const { return _param_mav_hb_forw_en.get(); }
 	bool odometry_loopback_enabled() const { return _param_mav_odom_lp.get(); }
 
+	bool failure_injection_enabled() const { return _param_sys_failure_injection_enabled.get(); }
+
 	struct ping_statistics_s {
 		uint64_t last_ping_time;
 		uint32_t last_ping_seq;
@@ -667,7 +669,8 @@ private:
 		(ParamBool<px4::params::MAV_HB_FORW_EN>) _param_mav_hb_forw_en,
 		(ParamBool<px4::params::MAV_ODOM_LP>) _param_mav_odom_lp,
 		(ParamInt<px4::params::MAV_RADIO_TOUT>)      _param_mav_radio_timeout,
-		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl
+		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl,
+		(ParamInt<px4::params::SYS_FAILURE_EN>) _param_sys_failure_injection_enabled
 	)
 
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": tx run elapsed")};                      /**< loop performance counter */

src/modules/mavlink/mavlink_receiver.cpp

@@ -472,6 +472,16 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 
 		_actuator_controls_pubs[actuator_controls_s::GROUP_INDEX_GIMBAL].publish(actuator_controls);
 
+	} else if (cmd_mavlink.command == MAV_CMD_INJECT_FAILURE) {
+		if (_mavlink->failure_injection_enabled()) {
+			_cmd_pub.publish(vehicle_command);
+			send_ack = false;
+
+		} else {
+			result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+			send_ack = true;
+		}
+
 	} else {
 
 		send_ack = false;