From a58ef9c9fc94add5d469fa944ced003b7c2e3244 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 4 Jun 2025 16:19:13 +0200 Subject: [PATCH 1/2] uavcan battery: fix battery_status.scale, time_remaining and timestamp - timestamp was 0 if uavcan::BatteryInfo was received before uavcan::BatteryInfoAux - scale was not set as unknown (-1) even though it is since it's never updated - time_remaining was not initialized correctly and could sometimes be 0 unexpectedly which causes the drone to failsafe because there's reportedly no flight time left --- src/drivers/uavcan/sensors/battery.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index 0198749f73e3..29af67c5158a 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -110,10 +110,11 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure= 1, or -1 if unknown + _battery_status[instance].scale = -1.f; _battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius // _battery_status[instance].cell_count = msg.; _battery_status[instance].connected = true; @@ -123,7 +124,6 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure Date: Wed, 4 Jun 2025 16:22:34 +0200 Subject: [PATCH 2/2] uavcan battery: add support for cuav::equipment::power::CBAT message emitted by e.g. uavcan Tattu batteries. --- src/drivers/uavcan/Kconfig | 2 +- src/drivers/uavcan/sensors/battery.cpp | 105 +++++++++++++++++++++++-- src/drivers/uavcan/sensors/battery.hpp | 9 +++ src/drivers/uavcan/uavcan_params.c | 3 +- 4 files changed, 111 insertions(+), 8 deletions(-) diff --git a/src/drivers/uavcan/Kconfig b/src/drivers/uavcan/Kconfig index 37306dabdfde..abf4d8367af6 100644 --- a/src/drivers/uavcan/Kconfig +++ b/src/drivers/uavcan/Kconfig @@ -47,7 +47,7 @@ if DRIVERS_UAVCAN default y config UAVCAN_SENSOR_BATTERY - bool "Subscribe to Battery: uavcan::equipment::power::BatteryInfo | ardupilot::equipment::power::BatteryInfoAux" + bool "Subscribe to Battery: uavcan::equipment::power::BatteryInfo | ardupilot::equipment::power::BatteryInfoAux | cuav::equipment::power::CBAT" default y config UAVCAN_SENSOR_DIFFERENTIAL_PRESSURE diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index 29af67c5158a..cb1619e0cf3b 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -44,6 +44,7 @@ UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) : ModuleParams(nullptr), _sub_battery(node), _sub_battery_aux(node), + _sub_cbat(node), _warning(battery_status_s::WARNING_NONE), _last_timestamp(0) { @@ -78,6 +79,13 @@ int UavcanBatteryBridge::init() return res; } + res = _sub_cbat.start(CBATCbBinder(this, &UavcanBatteryBridge::cbat_sub_cb)); + + if (res < 0) { + PX4_ERR("failed to start uavcan sub: %d", res); + return res; + } + return 0; } @@ -92,7 +100,8 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure= battery_status_s::MAX_INSTANCES) { + if (instance >= battery_status_s::MAX_INSTANCES + || _batt_update_mod[instance] == BatteryDataType::CBAT) { return; } @@ -165,11 +174,9 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure= battery_status_s::MAX_INSTANCES) { - return; - } - - if (_batt_update_mod[instance] == BatteryDataType::Filter) { + if (instance >= battery_status_s::MAX_INSTANCES + || _batt_update_mod[instance] == BatteryDataType::Filter + || _batt_update_mod[instance] == BatteryDataType::CBAT) { return; } @@ -197,6 +204,92 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + uint8_t instance = 0; + + for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) { + if (_battery_status[instance].id == msg.getSrcNodeID().get()) { + break; + } + } + + if (instance >= battery_status_s::MAX_INSTANCES + || _batt_update_mod[instance] == BatteryDataType::Filter) { + return; + } + + // If CBAT message with superset of data was received, skip BatteryInfo messages + _batt_update_mod[instance] = BatteryDataType::CBAT; + + _battery_status[instance].timestamp = hrt_absolute_time(); + _battery_status[instance].voltage_v = msg.voltage; + _battery_status[instance].current_a = -msg.current; // discharge reported negative + _battery_status[instance].current_average_a = -msg.average_current; // discharge reported negative + _battery_status[instance].discharged_mah = msg.full_charge_capacity - msg.remaining_capacity; // mAh + _battery_status[instance].remaining = msg.state_of_charge / 100.f; + _battery_status[instance].scale = -1.f; // not supported, needs to be computed centrally + _battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius + _battery_status[instance].full_charge_capacity_wh = + msg.full_charge_capacity * msg.nominal_voltage / 1000.f; // mAh -> Wh + _battery_status[instance].remaining_capacity_wh = msg.remaining_capacity * msg.nominal_voltage / 1000.f; // mAh -> Wh + _battery_status[instance].nominal_voltage = msg.nominal_voltage; + _battery_status[instance].capacity = msg.design_capacity; // mAh + _battery_status[instance].cycle_count = msg.cycle_count; + _battery_status[instance].average_time_to_empty = msg.average_time_to_empty; + _battery_status[instance].manufacture_date = msg.manufacture_date; + _battery_status[instance].state_of_health = msg.state_of_health; + _battery_status[instance].max_error = msg.max_error; + _battery_status[instance].over_discharge_count = msg.over_discharge_count; + _battery_status[instance].connected = true; + _battery_status[instance].cell_count = msg.cell_count; + _battery_status[instance].source = battery_status_s::SOURCE_EXTERNAL; + _battery_status[instance].id = msg.getSrcNodeID().get(); + _battery_status[instance].is_powering_off = msg.is_powering_off; + + // For time remaining calculation use the average current if supplied + const float remaining_ah = msg.remaining_capacity / 1000.f; // mAh -> Ah + const float current_a = math::isZero(_battery_status[instance].current_average_a) ? + _battery_status[instance].current_a : _battery_status[instance].current_average_a; + _battery_status[instance].time_remaining_s = + math::isZero(current_a) ? NAN : (remaining_ah / current_a * 3600.f); // Ah / A = h * 3600 = s + + for (uint8_t i = 0; i < _battery_status[instance].cell_count; i++) { + _battery_status[instance].voltage_cell_v[i] = msg.voltage_cell[i]; + } + + determineWarning(_battery_status[instance].remaining); + _battery_status[instance].warning = _warning; + + uint16_t faults = 0; + + if (msg.status_flags & cuav::equipment::power::CBAT::STATUS_FLAG_OVERLOAD) { + faults |= (1 << battery_status_s::FAULT_OVER_CURRENT); + } + + if (msg.status_flags & cuav::equipment::power::CBAT::STATUS_FLAG_BAD_BATTERY) { + faults |= (1 << battery_status_s::FAULT_HARDWARE_FAILURE); + } + + if (msg.status_flags & cuav::equipment::power::CBAT::STATUS_FLAG_TEMP_HOT) { + faults |= (1 << battery_status_s::FAULT_OVER_TEMPERATURE); + } + + if (msg.status_flags & cuav::equipment::power::CBAT::STATUS_FLAG_TEMP_COLD) { + faults |= (1 << battery_status_s::FAULT_UNDER_TEMPERATURE); + } + + _battery_status[instance].faults = faults; + + publish(msg.getSrcNodeID().get(), &_battery_status[instance]); + + _battery_info[instance].timestamp = _battery_status[instance].timestamp; + _battery_info[instance].id = _battery_status[instance].id; + snprintf(_battery_info[instance].serial_number, sizeof(_battery_info[instance].serial_number), "%" PRIu16, + msg.serial_number); + _battery_info_pub[instance].publish(_battery_info[instance]); +} + void UavcanBatteryBridge::sumDischarged(hrt_abstime timestamp, float current_a) { diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp index cba57acfe799..c4caacd039fd 100644 --- a/src/drivers/uavcan/sensors/battery.hpp +++ b/src/drivers/uavcan/sensors/battery.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -66,10 +67,12 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams Raw, // data from BatteryInfo message only RawAux, // data combination from BatteryInfo and BatteryInfoAux messages Filter, // filter data from BatteryInfo message with Battery library + CBAT, // CBAT messages }; void battery_sub_cb(const uavcan::ReceivedDataStructure &msg); void battery_aux_sub_cb(const uavcan::ReceivedDataStructure &msg); + void cbat_sub_cb(const uavcan::ReceivedDataStructure &msg); void sumDischarged(hrt_abstime timestamp, float current_a); void determineWarning(float remaining); void filterData(const uavcan::ReceivedDataStructure &msg, uint8_t instance); @@ -83,8 +86,14 @@ class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams (const uavcan::ReceivedDataStructure &) > BatteryInfoAuxCbBinder; + typedef uavcan::MethodBinder < UavcanBatteryBridge *, + void (UavcanBatteryBridge::*) + (const uavcan::ReceivedDataStructure &) > + CBATCbBinder; + uavcan::Subscriber _sub_battery; uavcan::Subscriber _sub_battery_aux; + uavcan::Subscriber _sub_cbat; DEFINE_PARAMETERS( (ParamFloat) _param_bat_low_thr, diff --git a/src/drivers/uavcan/uavcan_params.c b/src/drivers/uavcan/uavcan_params.c index 4e9253fbf60b..42ef96e6fd45 100644 --- a/src/drivers/uavcan/uavcan_params.c +++ b/src/drivers/uavcan/uavcan_params.c @@ -288,10 +288,11 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_BARO, 0); * Enable UAVCAN battery subscription. * uavcan::equipment::power::BatteryInfo * ardupilot::equipment::power::BatteryInfoAux + * cuav::equipment::power::CBAT * * 0 - Disable * 1 - Use raw data. Recommended for Smart battery - * 2 - Filter the data with internal battery library + * 2 - Filter the data with internal battery library (unsupported with CBAT) * * @min 0 * @max 2