dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

esc_battery: account for online ESCs when averaging voltage 

Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli 2022-06-20 18:14:13 -04:00 committed by GitHub
parent 450fcca8b8
commit 9d486b1ccd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/esc_battery/EscBattery.cpp
View File

@ -33,6 +33,8 @@
#include "EscBattery.hpp"
#include <math.h>
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);