failure_detector: add failure injector class for motor telemetry

This allows to test motor failures via 'failure motor off -i 1' on a real
system.

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -42,6 +42,123 @@
 
 using namespace time_literals;
 
+void FailureInjector::update()
+{
+	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);
+		const int instance = static_cast<int>(vehicle_command.param3 + 0.5f);
+
+		if (failure_unit == vehicle_command_s::FAILURE_UNIT_SYSTEM_MOTOR) {
+			handled = true;
+
+			if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) {
+				PX4_INFO("CMD_INJECT_FAILURE, motors ok");
+				supported = false;
+
+				// 0 to signal all
+				if (instance == 0) {
+					supported = true;
+
+					for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
+						PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", i);
+						_esc_blocked &= ~(1 << i);
+						_esc_wrong &= ~(1 << i);
+					}
+
+				} else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) {
+					supported = true;
+
+					PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", instance - 1);
+					_esc_blocked &= ~(1 << (instance - 1));
+					_esc_wrong &= ~(1 << (instance - 1));
+				}
+			}
+
+			else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) {
+				PX4_WARN("CMD_INJECT_FAILURE, motors off");
+				supported = true;
+
+				// 0 to signal all
+				if (instance == 0) {
+					for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
+						PX4_INFO("CMD_INJECT_FAILURE, motor %d off", i);
+						_esc_blocked |= 1 << i;
+					}
+
+				} else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) {
+					PX4_INFO("CMD_INJECT_FAILURE, motor %d off", instance - 1);
+					_esc_blocked |= 1 << (instance - 1);
+				}
+			}
+
+			else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) {
+				PX4_INFO("CMD_INJECT_FAILURE, motors wrong");
+				supported = true;
+
+				// 0 to signal all
+				if (instance == 0) {
+					for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
+						PX4_INFO("CMD_INJECT_FAILURE, motor %d wrong", i);
+						_esc_wrong |= 1 << i;
+					}
+
+				} else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) {
+					PX4_INFO("CMD_INJECT_FAILURE, motor %d wrong", instance - 1);
+					_esc_wrong |= 1 << (instance - 1);
+				}
+			}
+		}
+
+		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;
+			ack.timestamp = hrt_absolute_time();
+			_command_ack_pub.publish(ack);
+		}
+	}
+
+}
+
+void FailureInjector::manipulateEscStatus(esc_status_s &status)
+{
+	if (_esc_blocked != 0 || _esc_wrong != 0) {
+		unsigned offline = 0;
+
+		for (int i = 0; i < status.esc_count; i++) {
+			const unsigned i_esc = status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
+
+			if (_esc_blocked & (1 << i_esc)) {
+				unsigned function = status.esc[i].actuator_function;
+				memset(&status.esc[i], 0, sizeof(status.esc[i]));
+				status.esc[i].actuator_function = function;
+				offline |= 1 << i;
+
+			} else if (_esc_wrong & (1 << i_esc)) {
+				// Create wrong rerport for this motor by scaling key values up and down
+				status.esc[i].esc_voltage *= 0.1f;
+				status.esc[i].esc_current *= 0.1f;
+				status.esc[i].esc_rpm *= 10.0f;
+			}
+		}
+
+		status.esc_online_flags &= ~offline;
+	}
+}
+
 FailureDetector::FailureDetector(ModuleParams *parent) :
 	ModuleParams(parent)
 {
@@ -49,6 +166,8 @@ FailureDetector::FailureDetector(ModuleParams *parent) :
 
 bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode)
 {
+	_failure_injector.update();
+
 	failure_detector_status_u status_prev = _status;
 
 	if (vehicle_control_mode.flag_control_attitude_enabled) {
@@ -69,6 +188,8 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 	esc_status_s esc_status;
 
 	if (_esc_status_sub.update(&esc_status)) {
+		_failure_injector.manipulateEscStatus(esc_status);
+
 		if (_param_escs_en.get()) {
 			updateEscsStatus(vehicle_status, esc_status);
 		}

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -51,10 +51,13 @@
 
 // subscriptions
 #include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/actuator_motors.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_imu_status.h>
 #include <uORB/topics/vehicle_status.h>
@@ -77,6 +80,20 @@ union failure_detector_status_u {
 
 using uORB::SubscriptionData;
 
+class FailureInjector
+{
+public:
+	void update();
+
+	void manipulateEscStatus(esc_status_s &status);
+private:
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
+
+	uint32_t _esc_blocked{};
+	uint32_t _esc_wrong{};
+};
+
 class FailureDetector : public ModuleParams
 {
 public:
@@ -121,6 +138,8 @@ private:
 	uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
 	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
 
+	FailureInjector _failure_injector;
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
 		(ParamInt<px4::params::FD_FAIL_R>) _param_fd_fail_r,