diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index ccf587980f..d62d43dfb2 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -95,7 +95,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructureregisterDeviceCapability(msg.getSrcNodeID().get(), - _battery_status[instance].id, NodeInfoPublisher::DeviceCapability::BATTERY); + _node_ids[instance], NodeInfoPublisher::DeviceCapability::BATTERY); } } @@ -306,10 +309,10 @@ UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructureupdateCurrent(msg.current); _battery[instance]->updateBatteryStatus(hrt_absolute_time()); - /* 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].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]); diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp index e0e050e9c4..84dc075227 100644 --- a/src/drivers/uavcan/sensors/battery.hpp +++ b/src/drivers/uavcan/sensors/battery.hpp @@ -110,6 +110,7 @@ private: battery_info_s _battery_info[battery_status_s::MAX_INSTANCES] {}; battery_status_s _battery_status[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 BATTERY_INDEX_1 = 1;