From cfe3d793bf6fa9bcebcf7bef77433acc193e470c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 9 Aug 2022 15:08:17 +0200 Subject: [PATCH] commander: move battery handling into arming checks --- msg/vehicle_status.msg | 1 - msg/vehicle_status_flags.msg | 5 + src/modules/commander/Commander.cpp | 171 +------------- src/modules/commander/Commander.hpp | 9 - .../checks/batteryCheck.cpp | 215 +++++++++++++++++- .../checks/batteryCheck.hpp | 8 + 6 files changed, 231 insertions(+), 178 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index dc733c517e..c9507a73d1 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -118,5 +118,4 @@ bool parachute_system_healthy bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter bool avoidance_system_valid # Status of the obstacle avoidance system -bool battery_healthy # set if battery is available and not low diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 5d37b21e2b..4234917eda 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -42,3 +42,8 @@ bool rc_signal_found_once bool rc_calibration_in_progress bool vtol_transition_failure # Set to true if vtol transition failed +bool battery_low_remaining_time +bool battery_unhealthy + +uint8 battery_warning + diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 10c428ab25..c51110db78 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -532,54 +532,6 @@ 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::_max) + 1) - , "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"; - - } - - return ""; -}; - -using battery_mode_t = events::px4::enums::battery_mode_t; -static_assert(battery_status_s::BATTERY_MODE_COUNT == (static_cast(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(esc_fault_reason_t::_max) + 1) , "ESC fault flags mismatch!"); @@ -2421,9 +2373,7 @@ Commander::run() _geofence_warning_action_on = false; } - if (_battery_status_subs.updated()) { - battery_status_check(); - } + battery_status_check(); /* If in INIT state, try to proceed to STANDBY state */ if (!_vehicle_status_flags.calibration_enabled && _arm_state_machine.isInit()) { @@ -3691,112 +3641,11 @@ void Commander::data_link_check() void Commander::battery_status_check() { - size_t 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. - uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE; - // 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(); - float worst_battery_time_s{NAN}; - - 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 (_arm_state_machine.isArmed()) { - - if (_last_connected_batteries[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])) { - - mavlink_log_critical(&_mavlink_log_pub, "Battery %d is in %s mode! \t", index + 1, - battery_mode_str(static_cast(battery.mode))); - events::send(events::ID("commander_battery_mode"), {events::Log::Critical, events::LogInternal::Warning}, - "Battery {1} mode: {2}. Land now!", index + 1, static_cast(battery.mode)); - } - } - - _last_connected_batteries.set(index, battery.connected); - _last_battery_mode[index] = battery.mode; - - if (battery.connected) { - if (battery.warning > worst_warning) { - worst_warning = battery.warning; - } - - if (battery.timestamp < oldest_update) { - 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::_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(fault_index)), - _arm_state_machine.isArmed() ? "Land now!" : ""); - - events::px4::enums::suggested_action_t action = _arm_state_machine.isArmed() ? - 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 - (events::ID("commander_battery_fault"), {events::Log::Emergency, events::LogInternal::Warning}, - "Battery {1}: {2}. {3}", index + 1, static_cast(fault_index), action, battery.custom_faults); - } - } - } - } - - _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)))) { - worst_battery_time_s = battery.time_remaining_s; - } - - } - } - - rtl_time_estimate_s rtl_time_estimate{}; - // Compare estimate of RTL time to estimate of remaining flight time - if (_rtl_time_estimate_sub.copy(&rtl_time_estimate) - && (hrt_absolute_time() - rtl_time_estimate.timestamp) < 2_s - && rtl_time_estimate.valid + if (_vehicle_status_flags.battery_low_remaining_time && _arm_state_machine.isArmed() && !_vehicle_land_detected.ground_contact // not in any landing stage && !_rtl_time_actions_done - && PX4_ISFINITE(worst_battery_time_s) - && rtl_time_estimate.safe_time_estimate >= worst_battery_time_s && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { // Try to trigger RTL @@ -3819,31 +3668,21 @@ void Commander::battery_status_check() bool update_internal_battery_state = false; if (_arm_state_machine.isArmed()) { - if (worst_warning > _battery_warning) { + if (_vehicle_status_flags.battery_warning > _battery_warning) { battery_warning_level_increased_while_armed = true; update_internal_battery_state = true; } } else { - if (_battery_warning != worst_warning) { + if (_battery_warning != _vehicle_status_flags.battery_warning) { update_internal_battery_state = true; } } if (update_internal_battery_state) { - _battery_warning = worst_warning; + _battery_warning = _vehicle_status_flags.battery_warning; } - _vehicle_status.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) - && (_last_connected_batteries.count() >= battery_required_count) - // No currently-connected batteries have any warning - && (_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) { uint8_t failsafe_action = get_battery_failsafe_action(_commander_state, _battery_warning, diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 1861425963..4b68a47b02 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -41,7 +41,6 @@ #include "worker_thread.hpp" #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp" -#include #include #include #include @@ -341,12 +340,6 @@ private: uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; hrt_abstime _battery_failsafe_timestamp{0}; - px4::Bitset _last_connected_batteries; - 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}; Hysteresis _offboard_available{false}; @@ -405,7 +398,6 @@ private: uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _rtl_time_estimate_sub{ORB_ID(rtl_time_estimate)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; @@ -416,7 +408,6 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; uORB::SubscriptionMultiArray _telemetry_status_subs{ORB_ID::telemetry_status}; #if defined(BOARD_HAS_POWER_CONTROL) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index 66f8da9a6b..cbdab2191c 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -33,14 +33,202 @@ #include "batteryCheck.hpp" +#include + using namespace time_literals; +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::_max) + 1) + , "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"; + + } + + return ""; +}; + + +using battery_mode_t = events::px4::enums::battery_mode_t; +static_assert(battery_status_s::BATTERY_MODE_COUNT == (static_cast(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"; + } +} + + void BatteryChecks::checkAndReport(const Context &context, Report &reporter) { - if (!context.status().battery_healthy) { + 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. + uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE; + // 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(); + float worst_battery_time_s{NAN}; + int num_connected_batteries{0}; + 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 (context.isArmed()) { + + if (!battery.connected) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_disconnected"), + events::Log::Emergency, "Battery {1} disconnected", index + 1); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Battery %i disconnected\t", 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) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::battery, + events::ID("check_battery_mode"), + events::Log::Critical, "Battery {1} mode: {2}", index + 1, static_cast(battery.mode)); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Battery %d is in %s mode!\t", index + 1, + battery_mode_str(static_cast(battery.mode))); + } + } + } + + if (battery.connected) { + ++num_connected_batteries; + + if (battery.warning > worst_warning) { + worst_warning = battery.warning; + } + + if (battery.timestamp < oldest_update) { + oldest_update = battery.timestamp; + } + + if (battery.faults > 0) { + battery_has_fault = true; + + for (uint8_t fault_index = 0; fault_index <= static_cast(battery_fault_reason_t::_max); fault_index++) { + if (battery.faults & (1 << fault_index)) { + events::px4::enums::suggested_action_t action = context.isArmed() ? + 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 with. + * Manufacturer error code: {4} + */ + reporter.healthFailure + (NavModes::All, health_component_t::battery, events::ID("check_battery_fault"), {events::Log::Emergency, events::LogInternal::Warning}, + "Battery {1}: {2}. {3}", index + 1, static_cast(fault_index), action, battery.custom_faults); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "Battery %d: %s. %s \t", index + 1, + battery_fault_reason_str( + static_cast(fault_index)), + context.isArmed() ? "Land now!" : ""); + } + } + } + } + + 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)))) { + worst_battery_time_s = battery.time_remaining_s; + } + } + } + + if (context.isArmed()) { + if (worst_warning > reporter.failsafeFlags().battery_warning) { + reporter.failsafeFlags().battery_warning = worst_warning; + } + + } else { + reporter.failsafeFlags().battery_warning = worst_warning; + } + + if (reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE) { + bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::BATTERY_WARNING_CRITICAL; + NavModes affected_modes = critical_or_higher ? NavModes::All : NavModes::None; + events::LogLevel log_level = critical_or_higher ? events::Log::Critical : events::Log::Warning; /* EVENT */ + reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"), log_level, + "Low battery"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "Preflight Fail: low battery\t"); + } + + } + + rtlEstimateCheck(context, reporter, worst_battery_time_s); + + reporter.failsafeFlags().battery_unhealthy = + // 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 < battery_required_count + // No currently-connected batteries have any fault + || battery_has_fault; + + if (reporter.failsafeFlags().battery_unhealthy && !battery_has_fault) { // faults are reported above already + /* EVENT + * @description + * Make sure all batteries are connected. + */ reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_unhealthy"), events::Log::Error, "Battery unhealthy"); @@ -49,6 +237,29 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) } } - reporter.setIsPresent(health_component_t::battery); // assume it's present + if (num_connected_batteries > 0) { + reporter.setIsPresent(health_component_t::battery); + } + +} + +void BatteryChecks::rtlEstimateCheck(const Context &context, Report &reporter, float worst_battery_time_s) +{ + rtl_time_estimate_s rtl_time_estimate; + + // Compare estimate of RTL time to estimate of remaining flight time + reporter.failsafeFlags().battery_low_remaining_time = _rtl_time_estimate_sub.copy(&rtl_time_estimate) + && (hrt_absolute_time() - rtl_time_estimate.timestamp) < 2_s + && rtl_time_estimate.valid + && context.isArmed() + && PX4_ISFINITE(worst_battery_time_s) + && rtl_time_estimate.safe_time_estimate >= worst_battery_time_s; + + if (reporter.failsafeFlags().battery_low_remaining_time) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_rem_flight_time_low"), + events::Log::Error, "Remaining flight time low"); + } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp index 14b0924e41..c144f8f3b8 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp @@ -36,6 +36,9 @@ #include "../Common.hpp" #include +#include +#include +#include class BatteryChecks : public HealthAndArmingCheckBase { @@ -46,4 +49,9 @@ public: void checkAndReport(const Context &context, Report &reporter) override; private: + void rtlEstimateCheck(const Context &context, Report &reporter, float worst_battery_time_s); + + uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; + uORB::Subscription _rtl_time_estimate_sub{ORB_ID(rtl_time_estimate)}; + };