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 battery: add support for cuav::equipment::power::CBAT message

Browse Source 
emitted by e.g. uavcan Tattu batteries.
This commit is contained in:
Matthias Grob
2025-06-25 21:11:23 +02:00
parent 30d4ff3c6c
commit bb6d43d5e3
4 changed files with 111 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/Kconfig
+1 -1
View File
@@ -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
src/drivers/uavcan/sensors/battery.cpp
+99 -6
View File
@@ -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<uavcan::
}
}
if (instance >= 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<ardu
}
}
if (instance >= 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<ardu
}
}
void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &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)
{
src/drivers/uavcan/sensors/battery.hpp
+9
View File
@@ -42,6 +42,7 @@
#include <uORB/topics/battery_status.h>
#include <uavcan/equipment/power/BatteryInfo.hpp>
#include <ardupilot/equipment/power/BatteryInfoAux.hpp>
#include <cuav/equipment/power/CBAT.hpp>
#include <battery/battery.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/module_params.h>
@@ -66,10 +67,12 @@ private:
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<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);
@@ -83,8 +86,14 @@ private:
(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux> &) >
BatteryInfoAuxCbBinder;
typedef uavcan::MethodBinder < UavcanBatteryBridge *,
void (UavcanBatteryBridge::*)
(const uavcan::ReceivedDataStructure<cuav::equipment::power::CBAT> &) >
CBATCbBinder;
uavcan::Subscriber<uavcan::equipment::power::BatteryInfo, BatteryInfoCbBinder> _sub_battery;
uavcan::Subscriber<ardupilot::equipment::power::BatteryInfoAux, BatteryInfoAuxCbBinder> _sub_battery_aux;
uavcan::Subscriber<cuav::equipment::power::CBAT, CBATCbBinder> _sub_cbat;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
src/drivers/uavcan/uavcan_params.c
+2 -1
View File
@@ -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