fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-09 09:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcannode: battery info publishing

Browse Source
This commit is contained in:
Jacob Dahl
2020-04-02 23:30:31 -06:00
committed by GitHub
parent 0d8d4cd6e0
commit ff1e5a595d
2 changed files with 32 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcannode/UavcanNode.cpp
+30
View File
@@ -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
+2
View File
@@ -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)};