fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 11:38:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

UAVCAN:BAT: improve remaining time calculation (#25500)

Browse Source 
* 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 <maetugr@gmail.com>
This commit is contained in:
Claudio Chies
2025-09-05 10:57:13 +02:00
committed by GitHub
co-authored by Matthias Grob
parent 89c6d24946
commit 1840c0db48
4 changed files with 95 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/sensors/battery.cpp
+37 -76
View File
@@ -112,28 +112,22 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
}
_battery_status[instance].timestamp = hrt_absolute_time();
_battery[instance]->updateDt(_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::ReceivedDataStructure<uavcan::
_battery_status[instance].cell_count = 1;
}
// _battery_status[instance].max_cell_voltage_delta = msg.;
// _battery_status[instance].is_powering_off = msg.;
determineWarning(_battery_status[instance].remaining);
_battery_status[instance].warning = _warning;
_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_status[instance].warning = _battery[instance]->determineWarning(_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<ardu
_batt_update_mod[instance] = BatteryDataType::RawAux;
_battery_status[instance].discharged_mah = (_battery_status[instance].full_charge_capacity_wh -
_battery_status[instance].remaining_capacity_wh) / msg.nominal_voltage *
1000;
_battery_status[instance].cell_count = math::min((uint8_t)msg.voltage_cell.size(), (uint8_t)14);
_battery_status[instance].cycle_count = msg.cycle_count;
_battery_status[instance].over_discharge_count = msg.over_discharge_count;
_battery_status[instance].nominal_voltage = msg.nominal_voltage;
_battery_status[instance].time_remaining_s = math::isZero(_battery_status[instance].current_a) ? NAN :
(_battery_status[instance].remaining_capacity_wh /
_battery_status[instance].nominal_voltage / _battery_status[instance].current_a * 3600);
_battery_status[instance].is_powering_off = msg.is_powering_off;
if (msg.nominal_voltage > 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<cuav::
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].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<cuav::
_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;
// 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<cuav::
_battery_info_pub[instance].publish(_battery_info[instance]);
}
void
UavcanBatteryBridge::sumDischarged(hrt_abstime timestamp, float current_a)
{
// 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;
}
_last_timestamp = timestamp;
}
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::WARNING_EMERGENCY)) {
_warning = battery_status_s::WARNING_EMERGENCY;
} 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::WARNING_LOW)) {
_warning = battery_status_s::WARNING_LOW;
}
}
void
UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg,
uint8_t instance)
@@ -343,9 +302,11 @@ UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equi
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), "%" PRIu32,
msg.model_instance_id);
_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]);
}
}
src/drivers/uavcan/sensors/battery.hpp
-2
View File
@@ -73,8 +73,6 @@ private:
void battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg);
void battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux> &msg);
void cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &msg);
void sumDischarged(hrt_abstime timestamp, float current_a);
void determineWarning(float remaining);
void filterData(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg, uint8_t instance);
typedef uavcan::MethodBinder < UavcanBatteryBridge *,