diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 2835ef2457..af1d82a566 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/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; diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp index 91f462ad38..e818c66b7f 100644 --- a/src/drivers/uavcannode/UavcanNode.hpp +++ b/src/drivers/uavcannode/UavcanNode.hpp @@ -69,6 +69,7 @@ #include #include +#include #include #include #include @@ -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)};