From cfe4cc82eae25d25bb1382b2e392d2bcfb164f13 Mon Sep 17 00:00:00 2001
From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com>
Date: Mon, 3 Nov 2025 09:34:54 +0100
Subject: [PATCH] UAVCAN: fix message definition issues (#25809)

---
 .../uavcannode/Publishers/BatteryInfo.hpp     | 61 ++++++++++++++-----
 1 file changed, 46 insertions(+), 15 deletions(-)

diff --git a/src/drivers/uavcannode/Publishers/BatteryInfo.hpp b/src/drivers/uavcannode/Publishers/BatteryInfo.hpp
index 686dc05086..7cd45d6f2a 100644
--- a/src/drivers/uavcannode/Publishers/BatteryInfo.hpp
+++ b/src/drivers/uavcannode/Publishers/BatteryInfo.hpp
@@ -36,6 +36,7 @@
 #include "UavcanPublisherBase.hpp"
 
 #include <uavcan/equipment/power/BatteryInfo.hpp>
+#include <ardupilot/equipment/power/BatteryInfoAux.hpp>
 
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/battery_status.h>
@@ -47,15 +48,18 @@ namespace uavcannode
 class BatteryInfo :
 	public UavcanPublisherBase,
 	private uORB::SubscriptionCallbackWorkItem,
-	private uavcan::Publisher<uavcan::equipment::power::BatteryInfo>
+	private uavcan::Publisher<uavcan::equipment::power::BatteryInfo>,
+	private uavcan::Publisher<ardupilot::equipment::power::BatteryInfoAux>
 {
 public:
 	BatteryInfo(px4::WorkItem *work_item, uavcan::INode &node) :
 		UavcanPublisherBase(uavcan::equipment::power::BatteryInfo::DefaultDataTypeID),
 		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(battery_status)),
-		uavcan::Publisher<uavcan::equipment::power::BatteryInfo>(node)
+		uavcan::Publisher<uavcan::equipment::power::BatteryInfo>(node),
+		uavcan::Publisher<ardupilot::equipment::power::BatteryInfoAux>(node)
 	{
-		this->setPriority(uavcan::TransferPriority::MiddleLower);
+		uavcan::Publisher<uavcan::equipment::power::BatteryInfo>::setPriority(uavcan::TransferPriority::MiddleLower);
+		uavcan::Publisher<ardupilot::equipment::power::BatteryInfoAux>::setPriority(uavcan::TransferPriority::MiddleLower);
 	}
 
 	void PrintInfo() override
@@ -65,36 +69,63 @@ public:
 			       uORB::SubscriptionCallbackWorkItem::get_topic()->o_name,
 			       uavcan::equipment::power::BatteryInfo::getDataTypeFullName(),
 			       uavcan::equipment::power::BatteryInfo::DefaultDataTypeID);
+			printf("\t%s -> %s:%d\n",
+			       uORB::SubscriptionCallbackWorkItem::get_topic()->o_name,
+			       ardupilot::equipment::power::BatteryInfoAux::getDataTypeFullName(),
+			       ardupilot::equipment::power::BatteryInfoAux::DefaultDataTypeID);
 		}
 	}
 
 	void BroadcastAnyUpdates() override
 	{
-		// battery_status -> uavcan::equipment::power::BatteryInfo
+		// battery_status -> uavcan::equipment::power::BatteryInfo & ardupilot::equipment::power::BatteryInfoAux
 		battery_status_s battery;
 
 		if (uORB::SubscriptionCallbackWorkItem::update(&battery)) {
+			ardupilot::equipment::power::BatteryInfoAux battery_info_aux{};
+
+			battery_info_aux.timestamp.usec = battery.timestamp;
+
+			for (uint8_t i = 0; i < battery.cell_count && i < battery.voltage_cell_v.size(); i++) {
+				battery_info_aux.voltage_cell.push_back(battery.voltage_cell_v[i]);
+			}
+
+			battery_info_aux.cycle_count = battery.cycle_count;
+			battery_info_aux.over_discharge_count = battery.over_discharge_count;
+			battery_info_aux.max_current = battery.current_a;
+			battery_info_aux.nominal_voltage = battery.nominal_voltage;
+			battery_info_aux.is_powering_off = battery.is_powering_off;
+			battery_info_aux.battery_id = battery.id;
+
+			uavcan::Publisher<ardupilot::equipment::power::BatteryInfoAux>::broadcast(battery_info_aux);
+
 			uavcan::equipment::power::BatteryInfo battery_info{};
 			battery_info.voltage = battery.voltage_v;
-			battery_info.current = fabs(battery.current_a);
+			battery_info.current = battery.current_a;
 			battery_info.temperature = battery.temperature - atmosphere::kAbsoluteNullCelsius; // convert from C to K
-			battery_info.full_charge_capacity_wh = battery.capacity;
-			battery_info.remaining_capacity_wh = battery.remaining * battery.capacity;
-			battery_info.state_of_charge_pct = battery.remaining * 100;
+			battery_info.full_charge_capacity_wh = battery.full_charge_capacity_wh;
+			battery_info.remaining_capacity_wh = battery.remaining_capacity_wh;
+			battery_info.state_of_charge_pct = battery.remaining * 100.0f;
 			battery_info.state_of_charge_pct_stdev = battery.max_error;
+			battery_info.battery_id = battery.id;
 			battery_info.model_instance_id = 0; // TODO: what goes here?
-			battery_info.model_name = "ARK BMS Rev 0.2";
-			battery_info.battery_id = battery.serial_number;
-			battery_info.hours_to_full_charge = 0; // TODO: Read BQ40Z80_TIME_TO_FULL
+			battery_info.model_name = "PX4 CAN Battery";
+
+			// State of health
 			battery_info.state_of_health_pct = battery.state_of_health;
 
-			if (battery.current_a > 0.0f) {
-				battery_info.status_flags = uavcan::equipment::power::BatteryInfo::STATUS_FLAG_CHARGING;
+			uint8_t status_flags = 0;
 
-			} else {
-				battery_info.status_flags = uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE;
+			if (battery.connected) {
+				status_flags |= uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE;
 			}
 
+			if (battery.warning == battery_status_s::STATE_CHARGING) {
+				status_flags |= uavcan::equipment::power::BatteryInfo::STATUS_FLAG_CHARGING;
+			}
+
+			battery_info.status_flags = status_flags;
+
 			uavcan::Publisher<uavcan::equipment::power::BatteryInfo>::broadcast(battery_info);
 
 			// ensure callback is registered