From 11014cae0690cb00dde2d58e14ba07f442375c9a Mon Sep 17 00:00:00 2001 From: Bob Long Date: Wed, 25 Sep 2024 12:11:37 +1000 Subject: [PATCH] AP_BattMonitor: add option minimum volt option --- .../AP_BattMonitor/AP_BattMonitor_Params.cpp | 2 +- .../AP_BattMonitor/AP_BattMonitor_Params.h | 1 + .../AP_BattMonitor/AP_BattMonitor_Sum.cpp | 31 ++++++++++++++++--- 3 files changed, 28 insertions(+), 6 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 60cdf5ad76..63d4ebe4d8 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:Sum monitor measures minimum voltage instead of average // @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 8298a0c34a..4665e0eeaa 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -27,6 +27,7 @@ public: 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), // sum monitor measures 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 354afaca28..dcc37cc278 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; @@ -57,6 +58,9 @@ AP_BattMonitor_Sum::read() float temperature_sum = 0.0; uint8_t temperature_count = 0; + float consumed_mah_sum = 0; + float consumed_wh_sum = 0; + for (uint8_t i=0; i<_mon.num_instances(); i++) { if (i == _instance) { // never include self @@ -73,7 +77,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)) { @@ -86,23 +94,36 @@ AP_BattMonitor_Sum::read() temperature_sum += temperature; temperature_count++; } + + float consumed_mah; + if (_mon.consumed_mah(consumed_mah, i)) { + consumed_mah_sum += consumed_mah; + } + + float consumed_wh; + if (_mon.consumed_wh(consumed_wh, i)) { + consumed_wh_sum += consumed_wh; + } } const uint32_t tnow_us = AP_HAL::micros(); - const uint32_t dt_us = tnow_us - _state.last_time_micros; if (voltage_count > 0) { - _state.voltage = voltage_sum / voltage_count; + if (option_is_set(AP_BattMonitor_Params::Options::Minimum_Voltage)) { + _state.voltage = voltage_min; + } else { + _state.voltage = voltage_sum / voltage_count; + } } if (current_count > 0) { _state.current_amps = current_sum; + _state.consumed_mah = consumed_mah_sum; + _state.consumed_wh = consumed_wh_sum; } if (temperature_count > 0) { _state.temperature = temperature_sum / temperature_count; _state.temperature_time = AP_HAL::millis(); } - update_consumed(_state, dt_us); - _has_current = (current_count > 0); _has_temperature = (temperature_count > 0); _state.healthy = (voltage_count > 0);