diff --git a/msg/battery_status.msg b/msg/battery_status.msg index 105f42c927..23b89878e9 100644 --- a/msg/battery_status.msg +++ b/msg/battery_status.msg @@ -22,7 +22,7 @@ uint16 cycle_count # number of discharge cycles the battery has experienced uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min uint16 serial_number # serial number of the battery pack uint16 manufacture_date # manufacture date, part of serial number of the battery pack. formated as: Day + Month×32 + (Year–1980)×512 -uint16 state_of_health # state of health. FullChargeCapacity/DesignCapacity. +uint16 state_of_health # state of health. FullChargeCapacity/DesignCapacity, 0-100%. uint16 max_error # max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100% uint8 id # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed. uint16 interface_error # interface error counter @@ -31,6 +31,7 @@ float32[14] voltage_cell_v # Battery individual cell voltages, 0 if unknown float32 max_cell_voltage_delta # Max difference between individual cell voltages bool is_powering_off # Power off event imminent indication, false if unknown +bool is_required # Set if the battery is explicitly required before arming uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active @@ -38,8 +39,31 @@ uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage uint8 BATTERY_WARNING_CRITICAL = 2 # critical voltage, return / abort immediately uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely +uint8 BATTERY_STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field. +uint8 BATTERY_STATE_CHARGING = 7 # Battery is charging -uint8 warning # current battery warning +uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged +uint8 BATTERY_FAULT_SPIKES = 1 # Voltage spikes +uint8 BATTERY_FAULT_CELL_FAIL= 2 # One or more cells have failed +uint8 BATTERY_FAULT_OVER_CURRENT = 3 # Over-current +uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 # Over-temperature +uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault +uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with battery one +uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware +uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system +uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 # hardware problem +uint8 BATTERY_WARNING_OVER_TEMPERATURE = 10 # Over-temperature +uint8 BATTERY_FAULT_COUNT = 11 # Counter - keep it as last element! + +uint16 faults # Smart battery supply status/fault flags (bitmask) for health indication. +uint32 custom_faults # Bitmask indicating smart battery internal manufacturer faults, those are not user actionable. +uint8 warning # Current battery warning +uint8 mode # Battery mode. Note, the normal operation mode + +uint8 BATTERY_MODE_UNKNOWN = 0 # Battery does not support a mode, or if it does, is operational +uint8 BATTERY_MODE_AUTO_DISCHARGING = 1 # Battery is auto discharging (towards storage level) +uint8 BATTERY_MODE_HOT_SWAP = 2 # Battery in hot-swap mode +uint8 BATTERY_MODE_COUNT = 3 # Counter - keep it as last element (once we're fully migrated to events interface we can just comment this)! uint8 MAX_INSTANCES = 4 diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 43e82fc04f..931210cb85 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -244,6 +244,138 @@ "description": "[Unknown]" } } + }, + "battery_fault_reason_t": { + "type": "uint8_t", + "description": "Reason for battery fault", + "entries": { + "0": { + "name": "deep_discharge", + "description": "Battery has deep discharged" + }, + "1": { + "name": "voltage_spikes", + "description": "Battery detected voltage spikes" + }, + "2": { + "name": "cell_fail", + "description": "One or more battery cells have failed" + }, + "3": { + "name": "over_current", + "description": "Battery reported over-current" + }, + "4": { + "name": "fault_temperature", + "description": "Battery has reached a critical temperature which can result in a critical failure" + }, + "5": { + "name": "under_temperature", + "description": "Battery temperature is below operating range" + }, + "6": { + "name": "incompatible_voltage", + "description": "Vehicle voltage is not compatible with battery one" + }, + "7": { + "name": "incompatible_firmware", + "description": "Battery firmware is not compatible with current autopilot firmware" + }, + "8": { + "name": "incompatible_model", + "description": "Battery model is not supported by the system" + }, + "9": { + "name": "hardware_fault", + "description": "Battery reported an hardware problem" + }, + "10": { + "name": "over_temperature", + "description": "Battery is over-heating" + } + } + }, + "battery_mode_t": { + "type": "uint8_t", + "description": "Smart battery modes of operation", + "entries": { + "0": { + "name": "unknown", + "description": "unknown" + }, + "1": { + "name": "autodischarging", + "description": "auto discharging towards storage level" + }, + "2": { + "name": "hotswap", + "description": "hot-swap" + } + } + }, + "esc_fault_reason_t": { + "type": "uint8_t", + "description": "Bitfield for ESC failure reason", + "entries": { + "0": { + "name": "over_current", + "description": "detected over current" + }, + "1": { + "name": "over_voltage", + "description": "detected over voltage" + }, + "2": { + "name": "motor_over_temp", + "description": "Motor has reached a critical temperature" + }, + "3": { + "name": "over_rpm", + "description": "Motor RPM is exceeding the limits" + }, + "4": { + "name": "inconsistent_cmd", + "description": "received an invalid control command" + }, + "5": { + "name": "motor_stuck", + "description": "Motor stalled" + }, + "6": { + "name": "failure_generic", + "description": "detected a generic hardware failure" + }, + "7": { + "name": "motor_warn_temp", + "description": "Motor is over-heating" + }, + "8": { + "name": "esc_warn_temp", + "description": "is over-heating" + }, + "9": { + "name": "esc_over_temp", + "description": "reached a critical temperature" + } + } + }, + "suggested_action_t": { + "type": "uint8_t", + "description": "Suggested actions for the user in case of a safety critical event is triggered", + "entries": { + "0": { + "name": "none", + "description": "" + }, + "1": { + "name": "land", + "description": "Land now!" + }, + "2": { + "name": "reduce_throttle", + "description": "Reduce throttle!" + } + } } } } diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index f3e715f0e8..6602a71c7b 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/src/lib/mathlib/math/Functions.hpp @@ -237,6 +237,24 @@ constexpr int16_t negate(int16_t value) return -value; } +/* + * This function calculates the Hamming weight, i.e. counts the number of bits that are set + * in a given integer. + */ + +template +int countSetBits(T n) +{ + int count = 0; + + while (n) { + count += n & 1; + n >>= 1; + } + + return count; +} + inline bool isFinite(const float &value) { return PX4_ISFINITE(value); diff --git a/src/lib/mathlib/math/FunctionsTest.cpp b/src/lib/mathlib/math/FunctionsTest.cpp index 324f7f12fe..49fd5dee6b 100644 --- a/src/lib/mathlib/math/FunctionsTest.cpp +++ b/src/lib/mathlib/math/FunctionsTest.cpp @@ -212,3 +212,12 @@ TEST(FunctionsTest, lerp) EXPECT_FLOAT_EQ(lerp(.2f, .3f, -.1f), .19f); EXPECT_FLOAT_EQ(lerp(-.4f, .3f, 1.1f), .37f); } + +TEST(FunctionsTest, countSetBits) +{ + EXPECT_EQ(countSetBits(255), 8); + EXPECT_EQ(countSetBits(65535), 16); + EXPECT_EQ(countSetBits(0), 0); + EXPECT_EQ(countSetBits(0xffffffffu), 32); + EXPECT_EQ(countSetBits(754323), 9); +} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp index 66f3f69696..c5a134de23 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp @@ -38,21 +38,10 @@ #include #include #include +#include using namespace time_literals; -unsigned int countSetBits(unsigned int n) -{ - unsigned int count = 0; - - while (n) { - count += n & 1; - n >>= 1; - } - - return count; -} - bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, const bool prearm) { @@ -100,7 +89,7 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta } - const int power_module_count = countSetBits(system_power.brick_valid); + const int power_module_count = math::countSetBits(system_power.brick_valid); if (power_module_count < required_power_module_count) { success = false; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 828303b223..a92289a610 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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(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(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(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(fault_index)), _armed.armed ? "Land now!" : ""); + + events::send(events::ID("battery_fault"), {events::Log::Emergency, events::LogInternal::Warning}, + "Battery {1}: {2}", index + 1, static_cast(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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(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; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 2951f83270..0ec1bd576f 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -344,6 +344,11 @@ private: uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; float _battery_current{0.0f}; + uint8_t _last_connected_batteries{0}; + uint32_t _last_battery_custom_fault[battery_status_s::MAX_INSTANCES] {}; + uint16_t _last_battery_fault[battery_status_s::MAX_INSTANCES] {}; + uint8_t _last_battery_mode[battery_status_s::MAX_INSTANCES] {}; + Hysteresis _auto_disarm_landed{false}; Hysteresis _auto_disarm_killed{false}; diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 3e0bc7ef90..b7f59183d7 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -101,11 +101,35 @@ private: bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_FAILED; break; + case (battery_status_s::BATTERY_STATE_UNHEALTHY): + bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_UNHEALTHY; + break; + + case (battery_status_s::BATTERY_STATE_CHARGING): + bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_CHARGING; + break; + default: bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_UNDEFINED; break; } + switch (battery_status.mode) { + case (battery_status_s::BATTERY_MODE_AUTO_DISCHARGING): + bat_msg.mode = MAV_BATTERY_MODE_AUTO_DISCHARGING; + break; + + case (battery_status_s::BATTERY_MODE_HOT_SWAP): + bat_msg.mode = MAV_BATTERY_MODE_HOT_SWAP; + break; + + default: + bat_msg.mode = MAV_BATTERY_MODE_UNKNOWN; + break; + } + + bat_msg.fault_bitmask = battery_status.faults; + // check if temperature valid if (battery_status.connected && PX4_ISFINITE(battery_status.temperature)) { bat_msg.temperature = battery_status.temperature * 100.f;