Extend ESC checks to handle failures

msg/vehicle_status_flags.msg

@@ -15,6 +15,7 @@ bool condition_local_altitude_valid
 bool condition_power_input_valid                # set if input power is valid
 bool condition_battery_healthy                # set if battery is available and not low
 bool condition_escs_error		      # set to true if one or more ESCs reporting esc_status are offline
+bool condition_escs_failure		      # set to true if one or more ESCs reporting esc_status has a failure
 bool circuit_breaker_engaged_power_check
 bool circuit_breaker_engaged_airspd_check
 bool circuit_breaker_engaged_enginefailure_check
@@ -35,3 +36,6 @@ bool usb_connected                                # status of the USB power supp
 
 bool avoidance_system_required					  # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
 bool avoidance_system_valid                       # Status of the obstacle avoidance system
+
+bool onboard_logging_system_required              # Set to true if logging system is required for arming via the COM_ARM_WO_OBLOG parameter
+bool onboard_logging_system_valid                 # Status of the onboard logging system

src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp

@@ -140,6 +140,14 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
 		}
 	}
 
+	if (arm_requirements.esc_check && status_flags.condition_escs_failure) {
+		if (prearm_ok) {
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs have a failure"); }
+
+			prearm_ok = false;
+		}
+	}
+
 	if (status.is_vtol) {
 
 		if (status.in_transition_mode) {

src/modules/commander/Commander.cpp

@@ -1561,9 +1561,10 @@ Commander::run()
 			}
 		}
 
-		if (_esc_status_sub.updated()) {
+		if (_esc_status_sub.updated() || _esc_status_was_updated) {
 			/* ESCs status changed */
-			esc_status_s esc_status{};
+			esc_status_s esc_status;
+			_esc_status_was_updated = true;
 
 			if (_esc_status_sub.copy(&esc_status)) {
 				esc_status_check(esc_status);
@@ -4084,36 +4085,91 @@ Commander::offboard_control_update()
 
 void Commander::esc_status_check(const esc_status_s &esc_status)
 {
-	char esc_fail_msg[50];
-	esc_fail_msg[0] = '\0';
+	if ((esc_status.esc_count > 0) && (hrt_elapsed_time(&esc_status.timestamp) < 500_ms)) {
 
-	int online_bitmask = (1 << esc_status.esc_count) - 1;
+		char esc_fail_msg[50];
+		esc_fail_msg[0] = '\0';
 
-	// Check if ALL the ESCs are online
-	if (online_bitmask == esc_status.esc_online_flags) {
-		status_flags.condition_escs_error = false;
-		_last_esc_online_flags = esc_status.esc_online_flags;
+		int online_bitmask = (1 << esc_status.esc_count) - 1;
 
-	} else if (_last_esc_online_flags == esc_status.esc_online_flags || esc_status.esc_count == 0)  {
+		// Check if ALL the ESCs are online
+		if (online_bitmask == esc_status.esc_online_flags) {
 
-		// Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver
+			status_flags.condition_escs_error = false;
+			_last_esc_online_flags = esc_status.esc_online_flags;
 
-		status_flags.condition_escs_error = true;
+		} else if (_last_esc_online_flags == esc_status.esc_online_flags)  {
+
+			// Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver
+
+			status_flags.condition_escs_error = true;
+
+		} else if (esc_status.esc_online_flags < _last_esc_online_flags) {
+
+			// Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warnings at boot
+
+			for (int index = 0; index < esc_status.esc_count; index++) {
+				if ((esc_status.esc_online_flags & (1 << index)) == 0) {
+					snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1);
+					esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
+				}
+			}
 
-	} else if (esc_status.esc_online_flags < _last_esc_online_flags) {
+			mavlink_log_critical(&mavlink_log_pub, "%soffline", esc_fail_msg);
 
-		// Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warnings at boot
+			_last_esc_online_flags = esc_status.esc_online_flags;
+			status_flags.condition_escs_error = true;
+		}
+
+		status_flags.condition_escs_failure = false;
 
 		for (int index = 0; index < esc_status.esc_count; index++) {
-			if ((esc_status.esc_online_flags & (1 << index)) == 0) {
-				snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1);
-				esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
+
+			if (esc_status.esc[index].failures > esc_report_s::FAILURE_NONE) {
+				status_flags.condition_escs_failure = true;
+
+				if (esc_status.esc[index].failures != _last_esc_failure[index]) {
+
+					if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_CURRENT_MASK) {
+						mavlink_log_critical(&mavlink_log_pub, "ESC%d: over current", index + 1);
+					}
+
+					if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_VOLTAGE_MASK) {
+						mavlink_log_critical(&mavlink_log_pub, "ESC%d: over voltage", index + 1);
+					}
+
+					if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_TEMPERATURE_MASK) {
+						mavlink_log_critical(&mavlink_log_pub, "ESC%d: over temperature", index + 1);
+					}
+
+					if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_RPM_MASK) {
+						mavlink_log_critical(&mavlink_log_pub, "ESC%d: over RPM", index + 1);
+					}
+
+					if (esc_status.esc[index].failures & esc_report_s::FAILURE_INCONSISTENT_CMD_MASK) {
+						mavlink_log_critical(&mavlink_log_pub, "ESC%d: command inconsistency", index + 1);
+					}
+
+					if (esc_status.esc[index].failures & esc_report_s::FAILURE_MOTOR_STUCK_MASK) {
+						mavlink_log_critical(&mavlink_log_pub, "ESC%d: motor stuck", index + 1);
+					}
+
+					if (esc_status.esc[index].failures & esc_report_s::FAILURE_GENERIC_MASK) {
+						mavlink_log_critical(&mavlink_log_pub, "ESC%d: generic failure - code %d", index, esc_status.esc[index].esc_state);
+					}
+
+				}
+
+				_last_esc_failure[index] = esc_status.esc[index].failures;
 			}
 		}
 
-		mavlink_log_critical(&mavlink_log_pub, "%soffline", esc_fail_msg);
+	} else {
+
+		if (!status_flags.condition_escs_error) {
+			mavlink_log_critical(&mavlink_log_pub, "ESCs telemetry timeout");
+		}
 
-		_last_esc_online_flags = esc_status.esc_online_flags;
 		status_flags.condition_escs_error = true;
 	}
 }

src/modules/commander/Commander.hpp

@@ -323,6 +323,8 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	hrt_abstime	_high_latency_datalink_lost{0};
 
 	int		_last_esc_online_flags{-1};
+	int		_last_esc_failure[esc_status_s::CONNECTED_ESC_MAX] {0};
+	bool		_esc_status_was_updated{false};
 
 	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
 	float		_battery_current{0.0f};

src/modules/commander/commander_params.c

@@ -855,8 +855,9 @@ PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
 PARAM_DEFINE_INT32(COM_FLT_PROFILE, 0);
 
 /**
- * Require all the ESCs to be detected to arm.
+ * Enable checks on ESCs that report telemetry.
  *
+ * If this parameter is set, the system will check ESC's online status and failures.
  * This param is specific for ESCs reporting status. Normal ESCs configurations are not affected by the change of this param.
  *
  * @group Commander