failure detector: added esc failures detection

Signed-off-by: Claudio Micheli <claudio@auterion.com>

msg/vehicle_status.msg

@@ -12,10 +12,11 @@ uint8 ARMING_STATE_MAX = 6
 
 # FailureDetector status
 uint8 FAILURE_NONE = 0
-uint8 FAILURE_ROLL = 1 	# (1 << 0)
-uint8 FAILURE_PITCH = 2	# (1 << 1)
-uint8 FAILURE_ALT = 4 	# (1 << 2)
-uint8 FAILURE_EXT = 8 	# (1 << 3)
+uint8 FAILURE_ROLL = 1 	        # (1 << 0)
+uint8 FAILURE_PITCH = 2	        # (1 << 1)
+uint8 FAILURE_ALT = 4 	        # (1 << 2)
+uint8 FAILURE_EXT = 8 	        # (1 << 3)
+uint8 FAILURE_ARM_ESC = 16      # (1 << 4)
 
 # HIL
 uint8 HIL_STATE_OFF = 0

src/modules/commander/Commander.cpp

@@ -2145,15 +2145,26 @@ Commander::run()
 		    failure_detector_updated) {
 
 			if (_failure_detector.isFailure()) {
-				if ((hrt_elapsed_time(&_time_at_takeoff) < 3_s) &&
-				    !_lockdown_triggered) {
-					// This handles the case where something fails during the early takeoff phase
 
-					armed.lockdown = true;
-					_lockdown_triggered = true;
+				if (!_have_taken_off_since_arming) {
+					arm_disarm(false, true, &mavlink_log_pub, "Failure detector");
 					_status_changed = true;
 
-					mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: lockdown");
+					if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
+						mavlink_log_critical(&mavlink_log_pub, "ESCs did not respond to arm request");
+
+					}
+
+				} else if (hrt_elapsed_time(&_time_at_takeoff) < 3_s) {
+					// This handles the case where something fails during the early takeoff phase
+					if (!_lockdown_triggered) {
+
+						armed.lockdown = true;
+						_lockdown_triggered = true;
+						_status_changed = true;
+
+						mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: lockdown");
+					}
 
 				} else if (!status_flags.circuit_breaker_flight_termination_disabled &&
 					   !_flight_termination_triggered) {

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -47,10 +47,16 @@ FailureDetector::FailureDetector(ModuleParams *parent) :
 {
 }
 
-bool FailureDetector::resetStatus()
+bool FailureDetector::resetAttitudeStatus()
 {
-	bool status_changed = _status != FAILURE_NONE;
-	_status = FAILURE_NONE;
+
+	int attitude_fields_bitmask = _status & (FAILURE_ROLL | FAILURE_PITCH | FAILURE_ALT | FAILURE_EXT);
+	bool status_changed(false);
+
+	if (attitude_fields_bitmask > FAILURE_NONE) {
+		_status &= ~attitude_fields_bitmask;
+		status_changed = true;
+	}
 
 	return status_changed;
 }
@@ -58,17 +64,26 @@ bool FailureDetector::resetStatus()
 bool
 FailureDetector::update(const vehicle_status_s &vehicle_status)
 {
+
 	bool updated(false);
 
 	if (isAttitudeStabilized(vehicle_status)) {
-		updated = updateAttitudeStatus();
+		updated |= updateAttitudeStatus();
 
 		if (_param_fd_ext_ats_en.get()) {
 			updated |= updateExternalAtsStatus();
 		}
 
 	} else {
-		updated = resetStatus();
+		updated |= resetAttitudeStatus();
+	}
+
+	if (_sub_esc_status.updated()) {
+
+		if (_param_escs_en.get()) {
+			updated |= updateEscsStatus(vehicle_status);
+		}
+
 	}
 
 	return updated;
@@ -143,9 +158,9 @@ bool
 FailureDetector::updateExternalAtsStatus()
 {
 	pwm_input_s pwm_input;
-	bool updated = _sub_pwm_input.update(&pwm_input);
+	bool updated(false);
 
-	if (updated) {
+	if (_sub_pwm_input.update(&pwm_input)) {
 
 		uint32_t pulse_width = pwm_input.pulse_width;
 		bool ats_trigger_status = (pulse_width >= (uint32_t)_param_fd_ext_ats_trig.get()) && (pulse_width < 3_ms);
@@ -161,7 +176,44 @@ FailureDetector::updateExternalAtsStatus()
 		if (_ext_ats_failure_hysteresis.get_state()) {
 			_status |= FAILURE_EXT;
 		}
+
+		updated = true;
 	}
 
 	return updated;
-}
+}
+
+bool
+FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
+{
+	hrt_abstime time_now = hrt_absolute_time();
+	bool updated(false);
+
+	if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+
+		esc_status_s esc_status{};
+		_sub_esc_status.copy(&esc_status);
+
+		int all_escs_armed = (1 << esc_status.esc_count) - 1;
+
+
+		_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
+		_esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now);
+
+		if (_esc_failure_hysteresis.get_state() && !(_status & FAILURE_ARM_ESCS)) {
+			_status |= FAILURE_ARM_ESCS;
+			updated = true;
+		}
+
+	} else {
+		// reset ESC bitfield
+		_esc_failure_hysteresis.set_state_and_update(false, time_now);
+
+		if (_status & FAILURE_ARM_ESCS) {
+			_status &= ~FAILURE_ARM_ESCS;
+			updated = true;
+		}
+	}
+
+	return updated;
+}

src/modules/commander/failure_detector/FailureDetector.hpp

@@ -54,6 +54,7 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/esc_status.h>
 #include <uORB/topics/pwm_input.h>
 
 typedef enum {
@@ -62,6 +63,7 @@ typedef enum {
 	FAILURE_PITCH = vehicle_status_s::FAILURE_PITCH,
 	FAILURE_ALT = vehicle_status_s::FAILURE_ALT,
 	FAILURE_EXT = vehicle_status_s::FAILURE_EXT,
+	FAILURE_ARM_ESCS = vehicle_status_s::FAILURE_ARM_ESC
 } failure_detector_bitmak;
 
 using uORB::SubscriptionData;
@@ -84,11 +86,14 @@ private:
 		(ParamFloat<px4::params::FD_FAIL_R_TTRI>) _param_fd_fail_r_ttri,
 		(ParamFloat<px4::params::FD_FAIL_P_TTRI>) _param_fd_fail_p_ttri,
 		(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
-		(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig
+		(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
+		(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en
+
 	)
 
 	// Subscriptions
 	uORB::Subscription _sub_vehicule_attitude{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _sub_esc_status{ORB_ID(esc_status)};
 	uORB::Subscription _sub_pwm_input{ORB_ID(pwm_input)};
 
 
@@ -97,9 +102,11 @@ private:
 	systemlib::Hysteresis _roll_failure_hysteresis{false};
 	systemlib::Hysteresis _pitch_failure_hysteresis{false};
 	systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
+	systemlib::Hysteresis _esc_failure_hysteresis{false};
 
-	bool resetStatus();
+	bool resetAttitudeStatus();
 	bool isAttitudeStabilized(const vehicle_status_s &vehicle_status);
 	bool updateAttitudeStatus();
 	bool updateExternalAtsStatus();
+	bool updateEscsStatus(const vehicle_status_s &vehicle_status);
 };

src/modules/commander/failure_detector/failure_detector_params.c

@@ -129,3 +129,15 @@ PARAM_DEFINE_INT32(FD_EXT_ATS_EN, 0);
  * @group Failure Detector
  */
 PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900);
+
+/**
+ * Enable checks on ESCs that report their arming state.
+ * If enabled, failure detector will verify that all the ESCs have successfully armed when the vehicle has transitioned to the armed state.
+ * Timeout for receiving an acknowledgement from the ESCs is 0.3s, if no feedback is received the failure detector will auto disarm the vehicle.
+ *
+ * @boolean
+ * @reboot_required true
+ *
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(FD_ESCS_EN, 1);