diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index ea53aa6a3d..c670960287 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3711,69 +3711,102 @@ void Commander::data_link_check() void Commander::battery_status_check() { - /* update battery status */ - if (_battery_sub.updated()) { - battery_status_s battery{}; - - if (_battery_sub.copy(&battery)) { + bool battery_sub_updated = false; + battery_status_s batteries[ORB_MULTI_MAX_INSTANCES]; + size_t num_connected_batteries = 0; - bool battery_warning_level_increased_while_armed = false; - bool update_internal_battery_state = false; - if (armed.armed) { - if (battery.warning > _battery_warning) { - battery_warning_level_increased_while_armed = true; - update_internal_battery_state = true; - } + for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + if (_battery_subs[i].updated() && _battery_subs[i].copy(&batteries[num_connected_batteries])) { + // We need to update the status flag if ANY battery is updated, because the system source might have + // changed, or might be nothing (if there is no battery connected) + battery_sub_updated = true; - } else { - if (_battery_warning != battery.warning) { - update_internal_battery_state = true; - } + if (batteries[num_connected_batteries].connected) { + num_connected_batteries++; } + } + } + + /* update battery status */ + if (battery_sub_updated) { + + // 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. + uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE; + // Sum the total current of all connected batteries. This is used to detect engine failure. + float total_current = 0; + // 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(); - if (update_internal_battery_state) { - _battery_warning = battery.warning; + // Only iterate over connected batteries. We don't care if a disconnected battery is not regularly publishing. + for (size_t i = 0; i < num_connected_batteries; i++) { + if (batteries[i].warning > worst_warning) { + worst_warning = batteries[i].warning; } + if (hrt_elapsed_time(&batteries[i].timestamp) > hrt_elapsed_time(&oldest_update)) { + oldest_update = batteries[i].timestamp; + } - if ((hrt_elapsed_time(&battery.timestamp) < 5_s) - && battery.connected - && (_battery_warning == battery_status_s::BATTERY_WARNING_NONE)) { + total_current += batteries[i].current_filtered_a; + } - status_flags.condition_battery_healthy = true; + bool battery_warning_level_increased_while_armed = false; + bool update_internal_battery_state = false; - } else { - status_flags.condition_battery_healthy = false; + if (armed.armed) { + if (worst_warning > _battery_warning) { + battery_warning_level_increased_while_armed = true; + update_internal_battery_state = true; } - // execute battery failsafe if the state has gotten worse while we are armed - if (battery_warning_level_increased_while_armed) { - battery_failsafe(&mavlink_log_pub, status, status_flags, &_internal_state, battery.warning, - (low_battery_action_t)_param_com_low_bat_act.get()); + } else { + if (_battery_warning != worst_warning) { + update_internal_battery_state = true; } + } - // Handle shutdown request from emergency battery action - if (update_internal_battery_state) { + if (update_internal_battery_state) { + _battery_warning = worst_warning; + } - if ((_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) && shutdown_if_allowed()) { - mavlink_log_critical(&mavlink_log_pub, "Dangerously low battery! Shutting system down"); - px4_usleep(200000); + status_flags.condition_battery_healthy = + // 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 + // No currently-connected batteries have any warning + && (_battery_warning == battery_status_s::BATTERY_WARNING_NONE); - int ret_val = px4_shutdown_request(false, false); + // execute battery failsafe if the state has gotten worse while we are armed + if (battery_warning_level_increased_while_armed) { + battery_failsafe(&mavlink_log_pub, status, status_flags, &_internal_state, _battery_warning, + (low_battery_action_t)_param_com_low_bat_act.get()); + } - if (ret_val) { - mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown"); + // Handle shutdown request from emergency battery action + if (update_internal_battery_state) { - } else { - while (1) { px4_usleep(1); } - } + if ((_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) && shutdown_if_allowed()) { + mavlink_log_critical(&mavlink_log_pub, "Dangerously low battery! Shutting system down"); + px4_usleep(200000); + + int ret_val = px4_shutdown_request(false, false); + + if (ret_val) { + mavlink_log_critical(&mavlink_log_pub, "System does not support shutdown"); + + } else { + while (1) { px4_usleep(1); } } } - - _battery_current = battery.current_filtered_a; } + + _battery_current = total_current; } } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index d037fcbf5b..684ee1ccf4 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -43,6 +43,7 @@ #include #include #include +#include // publications #include @@ -374,7 +375,12 @@ private: // Subscriptions uORB::Subscription _actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS}; - uORB::Subscription _battery_sub{ORB_ID(battery_status)}; + uORB::Subscription _battery_subs[ORB_MULTI_MAX_INSTANCES] { + uORB::Subscription(ORB_ID(battery_status), 0), + uORB::Subscription(ORB_ID(battery_status), 1), + uORB::Subscription(ORB_ID(battery_status), 2), + uORB::Subscription(ORB_ID(battery_status), 3), + }; uORB::Subscription _cmd_sub{ORB_ID(vehicle_command)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};