diff --git a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp index 0e15b8f91e..e0ea2d0aff 100644 --- a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp +++ b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp @@ -94,7 +94,6 @@ public: bat_status.connected = bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_IN_USE; bat_status.source = 1; // External bat_status.capacity = bat_info.full_charge_capacity_wh; - bat_status.serial_number = bat_info.model_instance_id & 0xFFFF; // Take first 16 bits bat_status.state_of_health = bat_info.state_of_health_pct; // External bat_status.id = bat_info.battery_id; @@ -118,9 +117,17 @@ public: _battery_status_pub.publish(bat_status); print_message(ORB_ID(battery_status), bat_status); + + battery_info_s battery_info{}; + battery_info.timestamp = bat_status.timestamp; + battery_info.id = bat_status.id; + snprintf(battery_info.serial_number, sizeof(battery_info.serial_number), "%" PRIu32, + bat_info.model_instance_id); + _battery_info_pub.publish(battery_info); }; private: + uORB::PublicationMulti _battery_info_pub{ORB_ID(battery_info)}; uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; }; diff --git a/src/drivers/cyphal/Subscribers/udral/Battery.hpp b/src/drivers/cyphal/Subscribers/udral/Battery.hpp index 0a8a6c798e..310bc97663 100644 --- a/src/drivers/cyphal/Subscribers/udral/Battery.hpp +++ b/src/drivers/cyphal/Subscribers/udral/Battery.hpp @@ -41,6 +41,7 @@ #pragma once +#include #include #include @@ -120,6 +121,10 @@ public: // TODO uORB publication rate limiting _battery_status_pub.publish(bat_status); + _battery_info.timestamp = bat_status.timestamp; + _battery_info.id = bat_status.id; + _battery_info_pub.publish(_battery_info); + } else if (receive.metadata.port_id == _status_sub._canard_sub.port_id) { reg_udral_service_battery_Status_0_2 bat {}; size_t bat_size_in_bytes = receive.payload_size; @@ -163,7 +168,7 @@ public: bat_status.capacity = parameters.design_capacity.coulomb / 3.6f; // Coulomb -> mAh bat_status.cycle_count = parameters.cycle_count; - bat_status.serial_number = parameters.unique_id & 0xFFFF; + snprintf(_battery_info.serial_number, sizeof(_battery_info.serial_number), "%" PRIu64, parameters.unique_id); bat_status.state_of_health = parameters.state_of_health_pct; bat_status.max_error = 1; // UAVCAN didn't spec'ed this, but we're optimistic bat_status.id = 0; //TODO instancing @@ -174,6 +179,7 @@ public: private: float _nominal_voltage = NAN; + uORB::PublicationMulti _battery_info_pub{ORB_ID(battery_info)}; uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; SubjectSubscription _status_sub; @@ -182,5 +188,6 @@ private: const char *_status_name = "battery_status"; const char *_parameters_name = "battery_parameters"; + battery_info_s _battery_info{}; battery_status_s bat_status {0}; };