extend support for Battery status message

Signed-off-by: Claudio Micheli <claudio@auterion.com>

src/lib/mathlib/math/Functions.hpp

@@ -237,6 +237,24 @@ constexpr int16_t negate<int16_t>(int16_t value)
 	return -value;
 }
 
+/*
+ * This function calculates the Hamming weight, i.e. counts the number of bits that are set
+ * in a given integer.
+ */
+
+template<typename T>
+int countSetBits(T n)
+{
+	int count = 0;
+
+	while (n) {
+		count += n & 1;
+		n >>= 1;
+	}
+
+	return count;
+}
+
 inline bool isFinite(const float &value)
 {
 	return PX4_ISFINITE(value);

src/lib/mathlib/math/FunctionsTest.cpp

@@ -212,3 +212,12 @@ TEST(FunctionsTest, lerp)
 	EXPECT_FLOAT_EQ(lerp(.2f, .3f, -.1f), .19f);
 	EXPECT_FLOAT_EQ(lerp(-.4f, .3f, 1.1f), .37f);
 }
+
+TEST(FunctionsTest, countSetBits)
+{
+	EXPECT_EQ(countSetBits(255), 8);
+	EXPECT_EQ(countSetBits(65535), 16);
+	EXPECT_EQ(countSetBits(0), 0);
+	EXPECT_EQ(countSetBits(0xffffffffu), 32);
+	EXPECT_EQ(countSetBits(754323), 9);
+}