From 9d486b1ccde12512a581e0bb9461133fdf7681ac Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Mon, 20 Jun 2022 18:14:13 -0400 Subject: [PATCH] esc_battery: account for online ESCs when averaging voltage Signed-off-by: Claudio Micheli --- src/modules/esc_battery/EscBattery.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 798c17aa99..6a6a21f02a 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -33,6 +33,8 @@ #include "EscBattery.hpp" +#include + using namespace time_literals; EscBattery::EscBattery() : @@ -82,22 +84,22 @@ EscBattery::Run() if (_esc_status_sub.copy(&esc_status)) { - int online_bitmask = (1 << esc_status.esc_count) - 1; - - if (online_bitmask != esc_status.esc_online_flags || esc_status.esc_count == 0 || - esc_status.esc_count > esc_status_s::CONNECTED_ESC_MAX) { + if (esc_status.esc_count == 0 || esc_status.esc_count > esc_status_s::CONNECTED_ESC_MAX) { return; } + const uint8_t online_esc_count = math::countSetBits(esc_status.esc_online_flags); float average_voltage_v = 0.0f; float total_current_a = 0.0f; for (unsigned i = 0; i < esc_status.esc_count; ++i) { - average_voltage_v += esc_status.esc[i].esc_voltage; - total_current_a += esc_status.esc[i].esc_current; + if ((1 << i) & esc_status.esc_online_flags) { + average_voltage_v += esc_status.esc[i].esc_voltage; + total_current_a += esc_status.esc[i].esc_current; + } } - average_voltage_v /= esc_status.esc_count; + average_voltage_v /= online_esc_count; _battery.setConnected(true); _battery.updateVoltage(average_voltage_v);