zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Commander: more changes to use events with escs and battery failures 

Signed-off-by: Claudio Micheli <claudio@auterion.com>
master
Claudio Micheli 3 years ago committed by Beat Küng
parent
d122513197
commit
8a01135a93
2 changed files with 100 additions and 75 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 21
      msg/esc_report.msg
  2. 154
      src/modules/commander/Commander.cpp

21
msg/esc_report.msg
Unescape View File

@ -8,13 +8,16 @@ uint8 esc_address # Address of current ESC (in most cases 1-8 / must be set @@ -8,13 +8,16 @@ uint8 esc_address # Address of current ESC (in most cases 1-8 / must be set
uint8 esc_state # State of ESC - depend on Vendor
uint8 failures # Bitmask to indicate the internal ESC faults
uint16 failures # Bitmask to indicate the internal ESC faults
uint8 FAILURE_NONE = 0
uint8 FAILURE_OVER_CURRENT_MASK = 1 # (1 << 0)
uint8 FAILURE_OVER_VOLTAGE_MASK = 2 # (1 << 1)
uint8 FAILURE_OVER_TEMPERATURE_MASK = 4 # (1 << 2)
uint8 FAILURE_OVER_RPM_MASK = 8 # (1 << 3)
uint8 FAILURE_INCONSISTENT_CMD_MASK = 16 # (1 << 4) Set if ESC received an inconsistent command (i.e out of boundaries)
uint8 FAILURE_MOTOR_STUCK_MASK = 32 # (1 << 5)
uint8 FAILURE_GENERIC_MASK = 64 # (1 << 6)
uint8 FAILURE_OVER_CURRENT = 0 # (1 << 0)
uint8 FAILURE_OVER_VOLTAGE = 1 # (1 << 1)
uint8 FAILURE_MOTOR_OVER_TEMPERATURE = 2 # (1 << 2)
uint8 FAILURE_OVER_RPM = 3 # (1 << 3)
uint8 FAILURE_INCONSISTENT_CMD = 4 # (1 << 4) Set if ESC received an inconsistent command (i.e out of boundaries)
uint8 FAILURE_MOTOR_STUCK = 5 # (1 << 5)
uint8 FAILURE_GENERIC = 6 # (1 << 6)
uint8 FAILURE_MOTOR_WARN_TEMPERATURE = 7 # (1 << 7)
uint8 FAILURE_WARN_ESC_TEMPERATURE = 8 # (1 << 8)
uint8 FAILURE_OVER_ESC_TEMPERATURE = 9 # (1 << 9)
uint8 ESC_FAILURE_COUNT = 10 # Counter - keep it as last element!

154
src/modules/commander/Commander.cpp
Unescape View File

@ -511,7 +511,7 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r @@ -511,7 +511,7 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
};
using battery_fault_reason_t = events::px4::enums::battery_fault_reason_t;
static_assert(battery_status_s::BATTERY_FAULT_COUNT == static_cast<uint8_t>(battery_fault_reason_t::battery_fault_count)
static_assert(battery_status_s::BATTERY_FAULT_COUNT == (static_cast<uint8_t>(battery_fault_reason_t::_max) + 1)
, "Battery fault flags mismatch!");
static constexpr const char *battery_fault_reason_str(battery_fault_reason_t battery_fault_reason)
@ -539,7 +539,50 @@ static constexpr const char *battery_fault_reason_str(battery_fault_reason_t bat @@ -539,7 +539,50 @@ static constexpr const char *battery_fault_reason_str(battery_fault_reason_t bat
case battery_fault_reason_t::over_temperature: return "near temperature limit";
case battery_fault_reason_t::battery_fault_count: return "error! battery fault count";
}
return "";
};
using battery_mode_t = events::px4::enums::battery_mode_t;
static_assert(battery_status_s::BATTERY_MODE_COUNT == (static_cast<uint8_t>(battery_mode_t::_max) + 1)
, "Battery mode flags mismatch!");
static constexpr const char *battery_mode_str(battery_mode_t battery_mode)
{
switch (battery_mode) {
case battery_mode_t::autodischarging: return "auto discharging";
case battery_mode_t::hotswap: return "hot-swap";
default: return "unknown";
}
}
using esc_fault_reason_t = events::px4::enums::esc_fault_reason_t;
static_assert(esc_report_s::ESC_FAILURE_COUNT == (static_cast<uint8_t>(esc_fault_reason_t::_max) + 1)
, "ESC fault flags mismatch!");
static constexpr const char *esc_fault_reason_str(esc_fault_reason_t esc_fault_reason)
{
switch (esc_fault_reason) {
case esc_fault_reason_t::over_current: return "over current";
case esc_fault_reason_t::over_voltage: return "over voltage";
case esc_fault_reason_t::motor_over_temp: return "motor critical temperature";
case esc_fault_reason_t::over_rpm: return "over RPM";
case esc_fault_reason_t::inconsistent_cmd: return "control failure";
case esc_fault_reason_t::motor_stuck: return "motor stall";
case esc_fault_reason_t::failure_generic: return "hardware failure";
case esc_fault_reason_t::motor_warn_temp: return "motor over temperature";
case esc_fault_reason_t::esc_warn_temp: return "over temperature";
case esc_fault_reason_t::esc_over_temp: return "critical temperature";
}
@ -3726,23 +3769,11 @@ void Commander::battery_status_check() @@ -3726,23 +3769,11 @@ void Commander::battery_status_check()
}
if ((battery.mode > 0) && (battery.mode != _last_battery_mode[index])) {
const char *mode_text = nullptr;
switch (battery.mode) {
case (battery_status_s::BATTERY_MODE_AUTO_DISCHARGING):
mode_text = "auto discharging";
break;
case (battery_status_s::BATTERY_MODE_HOT_SWAP):
mode_text = "hot swap";
break;
default:
mode_text = "unknown";
break;
}
mavlink_log_critical(&_mavlink_log_pub, "Battery %d is in %s mode!", index + 1, mode_text);
mavlink_log_critical(&_mavlink_log_pub, "Battery %d is in %s mode! \t", index + 1,
battery_mode_str(static_cast<battery_mode_t>(battery.mode)));
events::send<uint8_t, events::px4::enums::battery_mode_t>(events::ID("commander_battery_mode"), {events::Log::Critical, events::LogInternal::Warning},
"Battery {1} mode: {2}. Land now!", index + 1, static_cast<battery_mode_t>(battery.mode));
}
}
@ -3764,14 +3795,23 @@ void Commander::battery_status_check() @@ -3764,14 +3795,23 @@ void Commander::battery_status_check()
battery_has_fault = true;
if (battery.faults != _last_battery_fault[index] || battery.custom_faults != _last_battery_custom_fault[index]) {
for (uint8_t fault_index = 0; fault_index < static_cast<uint8_t>(battery_fault_reason_t::battery_fault_count);
for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(battery_fault_reason_t::_max);
fault_index++) {
if (battery.faults & (1 << fault_index)) {
mavlink_log_emergency(&_mavlink_log_pub, "Battery %d: %s. %s \t", index + 1,
battery_fault_reason_str(static_cast<battery_fault_reason_t>(fault_index)), _armed.armed ? "Land now!" : "");
events::send<uint8_t, events::px4::enums::battery_fault_reason_t>(events::ID("battery_fault"), {events::Log::Emergency, events::LogInternal::Warning},
"Battery {1}: {2}", index + 1, static_cast<battery_fault_reason_t>(fault_index));
events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land :
events::px4::enums::suggested_action_t::none;
/* EVENT
* @description
* The battery reported a failure which might be dangerous to fly.
* Manufacturer error code: {4}
*/
events::send<uint8_t, battery_fault_reason_t, events::px4::enums::suggested_action_t, uint32_t>
(events::ID("commander_battery_fault"), {events::Log::Emergency, events::LogInternal::Warning},
"Battery {1}: {2}. {3}", index + 1, static_cast<battery_fault_reason_t>(fault_index), action, battery.custom_faults);
}
}
}
@ -4159,7 +4199,10 @@ void Commander::esc_status_check() @@ -4159,7 +4199,10 @@ void Commander::esc_status_check()
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';
events::send<uint8_t>(events::ID("commander_esc_offline"), events::Log::Critical, "ESC{1} offline", index + 1);
events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land :
events::px4::enums::suggested_action_t::none;
events::send<uint8_t, events::px4::enums::suggested_action_t>(events::ID("commander_esc_offline"),
events::Log::Critical, "ESC{1} offline. {2}", index + 1, action);
}
}
@ -4173,61 +4216,40 @@ void Commander::esc_status_check() @@ -4173,61 +4216,40 @@ void Commander::esc_status_check()
for (int index = 0; index < esc_status.esc_count; index++) {
_status_flags.condition_escs_failure |= esc_status.esc[index].failures > esc_report_s::FAILURE_NONE;
_status_flags.condition_escs_failure |= esc_status.esc[index].failures > 0;
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\t", index + 1);
events::send<uint8_t>(events::ID("commander_esc_over_current"), events::Log::Critical,
"ESC{1}: 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\t", index + 1);
events::send<uint8_t>(events::ID("commander_esc_over_voltage"), events::Log::Critical,
"ESC{1}: over voltage", index + 1);
}
for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(esc_fault_reason_t::_max);
fault_index++) {
if (esc_status.esc[index].failures & (1 << fault_index)) {
if (esc_status.esc[index].failures & esc_report_s::FAILURE_WARN_ESC_TEMPERATURE_MASK) {
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over temperature\t", index + 1);
events::send<uint8_t>(events::ID("commander_motor_over_temp"), events::Log::Critical,
"ESC{1}: over temperature", index + 1);
}
esc_fault_reason_t fault_reason_index = static_cast<esc_fault_reason_t>(fault_index);
if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_ESC_TEMPERATURE_MASK) {
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over temperature\t", index + 1);
events::send<uint8_t>(events::ID("commander_esc_over_temp"), events::Log::Critical,
"ESC{1}: over temperature", index + 1);
}
const char *user_action = nullptr;
events::px4::enums::suggested_action_t action = events::px4::enums::suggested_action_t::none;
if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_RPM_MASK) {
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: over RPM\t", index + 1);
events::send<uint8_t>(events::ID("commander_esc_over_rpm"), events::Log::Critical,
"ESC{1}: over RPM", index + 1);
}
if (fault_reason_index == esc_fault_reason_t::motor_warn_temp
|| fault_reason_index == esc_fault_reason_t::esc_warn_temp) {
user_action = "Reduce throttle";
action = events::px4::enums::suggested_action_t::reduce_throttle;
if (esc_status.esc[index].failures & esc_report_s::FAILURE_INCONSISTENT_CMD_MASK) {
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: command inconsistency\t", index + 1);
events::send<uint8_t>(events::ID("commander_esc_cmd_inconsistent"), events::Log::Critical,
"ESC{1}: command inconsistency", index + 1);
}
} else {
user_action = "Land now!";
action = events::px4::enums::suggested_action_t::land;
}
if (esc_status.esc[index].failures & esc_report_s::FAILURE_MOTOR_STUCK_MASK) {
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: motor stuck\t", index + 1);
events::send<uint8_t>(events::ID("commander_esc_motor_stuck"), events::Log::Critical,
"ESC{1}: motor stuck", index + 1);
}
mavlink_log_emergency(&_mavlink_log_pub, "ESC%d: %s. %s \t", index + 1,
esc_fault_reason_str(fault_reason_index), _armed.armed ? user_action : "");
if (esc_status.esc[index].failures & esc_report_s::FAILURE_GENERIC_MASK) {
mavlink_log_critical(&_mavlink_log_pub, "ESC%d: generic failure - code %d\t", index + 1,
esc_status.esc[index].esc_state);
events::send<uint8_t, uint8_t>(events::ID("commander_esc_generic_failure"), events::Log::Critical,
"ESC{1}: generic failure (code {2})", index + 1, esc_status.esc[index].esc_state);
events::send<uint8_t, events::px4::enums::esc_fault_reason_t, events::px4::enums::suggested_action_t>
(events::ID("commander_esc_fault"), {events::Log::Emergency, events::LogInternal::Warning},
"ESC {1}: {2}. {3}", index + 1, fault_reason_index, action);
}
}
_last_esc_failure[index] = esc_status.esc[index].failures;
}
_last_esc_failure[index] = esc_status.esc[index].failures;
}
}

Write Preview
Loading…
Cancel
Save