From ad09da07ee4b4be061a3e1dfee831a0f00110306 Mon Sep 17 00:00:00 2001 From: Lokesh-Ramina Date: Wed, 6 Mar 2024 17:34:35 -0800 Subject: [PATCH] AP_Battmonitor: add ESC minimum voltage option This commit adds a feature where AP_BATT_MONITOR_OPTION 7 will set ESC voltage reporting to minimum voltage on ESC rather than sum of voltage. SW-97 --- .../AP_BattMonitor/AP_BattMonitor_ESC.cpp | 21 ++++++++++++++++--- .../AP_BattMonitor/AP_BattMonitor_Params.h | 1 + 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp index 713561ecb9..86e053f3b9 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp @@ -32,11 +32,15 @@ void AP_BattMonitor_ESC::read(void) uint8_t voltage_escs = 0; // number of ESCs with valid voltage uint8_t temperature_escs = 0; // number of ESCs with valid temperature float voltage_sum = 0; + float voltage_min = 0; float current_sum = 0; float temperature_sum = 0; uint32_t highest_ms = 0; _state.consumed_mah = delta_mah; + const uint32_t options = uint32_t(_params._options.get()); + const bool esc_min_volt_enabled = (options & uint32_t(AP_BattMonitor_Params::Options::ESC_Minimum_Voltage)) != 0; + for (uint8_t i=0; i voltage) { + voltage_min = voltage; + } + } + else { + voltage_sum += voltage; + voltage_escs++; + } } if (telem.get_current(i, current)) { @@ -69,7 +80,11 @@ void AP_BattMonitor_ESC::read(void) } if (voltage_escs > 0) { - _state.voltage = voltage_sum / voltage_escs; + if (esc_min_volt_enabled) { + _state.voltage = voltage_min; + } else { + _state.voltage = voltage_sum / voltage_escs; + } _state.healthy = true; } else { _state.voltage = 0; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 32ae410223..136641615b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -25,6 +25,7 @@ class AP_BattMonitor_Params { MPPT_Power_Off_At_Boot = (1U<<4), // MPPT Disabled at startup (aka boot), if HW supports it MPPT_Power_On_At_Boot = (1U<<5), // MPPT Enabled at startup (aka boot), if HW supports it. If Power_Off_at_Boot is also set, the behavior is Power_Off_at_Boot GCS_Resting_Voltage = (1U<<6), // send resistance resting voltage to GCS + ESC_Minimum_Voltage = (1U<<7), // send minimum Voltage of ESC rather than average }; BattMonitor_LowVoltage_Source failsafe_voltage_source(void) const { return (enum BattMonitor_LowVoltage_Source)_failsafe_voltage_source.get(); }