diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp index a3b4619ca6..69f2538f84 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp @@ -35,11 +35,24 @@ #include #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) { @@ -79,6 +92,19 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta } } + int power_module_count = countSetBits(system_power.brick_valid); + int required_power_module_count; + param_get(param_find("COM_POWER_COUNT"), &required_power_module_count); + + if (power_module_count < required_power_module_count) { + success = false; + + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "Power redundancy not met: %d instead of %d", + power_module_count, required_power_module_count); + } + } + } else { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "system power unavailable"); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 44d47d3b25..fd8b4aff7a 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -912,3 +912,15 @@ PARAM_DEFINE_FLOAT(COM_KILL_DISARM, 5.0f); * @increment 1 */ PARAM_DEFINE_FLOAT(COM_CPU_MAX, 90.0f); + +/** + * Required number of redundant power modules + * + * This configures a check to verify the expected number of 5V rail power supplies are present. By default only one is expected. + * Note: CBRK_SUPPLY_CHK disables all power checks including this one. + * + * @group Commander + * @min 0 + * @max 4 + */ +PARAM_DEFINE_INT32(COM_POWER_COUNT, 1);