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

Simplify Battery-related Enum Naming #24265

Open
wants to merge 5 commits into
base: main
Choose a base branch
from
Open
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
2 changes: 1 addition & 1 deletion boards/bitcraze/crazyflie/syslink/syslink_main.h
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,7 @@ class Syslink

// nrf chip schedules battery updates with SYSLINK_SEND_PERIOD_MS
static constexpr uint32_t SYSLINK_BATTERY_STATUS_INTERVAL_US = 10_ms;
Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE};
Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE};

int32_t _rssi;
battery_state _bstate;
Expand Down
46 changes: 23 additions & 23 deletions msg/versioned/BatteryStatus.msg
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
uint32 MESSAGE_VERSION = 0
uint32 MESSAGE_VERSION = 1

uint64 timestamp # time since system start (microseconds)
bool connected # Whether or not a battery is connected, based on a voltage threshold
Expand All @@ -12,9 +12,9 @@ float32 time_remaining_s # predicted time in seconds remaining until battery i
float32 temperature # Temperature of the battery in degrees Celcius, NaN if unknown
uint8 cell_count # Number of cells, 0 if unknown

uint8 BATTERY_SOURCE_POWER_MODULE = 0
uint8 BATTERY_SOURCE_EXTERNAL = 1
uint8 BATTERY_SOURCE_ESCS = 2
uint8 SOURCE_POWER_MODULE = 0
uint8 SOURCE_EXTERNAL = 1
uint8 SOURCE_ESCS = 2
uint8 source # Battery source
uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
uint16 capacity # actual capacity of the battery
Expand All @@ -34,26 +34,26 @@ bool is_powering_off # Power off event imminent indication, false if unknown
bool is_required # Set if the battery is explicitly required before arming


uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active
uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage
uint8 BATTERY_WARNING_CRITICAL = 2 # critical voltage, return / abort immediately
uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required
uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely
uint8 BATTERY_STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field.
uint8 BATTERY_STATE_CHARGING = 7 # Battery is charging
uint8 WARNING_NONE = 0 # no battery low voltage warning active
uint8 WARNING_LOW = 1 # warning of low voltage
uint8 WARNING_CRITICAL = 2 # critical voltage, return / abort immediately
uint8 WARNING_EMERGENCY = 3 # immediate landing required
uint8 WARNING_FAILED = 4 # the battery has failed completely
uint8 STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field.
uint8 STATE_CHARGING = 7 # Battery is charging

uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged
uint8 BATTERY_FAULT_SPIKES = 1 # Voltage spikes
uint8 BATTERY_FAULT_CELL_FAIL= 2 # One or more cells have failed
uint8 BATTERY_FAULT_OVER_CURRENT = 3 # Over-current
uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 # Over-temperature
uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault
uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage).
uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware
uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system
uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 # hardware problem
uint8 BATTERY_FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming
uint8 BATTERY_FAULT_COUNT = 11 # Counter - keep it as last element!
uint8 FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged
uint8 FAULT_SPIKES = 1 # Voltage spikes
uint8 FAULT_CELL_FAIL= 2 # One or more cells have failed
uint8 FAULT_OVER_CURRENT = 3 # Over-current
uint8 FAULT_OVER_TEMPERATURE = 4 # Over-temperature
uint8 FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault
uint8 FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage).
uint8 FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware
uint8 FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system
uint8 FAULT_HARDWARE_FAILURE = 9 # hardware problem
uint8 FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming
uint8 FAULT_COUNT = 11 # Counter - keep it as last element!

uint16 faults # Smart battery supply status/fault flags (bitmask) for health indication.
uint8 warning # Current battery warning
Expand Down
4 changes: 2 additions & 2 deletions src/drivers/actuators/voxl_esc/voxl_esc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ VoxlEsc::VoxlEsc() :
_mixing_output{"VOXL_ESC", VOXL_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false},
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
_output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")),
_battery(1, nullptr, _battery_report_interval, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
_battery(1, nullptr, _battery_report_interval, battery_status_s::SOURCE_POWER_MODULE)
{
_device = VOXL_ESC_DEFAULT_PORT;

Expand Down Expand Up @@ -1547,7 +1547,7 @@ void VoxlEsc::print_params()

PX4_INFO("Params: VOXL_ESC_VLOG: %" PRId32, _parameters.verbose_logging);
PX4_INFO("Params: VOXL_ESC_PUB_BST: %" PRId32, _parameters.publish_battery_status);

PX4_INFO("Params: VOXL_ESC_T_WARN: %" PRId32, _parameters.esc_warn_temp_threshold);
PX4_INFO("Params: VOXL_ESC_T_OVER: %" PRId32, _parameters.esc_over_temp_threshold);
}
Expand Down
10 changes: 5 additions & 5 deletions src/drivers/batt_smbus/batt_smbus.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -172,19 +172,19 @@ void BATT_SMBUS::RunImpl()

// Check if max lifetime voltage delta is greater than allowed.
if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) {
new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
new_report.warning = battery_status_s::WARNING_CRITICAL;

} else if (new_report.remaining > _low_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_NONE;
new_report.warning = battery_status_s::WARNING_NONE;

} else if (new_report.remaining > _crit_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_LOW;
new_report.warning = battery_status_s::WARNING_LOW;

} else if (new_report.remaining > _emergency_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
new_report.warning = battery_status_s::WARNING_CRITICAL;

} else {
new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
new_report.warning = battery_status_s::WARNING_EMERGENCY;
}

new_report.interface_error = perf_event_count(_interface->_interface_errors);
Expand Down
2 changes: 1 addition & 1 deletion src/drivers/power_monitor/ina220/ina220.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ INA220::INA220(const I2CSPIDriverConfig &config, int battery_index) :
_collection_errors(perf_alloc(PC_COUNT, "ina220_collection_err")),
_measure_errors(perf_alloc(PC_COUNT, "ina220_measurement_err")),
_ch_type((PM_CH_TYPE)config.custom2),
_battery(battery_index, this, INA220_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
_battery(battery_index, this, INA220_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE)
{
float fvalue = MAX_CURRENT;
_max_current = fvalue;
Expand Down
2 changes: 1 addition & 1 deletion src/drivers/power_monitor/ina226/ina226.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) :
_comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")),
_collection_errors(perf_alloc(PC_COUNT, "ina226_collection_err")),
_measure_errors(perf_alloc(PC_COUNT, "ina226_measurement_err")),
_battery(battery_index, this, INA226_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
_battery(battery_index, this, INA226_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE)
{
float fvalue = MAX_CURRENT;
_max_current = fvalue;
Expand Down
2 changes: 1 addition & 1 deletion src/drivers/power_monitor/ina228/ina228.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) :
_comms_errors(perf_alloc(PC_COUNT, "ina228_com_err")),
_collection_errors(perf_alloc(PC_COUNT, "ina228_collection_err")),
_measure_errors(perf_alloc(PC_COUNT, "ina228_measurement_err")),
_battery(battery_index, this, INA228_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
_battery(battery_index, this, INA228_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE)
{
float fvalue = MAX_CURRENT;
_max_current = fvalue;
Expand Down
2 changes: 1 addition & 1 deletion src/drivers/power_monitor/ina238/ina238.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) :
_sample_perf(perf_alloc(PC_ELAPSED, "ina238_read")),
_comms_errors(perf_alloc(PC_COUNT, "ina238_com_err")),
_collection_errors(perf_alloc(PC_COUNT, "ina238_collection_err")),
_battery(battery_index, this, INA238_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
_battery(battery_index, this, INA238_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE)
{
float fvalue = DEFAULT_MAX_CURRENT;
_max_current = fvalue;
Expand Down
2 changes: 1 addition & 1 deletion src/drivers/power_monitor/voxlpm/voxlpm.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ VOXLPM::VOXLPM(const I2CSPIDriverConfig &config) :
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")),
_ch_type((VOXLPM_CH_TYPE)config.custom1),
_battery(1, this, _meas_interval_us, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
_battery(1, this, _meas_interval_us, battery_status_s::SOURCE_POWER_MODULE)
{
}

Expand Down
10 changes: 5 additions & 5 deletions src/drivers/smart_battery/batmon/batmon.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -199,19 +199,19 @@ void Batmon::RunImpl()
// TODO: This critical setting should be set with BMS info or through a paramter
// Setting a hard coded BATT_CELL_VOLTAGE_THRESHOLD_FAILED may not be appropriate
//if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) {
// new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
// new_report.warning = battery_status_s::WARNING_CRITICAL;

if (new_report.remaining > _low_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_NONE;
new_report.warning = battery_status_s::WARNING_NONE;

} else if (new_report.remaining > _crit_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_LOW;
new_report.warning = battery_status_s::WARNING_LOW;

} else if (new_report.remaining > _emergency_thr) {
new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
new_report.warning = battery_status_s::WARNING_CRITICAL;

} else {
new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
new_report.warning = battery_status_s::WARNING_EMERGENCY;
}

new_report.interface_error = perf_event_count(_interface->_interface_errors);
Expand Down
14 changes: 7 additions & 7 deletions src/drivers/uavcan/sensors/battery.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) :
ModuleParams(nullptr),
_sub_battery(node),
_sub_battery_aux(node),
_warning(battery_status_s::BATTERY_WARNING_NONE),
_warning(battery_status_s::WARNING_NONE),
_last_timestamp(0)
{
}
Expand Down Expand Up @@ -215,14 +215,14 @@ void
UavcanBatteryBridge::determineWarning(float remaining)
{
// propagate warning state only if the state is higher, otherwise remain in current warning state
if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::WARNING_EMERGENCY)) {
_warning = battery_status_s::WARNING_EMERGENCY;

} else if (remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
} else if (remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::WARNING_CRITICAL)) {
_warning = battery_status_s::WARNING_CRITICAL;

} else if (remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
_warning = battery_status_s::BATTERY_WARNING_LOW;
} else if (remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::WARNING_LOW)) {
_warning = battery_status_s::WARNING_LOW;
}
}

Expand Down
8 changes: 4 additions & 4 deletions src/drivers/uavcan/sensors/battery.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -107,10 +107,10 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams

static_assert(battery_status_s::MAX_INSTANCES <= BATTERY_INDEX_4, "Battery array too big");

Battery battery1 = {BATTERY_INDEX_1, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL};
Battery battery2 = {BATTERY_INDEX_2, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL};
Battery battery3 = {BATTERY_INDEX_3, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL};
Battery battery4 = {BATTERY_INDEX_4, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL};
Battery battery1 = {BATTERY_INDEX_1, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL};
Battery battery2 = {BATTERY_INDEX_2, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL};
Battery battery3 = {BATTERY_INDEX_3, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL};
Battery battery4 = {BATTERY_INDEX_4, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL};

Battery *_battery[battery_status_s::MAX_INSTANCES] = { &battery1, &battery2, &battery3, &battery4 };
};
10 changes: 5 additions & 5 deletions src/lib/battery/battery.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -307,16 +307,16 @@ void Battery::estimateStateOfCharge()
uint8_t Battery::determineWarning(float state_of_charge)
{
if (state_of_charge < _params.emergen_thr) {
return battery_status_s::BATTERY_WARNING_EMERGENCY;
return battery_status_s::WARNING_EMERGENCY;

} else if (state_of_charge < _params.crit_thr) {
return battery_status_s::BATTERY_WARNING_CRITICAL;
return battery_status_s::WARNING_CRITICAL;

} else if (state_of_charge < _params.low_thr) {
return battery_status_s::BATTERY_WARNING_LOW;
return battery_status_s::WARNING_LOW;

} else {
return battery_status_s::BATTERY_WARNING_NONE;
return battery_status_s::WARNING_NONE;
}
}

Expand All @@ -327,7 +327,7 @@ uint16_t Battery::determineFaults()
if ((_params.n_cells > 0)
&& (_voltage_v > (_params.n_cells * _params.v_charged * 1.05f))) {
// Reported as a "spike" since "over-voltage" does not exist in MAV_BATTERY_FAULT
faults |= (1 << battery_status_s::BATTERY_FAULT_SPIKES);
faults |= (1 << battery_status_s::FAULT_SPIKES);
}

return faults;
Expand Down
2 changes: 1 addition & 1 deletion src/lib/battery/battery.h
Original file line number Diff line number Diff line change
Expand Up @@ -175,7 +175,7 @@ class Battery : public ModuleParams
float _state_of_charge_volt_based{-1.f}; // [0,1]
float _state_of_charge{-1.f}; // [0,1]
float _scale{1.f};
uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE};
uint8_t _warning{battery_status_s::WARNING_NONE};
hrt_abstime _last_timestamp{0};
bool _armed{false};
bool _vehicle_status_is_fw{false};
Expand Down
4 changes: 2 additions & 2 deletions src/modules/battery_status/battery_status.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -137,9 +137,9 @@ class BatteryStatus : public ModuleBase<BatteryStatus>, public ModuleParams, pub
BatteryStatus::BatteryStatus() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_battery1(1, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0),
_battery1(1, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE, 0),
#if BOARD_NUMBER_BRICKS > 1
_battery2(2, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 1),
_battery2(2, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE, 1),
#endif
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME))
{
Expand Down
10 changes: 5 additions & 5 deletions src/modules/commander/Commander.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2198,13 +2198,13 @@ void Commander::updateTunes()

} else if (!_vehicle_status.usb_connected &&
(_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
(_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
(_battery_warning == battery_status_s::WARNING_CRITICAL)) {

/* play tune on battery critical */
set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST);

} else if ((_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
(_battery_warning == battery_status_s::BATTERY_WARNING_LOW)) {
(_battery_warning == battery_status_s::WARNING_LOW)) {
/* play tune on battery warning */
set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW);

Expand Down Expand Up @@ -2498,10 +2498,10 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
if (_vehicle_status.failsafe) {
led_color = led_control_s::COLOR_PURPLE;

} else if (battery_warning == battery_status_s::BATTERY_WARNING_LOW) {
} else if (battery_warning == battery_status_s::WARNING_LOW) {
led_color = led_control_s::COLOR_AMBER;

} else if (battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
} else if (battery_warning == battery_status_s::WARNING_CRITICAL) {
led_color = led_control_s::COLOR_RED;

} else {
Expand Down Expand Up @@ -2893,7 +2893,7 @@ void Commander::battery_status_check()
// Handle shutdown request from emergency battery action
if (_battery_warning != _failsafe_flags.battery_warning) {

if (_failsafe_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
if (_failsafe_flags.battery_warning == battery_status_s::WARNING_EMERGENCY) {
#if defined(BOARD_HAS_POWER_CONTROL)

if (!isArmed() && (px4_shutdown_request(60_s) == 0)) {
Expand Down
2 changes: 1 addition & 1 deletion src/modules/commander/Commander.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -263,7 +263,7 @@ class Commander : public ModuleBase<Commander>, public ModuleParams

hrt_abstime _last_health_and_arming_check{0};

uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
uint8_t _battery_warning{battery_status_s::WARNING_NONE};

bool _failsafe_user_override_request{false}; ///< override request due to stick movements

Expand Down
Loading
Loading