fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 46 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
src/drivers/uavcannode/Publishers/BatteryInfo.hpp
View File

@ -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