Skip to content

Commit

Permalink
AP_BattMonitor: add option minimum volt option
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
loki077 authored and robertlong13 committed Sep 20, 2024
1 parent fb5c337 commit 28faae5
Show file tree
Hide file tree
Showing 4 changed files with 22 additions and 1 deletion.
10 changes: 10 additions & 0 deletions libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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++;
}
Expand Down Expand Up @@ -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)
Expand Down
2 changes: 1 addition & 1 deletion libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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:Aggregate monitor reports minimum voltage instead of average.
// @User: Advanced
AP_GROUPINFO("OPTIONS", 21, AP_BattMonitor_Params, _options, 0),
#endif // HAL_BUILD_AP_PERIPH
Expand Down
1 change: 1 addition & 0 deletions libraries/AP_BattMonitor/AP_BattMonitor_Params.h
Original file line number Diff line number Diff line change
Expand Up @@ -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(); }
Expand Down
10 changes: 10 additions & 0 deletions libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -73,6 +74,9 @@ AP_BattMonitor_Sum::read()
if (!_mon.healthy(i)) {
continue;
}
if (voltage_count == 0 || _mon.voltage(i) < voltage_min) {
voltage_min = _mon.voltage(i);
}
voltage_sum += _mon.voltage(i);
voltage_count++;
float current;
Expand Down Expand Up @@ -103,6 +107,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);
Expand Down

0 comments on commit 28faae5

Please sign in to comment.