diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index af9ca66099..ae2da300f2 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -17,6 +17,7 @@ bool condition_home_position_valid # indicates a valid home position (a valid h 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 diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp index f725654966..bb3479502e 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp +++ b/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) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 019e11a30b..0c5d96caa4 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1957,9 +1957,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); @@ -3937,36 +3938,92 @@ 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) < 700_ms)) { + // Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that - 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) { - } else if (esc_status.esc_online_flags < _last_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 - // Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warnings at boot + _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'; + } + } + + mavlink_log_critical(&_mavlink_log_pub, "%soffline", esc_fail_msg); + + _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 + 1, 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 (_esc_status_was_updated && !_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; } } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 4124630719..b2b64769c1 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -333,6 +333,8 @@ private: 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}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 35689eaff9..7fd900931a 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -899,8 +899,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