fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan: fix battery sub not working

Browse Source
This commit is contained in:
alexcekay 2026-03-24 17:21:16 +01:00 committed by Alexander Lerach
parent 710adefb4c
commit d24d4a4fc4
2 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/drivers/uavcan/sensors/battery.cpp
View File

@ -95,7 +95,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
uint8_t instance = 0; uint8_t instance = 0;
for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) { for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
if (_battery_status[instance].id == msg.getSrcNodeID().get() || _battery_status[instance].id == 0) { if (_node_ids[instance] == msg.getSrcNodeID().get() || _node_ids[instance] == 0) {
break; break;
} }
} }
@ -110,6 +110,8 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
return; return;
} }
_node_ids[instance] = msg.getSrcNodeID().get();
if (_batt_update_mod[instance] == BatteryDataType::Filter) { if (_batt_update_mod[instance] == BatteryDataType::Filter) {
filterData(msg, instance); filterData(msg, instance);
@ -166,7 +168,7 @@ UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardu
uint8_t instance = 0; uint8_t instance = 0;
for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) { for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
if (_battery_status[instance].id == msg.getSrcNodeID().get()) { if (_node_ids[instance] == msg.getSrcNodeID().get()) {
break; break;
} }
} }
@ -212,7 +214,7 @@ void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::
uint8_t instance = 0; uint8_t instance = 0;
for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) { for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
if (_battery_status[instance].id == msg.getSrcNodeID().get()) { if (_node_ids[instance] == msg.getSrcNodeID().get() || _node_ids[instance] == 0) {
break; break;
} }
} }
@ -247,6 +249,7 @@ void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::
_battery_status[instance].connected = true; _battery_status[instance].connected = true;
_battery_status[instance].cell_count = msg.cell_count; _battery_status[instance].cell_count = msg.cell_count;
_battery_status[instance].source = battery_status_s::SOURCE_EXTERNAL; _battery_status[instance].source = battery_status_s::SOURCE_EXTERNAL;
_node_ids[instance] = msg.getSrcNodeID().get();
_battery_status[instance].id = msg.getSrcNodeID().get(); _battery_status[instance].id = msg.getSrcNodeID().get();
_battery_status[instance].is_powering_off = msg.is_powering_off; _battery_status[instance].is_powering_off = msg.is_powering_off;
@ -293,7 +296,7 @@ void UavcanBatteryBridge::cbat_sub_cb(const uavcan::ReceivedDataStructure<cuav::
if (_node_info_publisher != nullptr) { if (_node_info_publisher != nullptr) {
_node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), _node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(),
_battery_status[instance].id, NodeInfoPublisher::DeviceCapability::BATTERY); _node_ids[instance], NodeInfoPublisher::DeviceCapability::BATTERY);
} }
} }
@ -309,7 +312,7 @@ UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equi
/* Override data that is expected to arrive from UAVCAN msg */ /* Override data that is expected to arrive from UAVCAN msg */
_battery_status[instance] = _battery[instance]->getBatteryStatus(); _battery_status[instance] = _battery[instance]->getBatteryStatus();
_battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius _battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
_battery_status[instance].id = msg.getSrcNodeID().get(); // overwrite zeroed index from _battery _battery_status[instance].id = msg.battery_id;
publish(msg.getSrcNodeID().get(), &_battery_status[instance]); publish(msg.getSrcNodeID().get(), &_battery_status[instance]);

1
src/drivers/uavcan/sensors/battery.hpp
View File

@ -110,6 +110,7 @@ private:
battery_info_s _battery_info[battery_status_s::MAX_INSTANCES] {}; battery_info_s _battery_info[battery_status_s::MAX_INSTANCES] {};
battery_status_s _battery_status[battery_status_s::MAX_INSTANCES] {}; battery_status_s _battery_status[battery_status_s::MAX_INSTANCES] {};
BatteryDataType _batt_update_mod[battery_status_s::MAX_INSTANCES] {}; BatteryDataType _batt_update_mod[battery_status_s::MAX_INSTANCES] {};
uint8_t _node_ids[battery_status_s::MAX_INSTANCES] {}; // UAVCAN node ID per instance
static constexpr int FILTER_DATA = 2; static constexpr int FILTER_DATA = 2;
static constexpr int BATTERY_INDEX_1 = 1; static constexpr int BATTERY_INDEX_1 = 1;