diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index 0c8528670f..0198749f73 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -125,7 +125,6 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructuregetBatteryStatus(); _battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius - _battery_status[instance].serial_number = msg.model_instance_id; _battery_status[instance].id = msg.getSrcNodeID().get(); // overwrite zeroed index from _battery 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]); } diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp index a27ad64505..cba57acfe7 100644 --- a/src/drivers/uavcan/sensors/battery.hpp +++ b/src/drivers/uavcan/sensors/battery.hpp @@ -38,6 +38,7 @@ #pragma once #include "sensor_bridge.hpp" +#include #include #include #include @@ -95,6 +96,11 @@ private: float _discharged_mah_loop = 0.f; uint8_t _warning; hrt_abstime _last_timestamp; + + // Separate battery info publication because UavcanSensorBridgeBase only supports publishing one topic + uORB::PublicationMulti _battery_info_pub[battery_status_s::MAX_INSTANCES] {ORB_ID(battery_info), ORB_ID(battery_info), ORB_ID(battery_info), ORB_ID(battery_info)}; + + 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] {};