mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-01 17:20:35 +08:00
extend support for Battery status message
Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli
committed by
Beat Küng
Beat Küng
parent
bb1177d504
commit
d122513197
@@ -510,6 +510,44 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
|
||||
return "";
|
||||
};
|
||||
|
||||
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)
|
||||
, "Battery fault flags mismatch!");
|
||||
|
||||
static constexpr const char *battery_fault_reason_str(battery_fault_reason_t battery_fault_reason)
|
||||
{
|
||||
switch (battery_fault_reason) {
|
||||
case battery_fault_reason_t::deep_discharge: return "under voltage";
|
||||
|
||||
case battery_fault_reason_t::voltage_spikes: return "over voltage";
|
||||
|
||||
case battery_fault_reason_t::cell_fail: return "cell fault";
|
||||
|
||||
case battery_fault_reason_t::over_current: return "over current";
|
||||
|
||||
case battery_fault_reason_t::fault_temperature: return "critical temperature";
|
||||
|
||||
case battery_fault_reason_t::under_temperature: return "under temperature";
|
||||
|
||||
case battery_fault_reason_t::incompatible_voltage: return "voltage mismatch";
|
||||
|
||||
case battery_fault_reason_t::incompatible_firmware: return "incompatible firmware";
|
||||
|
||||
case battery_fault_reason_t::incompatible_model: return "incompatible model";
|
||||
|
||||
case battery_fault_reason_t::hardware_fault: return "hardware fault";
|
||||
|
||||
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 navigation_mode_t = events::px4::enums::navigation_mode_t;
|
||||
|
||||
static inline navigation_mode_t navigation_mode(uint8_t main_state)
|
||||
@@ -3652,6 +3690,8 @@ void Commander::avoidance_check()
|
||||
|
||||
void Commander::battery_status_check()
|
||||
{
|
||||
int battery_required_count{0};
|
||||
bool battery_has_fault = false;
|
||||
// There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest
|
||||
// option is to check if ANY of them have a warning, and specifically find which one has the most
|
||||
// urgent warning.
|
||||
@@ -3659,20 +3699,57 @@ void Commander::battery_status_check()
|
||||
// To make sure that all connected batteries are being regularly reported, we check which one has the
|
||||
// oldest timestamp.
|
||||
hrt_abstime oldest_update = hrt_absolute_time();
|
||||
size_t num_connected_batteries{0};
|
||||
float worst_battery_time_s{NAN};
|
||||
|
||||
_battery_current = 0.0f;
|
||||
|
||||
for (auto &battery_sub : _battery_status_subs) {
|
||||
int index = battery_sub.get_instance();
|
||||
battery_status_s battery;
|
||||
|
||||
if (!battery_sub.copy(&battery)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (battery.is_required) {
|
||||
battery_required_count++;
|
||||
}
|
||||
|
||||
if (_armed.armed) {
|
||||
|
||||
if ((_last_connected_batteries & (1 << index)) && !battery.connected) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Battery %d disconnected. Land now! \t", index + 1);
|
||||
events::send<uint8_t>(events::ID("commander_battery_disconnected"), {events::Log::Emergency, events::LogInternal::Warning},
|
||||
"Battery {1} disconnected. Land now!", index + 1);
|
||||
// trigger a battery failsafe action if a battery disconnects in flight
|
||||
worst_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
_last_battery_mode[index] = battery.mode;
|
||||
|
||||
if (battery.connected) {
|
||||
num_connected_batteries++;
|
||||
_last_connected_batteries |= 1 << index;
|
||||
|
||||
if (battery.warning > worst_warning) {
|
||||
worst_warning = battery.warning;
|
||||
@@ -3682,6 +3759,27 @@ void Commander::battery_status_check()
|
||||
oldest_update = battery.timestamp;
|
||||
}
|
||||
|
||||
if (battery.faults > 0) {
|
||||
// MAVLink supported faults, can be checked on the ground station
|
||||
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);
|
||||
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));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_last_battery_fault[index] = battery.faults;
|
||||
_last_battery_custom_fault[index] = battery.custom_faults;
|
||||
|
||||
if (PX4_ISFINITE(battery.time_remaining_s)
|
||||
&& (!PX4_ISFINITE(worst_battery_time_s)
|
||||
|| (PX4_ISFINITE(worst_battery_time_s) && (battery.time_remaining_s < worst_battery_time_s)))) {
|
||||
@@ -3745,9 +3843,11 @@ void Commander::battery_status_check()
|
||||
// All connected batteries are regularly being published
|
||||
(hrt_elapsed_time(&oldest_update) < 5_s)
|
||||
// There is at least one connected battery (in any slot)
|
||||
&& (num_connected_batteries > 0)
|
||||
&& (math::countSetBits(_last_connected_batteries) >= battery_required_count)
|
||||
// No currently-connected batteries have any warning
|
||||
&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE);
|
||||
&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE)
|
||||
// No currently-connected batteries have any fault
|
||||
&& (!battery_has_fault);
|
||||
|
||||
// execute battery failsafe if the state has gotten worse while we are armed
|
||||
if (battery_warning_level_increased_while_armed) {
|
||||
@@ -4063,7 +4163,7 @@ void Commander::esc_status_check()
|
||||
}
|
||||
}
|
||||
|
||||
mavlink_log_critical(&_mavlink_log_pub, "%soffline\t", esc_fail_msg);
|
||||
mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg, _armed.armed ? "Land now!" : "");
|
||||
|
||||
_last_esc_online_flags = esc_status.esc_online_flags;
|
||||
_status_flags.condition_escs_error = true;
|
||||
@@ -4073,54 +4173,57 @@ void Commander::esc_status_check()
|
||||
|
||||
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;
|
||||
_status_flags.condition_escs_failure |= esc_status.esc[index].failures > esc_report_s::FAILURE_NONE;
|
||||
|
||||
if (esc_status.esc[index].failures != _last_esc_failure[index]) {
|
||||
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_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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
if (esc_status.esc[index].failures & esc_report_s::FAILURE_OVER_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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
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 (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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
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 (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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
_last_esc_failure[index] = esc_status.esc[index].failures;
|
||||
|
||||
Reference in New Issue
Block a user