diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index 239eca727d..460ed302ed 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -95,6 +95,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) // 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; + float worst_battery_remaining = 1.f; // 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(); @@ -154,6 +155,10 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) worst_warning = battery.warning; } + if (battery.remaining < worst_battery_remaining) { + worst_battery_remaining = battery.remaining; + } + if (battery.timestamp < oldest_update) { oldest_update = battery.timestamp; } @@ -223,8 +228,25 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) if (reporter.mavlink_log_pub()) { mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery level\t"); } - } + } else if (!context.isArmed() && _param_arm_battery_level_min.get() > FLT_EPSILON + && worst_battery_remaining < _param_arm_battery_level_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/HealthAndArmingChecks/checks/batteryCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp index 2d5330f5ae..81f1058d35 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp @@ -56,4 +56,7 @@ private: bool _last_armed{false}; bool _battery_connected_at_arming[battery_status_s::MAX_INSTANCES] {}; + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamFloat) _param_arm_battery_level_min + ) }; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index e5a00ebcb0..623e8994f6 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -1098,3 +1098,20 @@ PARAM_DEFINE_FLOAT(COM_POS_LOW_EPH, -1.0f); * @group Commander */ 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. + * + * A value of 0 disables the check. + * + * @unit norm + * @min 0 + * @max 0.9 + * @decimal 2 + * @increment 0.01 + * @group Commander + */ +PARAM_DEFINE_FLOAT(COM_ARM_BAT_MIN, 0.f);