Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add and use Battery backend internal-use-only flag #27778

Merged
merged 3 commits into from
Aug 7, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 26 additions & 0 deletions Tools/autotest/arducopter.py
Original file line number Diff line number Diff line change
Expand Up @@ -11704,6 +11704,31 @@ def MissionRTLYawBehaviour(self):
if abs(new_heading - original_heading) > 5:
raise NotAchievedException(f"Should return to original heading want={original_heading} got={new_heading}")

def BatteryInternalUseOnly(self):
'''batteries marked as internal use only should not appear over mavlink'''
self.set_parameters({
"BATT_MONITOR": 4, # 4 is analog volt+curr
"BATT2_MONITOR": 4,
})
self.reboot_sitl()
self.wait_message_field_values('BATTERY_STATUS', {
"id": 0,
})
self.wait_message_field_values('BATTERY_STATUS', {
"id": 1,
})
self.progress("Making battery private")
self.set_parameters({
"BATT_OPTIONS": 256,
})
self.wait_message_field_values('BATTERY_STATUS', {
"id": 1,
})
for i in range(10):
self.assert_received_message_field_values('BATTERY_STATUS', {
"id": 1
})

def tests2b(self): # this block currently around 9.5mins here
'''return list of all tests'''
ret = ([
Expand Down Expand Up @@ -11802,6 +11827,7 @@ def tests2b(self): # this block currently around 9.5mins here
self.REQUIRE_POSITION_FOR_ARMING,
self.LoggingFormat,
self.MissionRTLYawBehaviour,
self.BatteryInternalUseOnly,
])
return ret

Expand Down
8 changes: 8 additions & 0 deletions libraries/AP_BattMonitor/AP_BattMonitor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -777,6 +777,14 @@ float AP_BattMonitor::gcs_voltage(uint8_t instance) const
return state[instance].voltage;
}

bool AP_BattMonitor::option_is_set(uint8_t instance, AP_BattMonitor_Params::Options option) const
{
if (instance >= _num_instances || drivers[instance] == nullptr) {
return false;
}
return drivers[instance]->option_is_set(option);
}

/// current_amps - returns the instantaneous current draw in amperes
bool AP_BattMonitor::current_amps(float &current, uint8_t instance) const {
if ((instance < _num_instances) && (drivers[instance] != nullptr) && drivers[instance]->has_current()) {
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_BattMonitor/AP_BattMonitor.h
Original file line number Diff line number Diff line change
Expand Up @@ -262,6 +262,8 @@ class AP_BattMonitor
void MPPT_set_powered_state_to_all(const bool power_on);
void MPPT_set_powered_state(const uint8_t instance, const bool power_on);

bool option_is_set(uint8_t instance, AP_BattMonitor_Params::Options option) const;

// cycle count
bool get_cycle_count(uint8_t instance, uint16_t &cycles) const;

Expand Down
3 changes: 2 additions & 1 deletion libraries/AP_BattMonitor/AP_BattMonitor_Params.h
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ class AP_BattMonitor_Params {
BattMonitor_LowVoltageSource_Raw = 0,
BattMonitor_LowVoltageSource_SagCompensated = 1
};
enum class Options : uint8_t {
enum class Options : uint16_t {
Ignore_UAVCAN_SoC = (1U<<0), // Ignore UAVCAN State-of-Charge (charge %) supplied value from the device and use the internally calculated one
MPPT_Use_Input_Value = (1U<<1), // MPPT reports voltage and current from Input (usually solar panel) instead of the output
MPPT_Power_Off_At_Disarm = (1U<<2), // MPPT Disabled when vehicle is disarmed, if HW supports it
Expand All @@ -26,6 +26,7 @@ class AP_BattMonitor_Params {
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
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
};

BattMonitor_LowVoltage_Source failsafe_voltage_source(void) const { return (enum BattMonitor_LowVoltage_Source)_failsafe_voltage_source.get(); }
Expand Down
3 changes: 2 additions & 1 deletion libraries/GCS_MAVLink/GCS_Common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -379,7 +379,8 @@ bool GCS_MAVLINK::send_battery_status()
const uint8_t battery_id = (last_battery_status_idx + 1) % AP_BATT_MONITOR_MAX_INSTANCES;
const auto configured_type = battery.configured_type(battery_id);
if (configured_type != AP_BattMonitor::Type::NONE &&
configured_type == battery.allocated_type(battery_id)) {
configured_type == battery.allocated_type(battery_id) &&
!battery.option_is_set(battery_id, AP_BattMonitor_Params::Options::InternalUseOnly)) {
CHECK_PAYLOAD_SIZE(BATTERY_STATUS);
send_battery_status(battery_id);
last_battery_status_idx = battery_id;
Expand Down
Loading