From c89880cbcbcf1e74efb224a6cd4a59f55bd95d29 Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Thu, 30 Apr 2020 16:41:13 +0200 Subject: [PATCH 1/3] Commander: extend ESCs checks to handle failures. Signed-off-by: Claudio Micheli --- msg/vehicle_status_flags.msg | 4 +++ src/modules/commander/Commander.cpp | 43 ++++++++++++++++++++++++ src/modules/commander/Commander.hpp | 1 + src/modules/commander/commander_params.c | 3 +- 4 files changed, 50 insertions(+), 1 deletion(-) diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 851bf96b812b..0d59aec62ea7 100644 --- a/msg/vehicle_status_flags.msg +++ b/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 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 408d2dc3a076..604efcadb8e2 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -4084,6 +4084,49 @@ Commander::offboard_control_update() void Commander::esc_status_check(const esc_status_s &esc_status) { + status_flags.condition_escs_failure = false; + + for (int index = 0; index < esc_status.esc_count; index++) { + + 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; + } + } + char esc_fail_msg[50]; esc_fail_msg[0] = '\0'; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index ccc1b0341e96..6ab9ba2cb9c1 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -323,6 +323,7 @@ class Commander : public ModuleBase, 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}; 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 44d47d3b25bc..7910d8433c5e 100644 --- a/src/modules/commander/commander_params.c +++ b/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 From faffa9a8341e7ee5b27122458bbb040273e99564 Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Thu, 30 Apr 2020 16:41:44 +0200 Subject: [PATCH 2/3] preArmCheck: check for ESCs failures before arming Signed-off-by: Claudio Micheli --- .../Arming/PreFlightCheck/checks/preArmCheck.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp index d0a16fda8655..7c4a4cd40f4a 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) { From 628d53b9b56a026f55297d661f51e94fa65d5421 Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Wed, 3 Jun 2020 13:22:21 +0200 Subject: [PATCH 3/3] Commander: robustify esc_status_checks Signed-off-by: Claudio Micheli --- src/modules/commander/Commander.cpp | 109 ++++++++++++++++------------ src/modules/commander/Commander.hpp | 1 + 2 files changed, 62 insertions(+), 48 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 604efcadb8e2..15f09195d5af 100644 --- a/src/modules/commander/Commander.cpp +++ b/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,79 +4085,91 @@ Commander::offboard_control_update() void Commander::esc_status_check(const esc_status_s &esc_status) { - status_flags.condition_escs_failure = false; + if ((esc_status.esc_count > 0) && (hrt_elapsed_time(&esc_status.timestamp) < 500_ms)) { - for (int index = 0; index < esc_status.esc_count; index++) { + char esc_fail_msg[50]; + esc_fail_msg[0] = '\0'; - if (esc_status.esc[index].failures > esc_report_s::FAILURE_NONE) { - status_flags.condition_escs_failure = true; + int online_bitmask = (1 << esc_status.esc_count) - 1; - if (esc_status.esc[index].failures != _last_esc_failure[index]) { + // Check if ALL the ESCs are online + if (online_bitmask == esc_status.esc_online_flags) { - 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); - } + status_flags.condition_escs_error = false; + _last_esc_online_flags = esc_status.esc_online_flags; - 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); - } + } else if (_last_esc_online_flags == esc_status.esc_online_flags) { - 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); - } + // Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver - 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); - } + status_flags.condition_escs_error = true; - 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); - } + } else if (esc_status.esc_online_flags < _last_esc_online_flags) { - 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); - } + // Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warnings at boot - 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); + 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'; } - } - _last_esc_failure[index] = esc_status.esc[index].failures; + 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; } - } - char esc_fail_msg[50]; - esc_fail_msg[0] = '\0'; + status_flags.condition_escs_failure = false; - int online_bitmask = (1 << esc_status.esc_count) - 1; + for (int index = 0; index < esc_status.esc_count; index++) { - // 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; + if (esc_status.esc[index].failures > esc_report_s::FAILURE_NONE) { + status_flags.condition_escs_failure = true; - } else if (_last_esc_online_flags == esc_status.esc_online_flags || esc_status.esc_count == 0) { + if (esc_status.esc[index].failures != _last_esc_failure[index]) { - // Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver + 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); + } - status_flags.condition_escs_error = true; + 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); + } - } else if (esc_status.esc_online_flags < _last_esc_online_flags) { + 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); + } - // Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warnings at boot + 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); + } - 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_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; } } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 6ab9ba2cb9c1..99b6455536db 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -324,6 +324,7 @@ class Commander : public ModuleBase, public ModuleParams 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};