From 9f2f5a4b1f598901a46e4d9a792009f97ac8d56c Mon Sep 17 00:00:00 2001 From: Loki077 Date: Thu, 19 Sep 2024 11:54:48 +1000 Subject: [PATCH] AP_BattMonitor: add option minimum volt option Allows aggregate monitors like Sum and ESC to report the minimum voltage instead of the average voltage. This is useful for systems with multiple isolated batteries where the lowest voltage is the limiting factor. --- libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp | 10 ++++++++++ libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp | 2 +- libraries/AP_BattMonitor/AP_BattMonitor_Params.h | 1 + libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp | 13 ++++++++++++- 4 files changed, 24 insertions(+), 2 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp index 7b76295f3b5a2..6a3ccd587a143 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp @@ -57,6 +57,7 @@ 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; float consumed_mah_sum = 0.0; @@ -82,6 +83,9 @@ void AP_BattMonitor_ESC::read(void) } if (telem.get_voltage(i, voltage)) { + if (voltage_escs == 0 || voltage_min > voltage) { + voltage_min = voltage; + } voltage_sum += voltage; voltage_escs++; } @@ -133,6 +137,12 @@ void AP_BattMonitor_ESC::read(void) update_consumed(_state, dt_us); } + + // Check if we want to report the minimum voltage instead of the average + // (we do this after calculating consumed Wh so this doesn't affect that calculation) + if (option_is_set(AP_BattMonitor_Params::Options::Minimum_Voltage)) { + _state.voltage = voltage_min; + } } bool AP_BattMonitor_ESC::reset_remaining(float percentage) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 60cdf5ad769bc..4331fe79b4b4e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -154,7 +154,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Param: OPTIONS // @DisplayName: Battery monitor options // @Description: This sets options to change the behaviour of the battery monitor - // @Bitmask: 0:Ignore DroneCAN SoC, 1:MPPT reports input voltage and current, 2:MPPT Powered off when disarmed, 3:MPPT Powered on when armed, 4:MPPT Powered off at boot, 5:MPPT Powered on at boot, 6:Send resistance compensated voltage to GCS, 7:Allow DroneCAN InfoAux to be from a different CAN node + // @Bitmask: 0:Ignore DroneCAN SoC, 1:MPPT reports input voltage and current, 2:MPPT Powered off when disarmed, 3:MPPT Powered on when armed, 4:MPPT Powered off at boot, 5:MPPT Powered on at boot, 6:Send resistance compensated voltage to GCS, 7:Allow DroneCAN InfoAux to be from a different CAN node, 9:Report minimum voltage instead of average (ESC/SUM monitors only) // @User: Advanced AP_GROUPINFO("OPTIONS", 21, AP_BattMonitor_Params, _options, 0), #endif // HAL_BUILD_AP_PERIPH diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 8298a0c34a56c..13de0799ba1ea 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -27,6 +27,7 @@ class AP_BattMonitor_Params { GCS_Resting_Voltage = (1U<<6), // send resistance resting voltage to GCS AllowSplitAuxInfo = (1U<<7), // allow different node to provide aux info for DroneCAN InternalUseOnly = (1U<<8), // for use internally to ArduPilot, not to be (eg.) sent via MAVLink BATTERY_STATUS + Minimum_Voltage = (1U<<9), // aggregate monitors report minimum voltage rather than average }; BattMonitor_LowVoltage_Source failsafe_voltage_source(void) const { return (enum BattMonitor_LowVoltage_Source)_failsafe_voltage_source.get(); } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp index 354afaca28788..660e97106690d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp @@ -50,6 +50,7 @@ void AP_BattMonitor_Sum::read() { float voltage_sum = 0; + float voltage_min = 0; uint8_t voltage_count = 0; float current_sum = 0; uint8_t current_count = 0; @@ -73,7 +74,11 @@ AP_BattMonitor_Sum::read() if (!_mon.healthy(i)) { continue; } - voltage_sum += _mon.voltage(i); + const float voltage = _mon.voltage(i); + if (voltage_count == 0 || voltage < voltage_min) { + voltage_min = voltage; + } + voltage_sum += voltage; voltage_count++; float current; if (_mon.current_amps(current, i)) { @@ -103,6 +108,12 @@ AP_BattMonitor_Sum::read() update_consumed(_state, dt_us); + // Check if we want to report the minimum voltage instead of the average + // (we do this after calculating consumed Wh so this doesn't affect that calculation + if (option_is_set(AP_BattMonitor_Params::Options::Minimum_Voltage)) { + _state.voltage = voltage_min; + } + _has_current = (current_count > 0); _has_temperature = (temperature_count > 0); _state.healthy = (voltage_count > 0);