From 1840c0db48f4cc260a3769a8ed5fa7420f6e9818 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Fri, 5 Sep 2025 10:57:13 +0200 Subject: [PATCH] UAVCAN:BAT: improve remaining time calculation (#25500) * UAVCAN:BAT: improve remaining time calculation * UAVCAN:BAT: fix time_remaining calculation, bugfixes, improved filter convergence time * UAVCAN:BAT: remove BatteryInfo Publishing if no valid info * UAVCAN + Battery library: suggestions while reviewing --------- Co-authored-by: Matthias Grob --- src/drivers/uavcan/sensors/battery.cpp | 113 ++++++++----------------- src/drivers/uavcan/sensors/battery.hpp | 2 - src/lib/battery/battery.cpp | 57 +++++++------ src/lib/battery/battery.h | 31 +++++-- 4 files changed, 95 insertions(+), 108 deletions(-) diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index cb1619e0cf..643e699ad0 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -112,28 +112,22 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructureupdateDt(_battery_status[instance].timestamp); _battery_status[instance].voltage_v = msg.voltage; _battery_status[instance].current_a = msg.current; - _battery_status[instance].current_average_a = msg.current; if (_batt_update_mod[instance] == BatteryDataType::Raw) { - sumDischarged(_battery_status[instance].timestamp, _battery_status[instance].current_a); - _battery_status[instance].discharged_mah = _discharged_mah; + _battery_status[instance].discharged_mah = _battery[instance]->sumDischarged(fabsf(msg.current)); _battery_status[instance].time_remaining_s = NAN; } _battery_status[instance].remaining = msg.state_of_charge_pct / 100.0f; // between 0 and 1 _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; _battery_status[instance].source = msg.status_flags & uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE; - // _battery_status[instance].priority = msg.; - _battery_status[instance].capacity = msg.full_charge_capacity_wh; _battery_status[instance].full_charge_capacity_wh = msg.full_charge_capacity_wh; _battery_status[instance].remaining_capacity_wh = msg.remaining_capacity_wh; - // _battery_status[instance].cycle_count = msg.; - // _battery_status[instance].average_time_to_empty = msg.; _battery_status[instance].id = msg.getSrcNodeID().get(); if (_batt_update_mod[instance] == BatteryDataType::Raw) { @@ -144,21 +138,18 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructuredetermineWarning(_battery_status[instance].remaining); if (_batt_update_mod[instance] == BatteryDataType::Raw) { publish(msg.getSrcNodeID().get(), &_battery_status[instance]); - _battery_info_pub[instance].publish(_battery_info[instance]); + + if (msg.model_instance_id > 0) { + _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), + "%" PRIu32, msg.model_instance_id); + _battery_info_pub[instance].publish(_battery_info[instance]); + } } } @@ -182,18 +173,24 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure FLT_EPSILON) { + _battery_status[instance].capacity = + _battery_status[instance].full_charge_capacity_wh * 1000.f / msg.nominal_voltage; + } + + _battery[instance]->setCapacityMah(_battery_status[instance].capacity); + _battery[instance]->setStateOfCharge(_battery_status[instance].remaining); + // Absolute value of current as sign not clearly defined and vendors are inconsistent + _battery_status[instance].time_remaining_s = + _battery[instance]->computeRemainingTime(fabsf(_battery_status[instance].current_a)); + _battery_status[instance].current_average_a = _battery[instance]->getCurrentAverage(); + for (uint8_t i = 0; i < _battery_status[instance].cell_count; i++) { _battery_status[instance].voltage_cell_v[i] = msg.voltage_cell[i]; } @@ -234,7 +231,7 @@ void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure 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].capacity = msg.full_charge_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; @@ -247,19 +244,18 @@ void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure Ah - const float current_a = math::isZero(_battery_status[instance].current_average_a) ? - _battery_status[instance].current_a : _battery_status[instance].current_average_a; + // use Battery class for time_remaining calculation + _battery[instance]->updateDt(_battery_status[instance].timestamp); + _battery[instance]->setStateOfCharge(_battery_status[instance].remaining); + _battery[instance]->setCapacityMah(_battery_status[instance].capacity); _battery_status[instance].time_remaining_s = - math::isZero(current_a) ? NAN : (remaining_ah / current_a * 3600.f); // Ah / A = h * 3600 = s + _battery[instance]->computeRemainingTime(_battery_status[instance].current_a); 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; + _battery_status[instance].warning = _battery[instance]->determineWarning(_battery_status[instance].remaining); uint16_t faults = 0; @@ -290,43 +286,6 @@ void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure &msg, uint8_t instance) @@ -343,9 +302,11 @@ UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure 0) { + _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), + "%" PRIu32, msg.model_instance_id); + _battery_info_pub[instance].publish(_battery_info[instance]); + } } diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp index ed61cd0817..0c0ab3c949 100644 --- a/src/drivers/uavcan/sensors/battery.hpp +++ b/src/drivers/uavcan/sensors/battery.hpp @@ -73,8 +73,6 @@ private: 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); typedef uavcan::MethodBinder < UavcanBatteryBridge *, diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 10dbb93a68..844f24de74 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -113,6 +113,8 @@ void Battery::updateTemperature(const float temperature_c) void Battery::updateBatteryStatus(const hrt_abstime ×tamp) { + updateDt(timestamp); + // Require minimum voltage otherwise override connected status if (_voltage_v < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) { _connected = false; @@ -129,7 +131,7 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp) resetInternalResistanceEstimation(_voltage_v, _current_a); } - sumDischarged(timestamp, _current_a); + sumDischarged(_current_a); _state_of_charge_volt_based = calculateStateOfChargeVoltageBased(_voltage_v, _current_a); @@ -159,7 +161,7 @@ battery_status_s Battery::getBatteryStatus() battery_status.connected = _connected; battery_status.source = _source; battery_status.priority = _priority; - battery_status.capacity = _params.capacity > 0.f ? static_cast(_params.capacity) : 0; + battery_status.capacity = static_cast(_capacity_mah); battery_status.id = static_cast(_index); battery_status.warning = _warning; battery_status.timestamp = hrt_absolute_time(); @@ -188,28 +190,26 @@ void Battery::updateAndPublishBatteryStatus(const hrt_abstime ×tamp) updateBatteryStatus(timestamp); publishBatteryStatus(getBatteryStatus()); } - -void Battery::sumDischarged(const hrt_abstime ×tamp, float current_a) +void Battery::updateDt(const hrt_abstime ×tamp) { - // Not a valid measurement - if (current_a < 0.f) { - // Because the measurement was invalid we need to stop integration - // and re-initialize with the next valid measurement - _last_timestamp = 0; - return; - } - - // Ignore first update because we don't know dt. if (_last_timestamp != 0) { - const float dt = (timestamp - _last_timestamp) / 1e6; - // mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h]) - _discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f); - _discharged_mah += _discharged_mah_loop; + _dt = math::min((timestamp - _last_timestamp) / 1e6f, 2.f); // guard to a maximum 2 seconds dt } _last_timestamp = timestamp; } +float Battery::sumDischarged(float current_a) +{ + if (_dt > FLT_EPSILON) { + // mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h]) + _discharged_mah_loop = (current_a * 1e3f) * (_dt / 3600.f); + _discharged_mah += _discharged_mah_loop; + } + + return _discharged_mah; +} + float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const float current_a) { if (_params.n_cells == 0) { @@ -287,16 +287,16 @@ void Battery::resetInternalResistanceEstimation(const float voltage_v, const flo void Battery::estimateStateOfCharge() { // choose which quantity we're using for final reporting - if ((_params.capacity > 0.f) && _battery_initialized) { + if ((_capacity_mah > 0.f) && _battery_initialized) { // if battery capacity is known, fuse voltage measurement with used capacity // The lower the voltage the more adjust the estimate with it to avoid deep discharge const float weight_v = 3e-2f * (1 - _state_of_charge_volt_based); _state_of_charge = (1 - weight_v) * _state_of_charge + weight_v * _state_of_charge_volt_based; // directly apply current capacity slope calculated using current - _state_of_charge -= _discharged_mah_loop / _params.capacity; + _state_of_charge -= _discharged_mah_loop / _capacity_mah; _state_of_charge = math::max(_state_of_charge, 0.f); - const float state_of_charge_current_based = math::max(1.f - _discharged_mah / _params.capacity, 0.f); + const float state_of_charge_current_based = math::max(1.f - _discharged_mah / _capacity_mah, 0.f); _state_of_charge = math::min(state_of_charge_current_based, _state_of_charge); } else { @@ -376,14 +376,18 @@ float Battery::computeRemainingTime(float current_a) // For FW only update when we are in level flight if (!_vehicle_status_is_fw || ((hrt_absolute_time() - _flight_phase_estimation_sub.get().timestamp) < 2_s && _flight_phase_estimation_sub.get().flight_phase == flight_phase_estimation_s::FLIGHT_PHASE_LEVEL)) { - // only update with positive numbers - _current_average_filter_a.update(fmaxf(current_a, 0.f)); + if (_dt > FLT_EPSILON) { + _current_average_filter_a.update(fmaxf(current_a, 0.f), _dt); + + } else { + _current_average_filter_a.update(fmaxf(current_a, 0.f)); + } } } // Remaining time estimation only possible with capacity - if (_params.capacity > 0.f) { - const float remaining_capacity_mah = _state_of_charge * _params.capacity; + if (_capacity_mah > 0.f) { + const float remaining_capacity_mah = _state_of_charge * _capacity_mah; const float current_ma = fmaxf(_current_average_filter_a.getState() * 1e3f, FLT_EPSILON); time_remaining_s = remaining_capacity_mah / current_ma * 3600.f; } @@ -397,7 +401,6 @@ void Battery::updateParams() param_get(_param_handles.v_empty, &_params.v_empty); param_get(_param_handles.v_charged, &_params.v_charged); param_get(_param_handles.n_cells, &_params.n_cells); - param_get(_param_handles.capacity, &_params.capacity); param_get(_param_handles.r_internal, &_params.r_internal); param_get(_param_handles.source, &_params.source); param_get(_param_handles.low_thr, &_params.low_thr); @@ -405,6 +408,10 @@ void Battery::updateParams() param_get(_param_handles.emergen_thr, &_params.emergen_thr); param_get(_param_handles.bat_avrg_current, &_params.bat_avrg_current); + float capacity{0.f}; + param_get(_param_handles.capacity, &capacity); + setCapacityMah(capacity); + if (n_cells != _params.n_cells) { _internal_resistance_initialized = false; } diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index baf6e5ecd6..66dd73bf27 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -88,7 +88,8 @@ public: void setPriority(const uint8_t priority) { _priority = priority; } void setConnected(const bool connected) { _connected = connected; } - void setStateOfCharge(const float soc) { _state_of_charge = soc; _external_state_of_charge = true; } + void setStateOfCharge(const float soc) { _state_of_charge = math::constrain(soc, 0.f, 1.f); _external_state_of_charge = true; } + void setCapacityMah(const float capacity) { _capacity_mah = math::max(capacity, 0.f); } void updateVoltage(const float voltage_v); void updateCurrent(const float current_a); void updateTemperature(const float temperature_c); @@ -101,6 +102,7 @@ public: void updateBatteryStatus(const hrt_abstime ×tamp); battery_status_s getBatteryStatus(); + float getCurrentAverage() const { return PX4_ISFINITE(_current_average_filter_a.getState()) ? _current_average_filter_a.getState() : -1.f; } void publishBatteryStatus(const battery_status_s &battery_status); /** @@ -110,6 +112,27 @@ public: */ void updateAndPublishBatteryStatus(const hrt_abstime ×tamp); + /** + * Calculates how much time is left before the battery is depleted, + * given the heavily low-pass filtered current consumption. + * Requires the capacity and state of charge e.g. externally set through setCapacity() and setStateOfCharge(). + * + * @param current_a The current draw from the battery in amperes. + * @return Estimated remaining time in seconds. + */ + float computeRemainingTime(float current_a); + + /** + * Updates coulomb counting + * Requires a dt, seeupdateDt() + * + * @param current_a Positive current draw in A + * @return Accumulated used capacity in mAh + */ + float sumDischarged(float current_a); + uint8_t determineWarning(float state_of_charge); + void updateDt(const hrt_abstime ×tamp); + protected: static constexpr float LITHIUM_BATTERY_RECOGNITION_VOLTAGE = 2.1f; @@ -130,7 +153,6 @@ protected: float v_empty; float v_charged; int32_t n_cells; - float capacity; float r_internal; float low_thr; float crit_thr; @@ -145,13 +167,10 @@ protected: void updateParams() override; private: - void sumDischarged(const hrt_abstime ×tamp, float current_a); float calculateStateOfChargeVoltageBased(const float voltage_v, const float current_a); void estimateStateOfCharge(); - uint8_t determineWarning(float state_of_charge); uint16_t determineFaults(); void computeScale(); - float computeRemainingTime(float current_a); uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::SubscriptionData _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)}; @@ -176,6 +195,8 @@ private: float _state_of_charge{-1.f}; // [0,1] float _scale{1.f}; uint8_t _warning{battery_status_s::WARNING_NONE}; + float _dt{0.f}; + float _capacity_mah{0.f}; hrt_abstime _last_timestamp{0}; bool _armed{false}; bool _vehicle_status_is_fw{false};