dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander/preflight checks: Add monitoring to ESC failures 

Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli 2021-01-31 22:01:53 +01:00 committed by Nuno Marques
parent 1c624d20f5
commit 371fa98579
5 changed files with 89 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/vehicle_status_flags.msg
View File

@ -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

8
src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp
View File

@ -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) {

95
src/modules/commander/Commander.cpp
View File

@ -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;
}
}

2
src/modules/commander/Commander.hpp
View File

@ -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};

3
src/modules/commander/commander_params.c
View File

@ -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