From b5f37c9fa6bf1a6786ff390214b12c978d9f52e6 Mon Sep 17 00:00:00 2001 From: Sebastian Domoszlai Date: Fri, 28 Feb 2025 21:42:40 +0100 Subject: [PATCH] Simplify Battery-related Enum Naming (#24265) * Simplify battery-related enum naming * Fix mistakenly removed string in enum names * Fix missing renamings * Update outdated file * msg: Increase battery_status version since the enum naming was changed * Revert message version increase --------- Co-authored-by: Matthias Grob --- .../bitcraze/crazyflie/syslink/syslink_main.h | 2 +- msg/versioned/BatteryStatus.msg | 44 +++++++++---------- src/drivers/actuators/voxl_esc/voxl_esc.cpp | 4 +- src/drivers/batt_smbus/batt_smbus.cpp | 10 ++--- src/drivers/power_monitor/ina220/ina220.cpp | 2 +- src/drivers/power_monitor/ina226/ina226.cpp | 2 +- src/drivers/power_monitor/ina228/ina228.cpp | 2 +- src/drivers/power_monitor/ina238/ina238.cpp | 2 +- src/drivers/power_monitor/voxlpm/voxlpm.cpp | 2 +- src/drivers/smart_battery/batmon/batmon.cpp | 10 ++--- src/drivers/uavcan/sensors/battery.cpp | 14 +++--- src/drivers/uavcan/sensors/battery.hpp | 8 ++-- src/lib/battery/battery.cpp | 10 ++--- src/lib/battery/battery.h | 2 +- src/modules/battery_status/battery_status.cpp | 4 +- src/modules/commander/Commander.cpp | 10 ++--- src/modules/commander/Commander.hpp | 2 +- .../checks/batteryCheck.cpp | 22 +++++----- src/modules/commander/failsafe/failsafe.cpp | 20 ++++----- src/modules/esc_battery/EscBattery.cpp | 2 +- src/modules/events/set_leds.cpp | 4 +- src/modules/mavlink/mavlink_receiver.cpp | 6 +-- .../mavlink/streams/BATTERY_STATUS.hpp | 14 +++--- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 2 +- .../battery_simulator/BatterySimulator.cpp | 2 +- 25 files changed, 101 insertions(+), 101 deletions(-) diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.h b/boards/bitcraze/crazyflie/syslink/syslink_main.h index a54859508d..8bf97b1c6e 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.h +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.h @@ -139,7 +139,7 @@ private: // 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; diff --git a/msg/versioned/BatteryStatus.msg b/msg/versioned/BatteryStatus.msg index 9838786f3f..852db501df 100644 --- a/msg/versioned/BatteryStatus.msg +++ b/msg/versioned/BatteryStatus.msg @@ -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 @@ -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 diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp index 67aa579eda..89aebcbbaf 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -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; @@ -1636,7 +1636,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); diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index b9059f5445..da47a50905 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -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); diff --git a/src/drivers/power_monitor/ina220/ina220.cpp b/src/drivers/power_monitor/ina220/ina220.cpp index 305d1a61a2..c8f52bf71c 100644 --- a/src/drivers/power_monitor/ina220/ina220.cpp +++ b/src/drivers/power_monitor/ina220/ina220.cpp @@ -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; diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index 2c117caf1d..f61a7c7b79 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -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; diff --git a/src/drivers/power_monitor/ina228/ina228.cpp b/src/drivers/power_monitor/ina228/ina228.cpp index 8c0207b3f2..6505a7d65e 100644 --- a/src/drivers/power_monitor/ina228/ina228.cpp +++ b/src/drivers/power_monitor/ina228/ina228.cpp @@ -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; diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index b46b277a84..ff3f193385 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -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; diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp index 5fa109d57d..51a496bd6d 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -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) { } diff --git a/src/drivers/smart_battery/batmon/batmon.cpp b/src/drivers/smart_battery/batmon/batmon.cpp index 9061a6d3e4..ed424c1e18 100644 --- a/src/drivers/smart_battery/batmon/batmon.cpp +++ b/src/drivers/smart_battery/batmon/batmon.cpp @@ -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); diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index 77698cff16..0c8528670f 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -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) { } @@ -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; } } diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp index ceb6222da0..a27ad64505 100644 --- a/src/drivers/uavcan/sensors/battery.hpp +++ b/src/drivers/uavcan/sensors/battery.hpp @@ -107,10 +107,10 @@ private: 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 }; }; diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 21421d592d..10dbb93a68 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -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; } } @@ -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; diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 64649c3c08..baf6e5ecd6 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -175,7 +175,7 @@ private: 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}; diff --git a/src/modules/battery_status/battery_status.cpp b/src/modules/battery_status/battery_status.cpp index 1d104e4087..8d92ad251b 100644 --- a/src/modules/battery_status/battery_status.cpp +++ b/src/modules/battery_status/battery_status.cpp @@ -137,9 +137,9 @@ private: 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)) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 4c8dce5618..9583e7341c 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2193,13 +2193,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); @@ -2493,10 +2493,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 { @@ -2867,7 +2867,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)) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 395e7954cb..af16a7f81d 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -262,7 +262,7 @@ private: 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 diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index 85a19cf550..c20919ba56 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -39,7 +39,7 @@ using namespace time_literals; using battery_fault_reason_t = events::px4::enums::battery_fault_reason_t; -static_assert(battery_status_s::BATTERY_FAULT_COUNT == (static_cast(battery_fault_reason_t::_max) + 1) +static_assert(battery_status_s::FAULT_COUNT == (static_cast(battery_fault_reason_t::_max) + 1) , "Battery fault flags mismatch!"); static constexpr const char *battery_fault_reason_str(battery_fault_reason_t battery_fault_reason) @@ -78,7 +78,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) // Reset related failsafe flags otherwise failures from before disabling the check cause failsafes without reported reason reporter.failsafeFlags().battery_unhealthy = false; reporter.failsafeFlags().battery_low_remaining_time = false; - reporter.failsafeFlags().battery_warning = battery_status_s::BATTERY_WARNING_NONE; + reporter.failsafeFlags().battery_warning = battery_status_s::WARNING_NONE; return; } @@ -86,7 +86,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) // There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest // option is to check if ANY of them have a warning, and specifically find which one has the most // urgent warning. - uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE; + uint8_t worst_warning = battery_status_s::WARNING_NONE; float worst_battery_remaining = 1.f; // To make sure that all connected batteries are being regularly reported, we check which one has the // oldest timestamp. @@ -132,7 +132,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) } // trigger a battery failsafe action if a battery disconnects in flight - worst_warning = battery_status_s::BATTERY_WARNING_CRITICAL; + worst_warning = battery_status_s::WARNING_CRITICAL; } if (battery.connected) { @@ -195,13 +195,13 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) reporter.failsafeFlags().battery_warning = worst_warning; } - const bool battery_warning = reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE - && reporter.failsafeFlags().battery_warning < battery_status_s::BATTERY_WARNING_FAILED; + const bool battery_warning = reporter.failsafeFlags().battery_warning > battery_status_s::WARNING_NONE + && reporter.failsafeFlags().battery_warning < battery_status_s::WARNING_FAILED; const bool configured_arm_threshold_in_use = !context.isArmed() && (_param_com_arm_bat_min.get() >= -FLT_EPSILON); const bool below_configured_arm_threshold = (worst_battery_remaining < _param_com_arm_bat_min.get()); if (battery_warning || (configured_arm_threshold_in_use && below_configured_arm_threshold)) { - const bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::BATTERY_WARNING_CRITICAL; + const bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::WARNING_CRITICAL; NavModes affected_modes = (!configured_arm_threshold_in_use && critical_or_higher) || (configured_arm_threshold_in_use && below_configured_arm_threshold) ? NavModes::All : NavModes::None; events::LogLevel log_level = critical_or_higher || below_configured_arm_threshold @@ -209,7 +209,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) switch (reporter.failsafeFlags().battery_warning) { default: - case battery_status_s::BATTERY_WARNING_LOW: + case battery_status_s::WARNING_LOW: /* EVENT * @description * The lowest battery state of charge is below the low threshold. @@ -227,7 +227,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) break; - case battery_status_s::BATTERY_WARNING_CRITICAL: + case battery_status_s::WARNING_CRITICAL: /* EVENT * @description * The lowest battery state of charge is below the critical threshold. @@ -245,7 +245,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) break; - case battery_status_s::BATTERY_WARNING_EMERGENCY: + case battery_status_s::WARNING_EMERGENCY: /* EVENT * @description * The lowest battery state of charge is below the emergency threshold. @@ -275,7 +275,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) || is_required_battery_missing // No currently-connected batteries have any fault || battery_has_fault - || reporter.failsafeFlags().battery_warning == battery_status_s::BATTERY_WARNING_FAILED; + || reporter.failsafeFlags().battery_warning == battery_status_s::WARNING_FAILED; if (reporter.failsafeFlags().battery_unhealthy && !is_required_battery_missing && !battery_has_fault) { // missing batteries and faults are reported above already diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 3ba1131d97..6e6aee83b2 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -192,16 +192,16 @@ FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value switch (battery_warning) { default: - case battery_status_s::BATTERY_WARNING_NONE: + case battery_status_s::WARNING_NONE: options.action = Action::None; break; - case battery_status_s::BATTERY_WARNING_LOW: + case battery_status_s::WARNING_LOW: options.action = Action::Warn; options.cause = Cause::BatteryLow; break; - case battery_status_s::BATTERY_WARNING_CRITICAL: + case battery_status_s::WARNING_CRITICAL: options.action = Action::Warn; options.cause = Cause::BatteryCritical; @@ -222,7 +222,7 @@ FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value break; - case battery_status_s::BATTERY_WARNING_EMERGENCY: + case battery_status_s::WARNING_EMERGENCY: options.action = Action::Warn; options.cause = Cause::BatteryEmergency; @@ -550,21 +550,21 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, _param_com_low_bat_act.get() : (int32_t)LowBatteryAction::Warning; switch (status_flags.battery_warning) { - case battery_status_s::BATTERY_WARNING_LOW: + case battery_status_s::WARNING_LOW: _last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low, - true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_LOW)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::WARNING_LOW)); break; - case battery_status_s::BATTERY_WARNING_CRITICAL: + case battery_status_s::WARNING_CRITICAL: _last_state_battery_warning_critical = checkFailsafe(_caller_id_battery_warning_critical, _last_state_battery_warning_critical, - true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_CRITICAL)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::WARNING_CRITICAL)); break; - case battery_status_s::BATTERY_WARNING_EMERGENCY: + case battery_status_s::WARNING_EMERGENCY: _last_state_battery_warning_emergency = checkFailsafe(_caller_id_battery_warning_emergency, _last_state_battery_warning_emergency, - true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_EMERGENCY)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::WARNING_EMERGENCY)); break; default: diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 0fb4a3efad..78d1c2c71f 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -40,7 +40,7 @@ using namespace time_literals; EscBattery::EscBattery() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::lp_default), - _battery(1, this, ESC_BATTERY_INTERVAL_US, battery_status_s::BATTERY_SOURCE_ESCS) + _battery(1, this, ESC_BATTERY_INTERVAL_US, battery_status_s::SOURCE_ESCS) { } diff --git a/src/modules/events/set_leds.cpp b/src/modules/events/set_leds.cpp index 20541f6fbb..88c2ed43d0 100644 --- a/src/modules/events/set_leds.cpp +++ b/src/modules/events/set_leds.cpp @@ -101,12 +101,12 @@ void StatusDisplay::set_leds() } // handle battery warnings, once a state is reached it can not be reset - if (_battery_status_sub.get().warning == battery_status_s::BATTERY_WARNING_CRITICAL || _critical_battery) { + if (_battery_status_sub.get().warning == battery_status_s::WARNING_CRITICAL || _critical_battery) { _led_control.color = led_control_s::COLOR_RED; _led_control.mode = led_control_s::MODE_BLINK_FAST; _critical_battery = true; - } else if (_battery_status_sub.get().warning == battery_status_s::BATTERY_WARNING_LOW || _low_battery) { + } else if (_battery_status_sub.get().warning == battery_status_s::WARNING_LOW || _low_battery) { _led_control.color = led_control_s::COLOR_RED; _led_control.mode = led_control_s::MODE_FLASH; _low_battery = true; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cc20e9f355..39b626b4d2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1814,13 +1814,13 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) // Set the battery warning based on remaining charge. // Note: Smallest values must come first in evaluation. if (battery_status.remaining < _param_bat_emergen_thr.get()) { - battery_status.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + battery_status.warning = battery_status_s::WARNING_EMERGENCY; } else if (battery_status.remaining < _param_bat_crit_thr.get()) { - battery_status.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + battery_status.warning = battery_status_s::WARNING_CRITICAL; } else if (battery_status.remaining < _param_bat_low_thr.get()) { - battery_status.warning = battery_status_s::BATTERY_WARNING_LOW; + battery_status.warning = battery_status_s::WARNING_LOW; } _battery_pub.publish(battery_status); diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 633807a86e..5824c534eb 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -81,31 +81,31 @@ private: math::max((int)battery_status.time_remaining_s, 1) : 0; switch (battery_status.warning) { - case (battery_status_s::BATTERY_WARNING_NONE): + case (battery_status_s::WARNING_NONE): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_OK; break; - case (battery_status_s::BATTERY_WARNING_LOW): + case (battery_status_s::WARNING_LOW): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_LOW; break; - case (battery_status_s::BATTERY_WARNING_CRITICAL): + case (battery_status_s::WARNING_CRITICAL): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_CRITICAL; break; - case (battery_status_s::BATTERY_WARNING_EMERGENCY): + case (battery_status_s::WARNING_EMERGENCY): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_EMERGENCY; break; - case (battery_status_s::BATTERY_WARNING_FAILED): + case (battery_status_s::WARNING_FAILED): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_FAILED; break; - case (battery_status_s::BATTERY_STATE_UNHEALTHY): + case (battery_status_s::STATE_UNHEALTHY): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_UNHEALTHY; break; - case (battery_status_s::BATTERY_STATE_CHARGING): + case (battery_status_s::STATE_CHARGING): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_CHARGING; break; diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 8623950e02..8723f14131 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -285,7 +285,7 @@ private: updated = true; _batteries[i].connected = battery.connected; - if (battery.warning > battery_status_s::BATTERY_WARNING_LOW) { + if (battery.warning > battery_status_s::WARNING_LOW) { msg->failure_flags |= HL_FAILURE_FLAG_BATTERY; } } diff --git a/src/modules/simulation/battery_simulator/BatterySimulator.cpp b/src/modules/simulation/battery_simulator/BatterySimulator.cpp index 45e8fde168..92c22285bc 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.cpp @@ -36,7 +36,7 @@ BatterySimulator::BatterySimulator() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _battery(1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) { }