From f5e7b1e6a8838bbce87fac985764db586bf51932 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Tue, 7 Jun 2022 15:36:53 +0200
Subject: [PATCH] Commander: switch battery bitflied to dedicated datatype

---
 src/modules/commander/Commander.cpp | 14 ++++----------
 src/modules/commander/Commander.hpp |  3 ++-
 2 files changed, 6 insertions(+), 11 deletions(-)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 8e07e24572..d51ea0aeb2 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -3806,7 +3806,7 @@ void Commander::avoidance_check()
 
 void Commander::battery_status_check()
 {
-	int battery_required_count{0};
+	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
@@ -3831,7 +3831,7 @@ void Commander::battery_status_check()
 
 		if (_arm_state_machine.isArmed()) {
 
-			if ((_last_connected_batteries & (1 << index)) && !battery.connected) {
+			if (_last_connected_batteries[index] && !battery.connected) {
 				mavlink_log_critical(&_mavlink_log_pub, "Battery %d disconnected. Land now! \t", index + 1);
 				events::send<uint8_t>(events::ID("commander_battery_disconnected"), {events::Log::Emergency, events::LogInternal::Warning},
 						      "Battery {1} disconnected. Land now!", index + 1);
@@ -3848,13 +3848,7 @@ void Commander::battery_status_check()
 			}
 		}
 
-		if (battery.connected) {
-			_last_connected_batteries |= 1 << index;
-
-		} else {
-			_last_connected_batteries &= ~(1 << index);
-		}
-
+		_last_connected_batteries.set(index, battery.connected);
 		_last_battery_mode[index] = battery.mode;
 
 		if (battery.connected) {
@@ -3959,7 +3953,7 @@ void Commander::battery_status_check()
 		// All connected batteries are regularly being published
 		(hrt_elapsed_time(&oldest_update) < 5_s)
 		// There is at least one connected battery (in any slot)
-		&& (math::countSetBits(_last_connected_batteries) >= battery_required_count)
+		&& (_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
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index 6c631ea66a..4278c0aa01 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -41,6 +41,7 @@
 #include "state_machine_helper.h"
 #include "worker_thread.hpp"
 
+#include <containers/Bitset.hpp>
 #include <lib/controllib/blocks.hpp>
 #include <lib/hysteresis/hysteresis.h>
 #include <lib/mathlib/mathlib.h>
@@ -347,7 +348,7 @@ private:
 
 	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
 	hrt_abstime	_battery_failsafe_timestamp{0};
-	uint8_t		_last_connected_batteries{0};
+	px4::Bitset<battery_status_s::MAX_INSTANCES> _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] {};