uavcannode: battery info publishing

src/drivers/uavcannode/UavcanNode.cpp

@@ -322,6 +322,36 @@ void UavcanNode::Run()
 		PX4_ERR("node spin error %i", spin_res);
 	}
 
+	// battery_status -> uavcan::equipment::power::BatteryInfo
+	if (_battery_status_sub.updated()) {
+		battery_status_s battery;
+
+		if (_battery_status_sub.copy(&battery)) {
+			uavcan::equipment::power::BatteryInfo battery_info{};
+			battery_info.voltage = battery.voltage_v;
+			battery_info.current = fabs(battery.current_a);
+			battery_info.temperature = battery.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // 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.state_of_charge_pct_stdev = battery.max_error;
+			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.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;
+			}
+			else {
+				battery_info.status_flags = uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE;
+			}
+
+			_power_battery_info_publisher.broadcast(battery_info);
+		}
+	}
+
 	// sensor_baro -> uavcan::equipment::air_data::StaticTemperature
 	if (_sensor_baro_sub.updated()) {
 		sensor_baro_s baro;

src/drivers/uavcannode/UavcanNode.hpp

@@ -69,6 +69,7 @@
 
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/battery_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_mag.h>
@@ -175,6 +176,7 @@ private:
 
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 
+	uORB::SubscriptionCallbackWorkItem _battery_status_sub{this, ORB_ID(battery_status)};
 	uORB::SubscriptionCallbackWorkItem _sensor_baro_sub{this, ORB_ID(sensor_baro)};
 	uORB::SubscriptionCallbackWorkItem _sensor_mag_sub{this, ORB_ID(sensor_mag)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_gps_position_sub{this, ORB_ID(vehicle_gps_position)};