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

UAVCAN: fix message definition issues (#25809)

Browse Source
This commit is contained in:
Claudio Chies
2025-11-03 09:34:54 +01:00
committed by GitHub
parent f3ee45b173
commit cfe4cc82ea
1 changed files with 46 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcannode/Publishers/BatteryInfo.hpp
+46 -15
View File
@@ -36,6 +36,7 @@
#include "UavcanPublisherBase.hpp" #include "UavcanPublisherBase.hpp"
#include <uavcan/equipment/power/BatteryInfo.hpp> #include <uavcan/equipment/power/BatteryInfo.hpp>
#include <ardupilot/equipment/power/BatteryInfoAux.hpp>
#include <uORB/SubscriptionCallback.hpp> #include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/battery_status.h> #include <uORB/topics/battery_status.h>
@@ -47,15 +48,18 @@ namespace uavcannode
class BatteryInfo : class BatteryInfo :
public UavcanPublisherBase, public UavcanPublisherBase,
private uORB::SubscriptionCallbackWorkItem, 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: public:
BatteryInfo(px4::WorkItem *work_item, uavcan::INode &node) : BatteryInfo(px4::WorkItem *work_item, uavcan::INode &node) :
UavcanPublisherBase(uavcan::equipment::power::BatteryInfo::DefaultDataTypeID), UavcanPublisherBase(uavcan::equipment::power::BatteryInfo::DefaultDataTypeID),
uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(battery_status)), 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 void PrintInfo() override
@@ -65,36 +69,63 @@ public:
uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, uORB::SubscriptionCallbackWorkItem::get_topic()->o_name,
uavcan::equipment::power::BatteryInfo::getDataTypeFullName(), uavcan::equipment::power::BatteryInfo::getDataTypeFullName(),
uavcan::equipment::power::BatteryInfo::DefaultDataTypeID); 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 void BroadcastAnyUpdates() override
{ {
// battery_status -> uavcan::equipment::power::BatteryInfo // battery_status -> uavcan::equipment::power::BatteryInfo & ardupilot::equipment::power::BatteryInfoAux
battery_status_s battery; battery_status_s battery;
if (uORB::SubscriptionCallbackWorkItem::update(&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{}; uavcan::equipment::power::BatteryInfo battery_info{};
battery_info.voltage = battery.voltage_v; 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.temperature = battery.temperature - atmosphere::kAbsoluteNullCelsius; // convert from C to K
battery_info.full_charge_capacity_wh = battery.capacity; battery_info.full_charge_capacity_wh = battery.full_charge_capacity_wh;
battery_info.remaining_capacity_wh = battery.remaining * battery.capacity; battery_info.remaining_capacity_wh = battery.remaining_capacity_wh;
battery_info.state_of_charge_pct = battery.remaining * 100; battery_info.state_of_charge_pct = battery.remaining * 100.0f;
battery_info.state_of_charge_pct_stdev = battery.max_error; 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_instance_id = 0; // TODO: what goes here?
battery_info.model_name = "ARK BMS Rev 0.2"; battery_info.model_name = "PX4 CAN Battery";
battery_info.battery_id = battery.serial_number;
battery_info.hours_to_full_charge = 0; // TODO: Read BQ40Z80_TIME_TO_FULL // State of health
battery_info.state_of_health_pct = battery.state_of_health; battery_info.state_of_health_pct = battery.state_of_health;
if (battery.current_a > 0.0f) { uint8_t status_flags = 0;
battery_info.status_flags = uavcan::equipment::power::BatteryInfo::STATUS_FLAG_CHARGING;
} else { if (battery.connected) {
battery_info.status_flags = uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE; 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); uavcan::Publisher<uavcan::equipment::power::BatteryInfo>::broadcast(battery_info);
// ensure callback is registered // ensure callback is registered