From 7941310afd0c523cd0050436f8506a7139e9d17c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 7 Oct 2024 18:31:27 +0200 Subject: [PATCH] batteryCheck: allow COM_ARM_BAT_MIN to be set lower than BAT_CRIT_THR Which still allows to arm the vehicle even if it already has a critical battery level. --- .../checks/batteryCheck.cpp | 36 +++++++------------ src/modules/commander/commander_params.c | 9 +++-- 2 files changed, 16 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index 7d7d6f2a3e..339eae1a3a 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -191,17 +191,23 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) reporter.failsafeFlags().battery_warning = worst_warning; } - if (reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE - && reporter.failsafeFlags().battery_warning < battery_status_s::BATTERY_WARNING_FAILED) { + const bool battery_warning = reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE + && reporter.failsafeFlags().battery_warning < battery_status_s::BATTERY_WARNING_FAILED; + const bool configured_arm_threshold_in_use = !context.isArmed() && (_param_com_arm_bat_min.get() >= -FLT_EPSILON); + const bool below_configured_arm_threshold = (worst_battery_remaining < _param_com_arm_bat_min.get()); + + if (battery_warning || (configured_arm_threshold_in_use && below_configured_arm_threshold)) { const 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; + NavModes affected_modes = (!configured_arm_threshold_in_use && critical_or_higher) + || (configured_arm_threshold_in_use && below_configured_arm_threshold) ? NavModes::All : NavModes::None; + events::LogLevel log_level = critical_or_higher || below_configured_arm_threshold + ? events::Log::Critical : events::Log::Warning; /* EVENT * @description - * The battery state of charge of the worst battery is below the warning threshold. + * The battery state of charge of the worst battery is below the threshold. * * - * This check can be configured via BAT_LOW_THR, BAT_CRIT_THR and BAT_EMERGEN_THR parameters. + * This check can be configured via BAT_LOW_THR, BAT_CRIT_THR, BAT_EMERGEN_THR and COM_ARM_BAT_MIN parameters. * */ reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"), log_level, @@ -211,24 +217,6 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery level\t"); } - } else if (!context.isArmed() && _param_com_arm_bat_min.get() > FLT_EPSILON - && worst_battery_remaining < _param_com_arm_bat_min.get()) { - // if not armed, additionally check if the battery is below the separately configurable preflight threshold - /* EVENT - * @description - * The battery state of charge of the worst battery is below the preflight threshold. - * - * - * This check can be configured via COM_ARM_BAT_MIN parameter. - * - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_preflight_low"), - events::Log::Critical, - "Low battery"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery level\t"); - } } rtlEstimateCheck(context, reporter, worst_battery_time_s); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 8c94771930..da245544ec 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -960,19 +960,18 @@ PARAM_DEFINE_INT32(COM_ARMABLE, 1); /** * Minimum battery level for arming * - * Additional battery level check that only allows arming if the state of charge of the emptiest - * connected battery is above this value. + * Threshold for battery percentage below arming is prohibited. * - * A value of 0 disables the check. + * A negative value means BAT_CRIT_THR is the threshold. * * @unit norm - * @min 0 + * @min -1 * @max 0.9 * @decimal 2 * @increment 0.01 * @group Commander */ -PARAM_DEFINE_FLOAT(COM_ARM_BAT_MIN, 0.f); +PARAM_DEFINE_FLOAT(COM_ARM_BAT_MIN, -1.f); /** * Enable throw-start