fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 14:18:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan: add node status logging to uORB (#23890)

Browse Source 
Co-authored-by: Jordan Leiber <jordan.leiber@vertiq.co>
This commit is contained in:
Jacob Dahl
2025-07-22 23:17:58 -08:00
committed by GitHub
co-authored by Jordan Leiber
parent ce57153ce5
commit af878cee7b
6 changed files with 158 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/uavcan_main.cpp
+97 -33
View File
@@ -746,40 +746,9 @@ UavcanNode::Run()
_node.spinOnce(); // expected to be non-blocking
// Publish status
constexpr hrt_abstime status_pub_interval = 100_ms;
publish_can_interface_statuses();
if (hrt_absolute_time() - _last_can_status_pub >= status_pub_interval) {
_last_can_status_pub = hrt_absolute_time();
for (int i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
if (i > UAVCAN_NUM_IFACES) {
break;
}
auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
if (!iface) {
continue;
}
auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
can_interface_status_s status{
.timestamp = hrt_absolute_time(),
.io_errors = iface_perf_cnt.errors,
.frames_tx = iface_perf_cnt.frames_tx,
.frames_rx = iface_perf_cnt.frames_rx,
.interface = static_cast<uint8_t>(i),
};
if (_can_status_pub_handles[i] == nullptr) {
int instance{0};
_can_status_pub_handles[i] = orb_advertise_multi(ORB_ID(can_interface_status), nullptr, &instance);
}
(void)orb_publish(ORB_ID(can_interface_status), _can_status_pub_handles[i], &status);
}
}
publish_node_statuses();
// check for parameter updates
if (_parameter_update_sub.updated()) {
@@ -1011,6 +980,101 @@ UavcanNode::Run()
}
}
void UavcanNode::publish_can_interface_statuses()
{
constexpr hrt_abstime status_pub_interval = 100_ms;
if (hrt_absolute_time() - _last_can_status_pub >= status_pub_interval) {
_last_can_status_pub = hrt_absolute_time();
for (int i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
if (i > UAVCAN_NUM_IFACES) {
break;
}
auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
if (!iface) {
continue;
}
auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
can_interface_status_s status{
.timestamp = hrt_absolute_time(),
.io_errors = iface_perf_cnt.errors,
.frames_tx = iface_perf_cnt.frames_tx,
.frames_rx = iface_perf_cnt.frames_rx,
.interface = static_cast<uint8_t>(i),
};
if (_can_status_pub_handles[i] == nullptr) {
int instance{0};
_can_status_pub_handles[i] = orb_advertise_multi(ORB_ID(can_interface_status), nullptr, &instance);
}
(void)orb_publish(ORB_ID(can_interface_status), _can_status_pub_handles[i], &status);
}
}
}
void UavcanNode::publish_node_statuses()
{
constexpr hrt_abstime status_pub_interval = 100_ms;
if (hrt_absolute_time() - _last_node_status_pub >= status_pub_interval) {
_last_node_status_pub = hrt_absolute_time();
_node_status_monitor.forEachNode([this](uavcan::NodeID node_id, uavcan::NodeStatusMonitor::NodeStatus node_status) {
if (node_id.get() == 0) {
return;
}
// See if we have NodeID <--> uORB_index mapped
int uorb_index = -1;
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (_node_status_uorb_index_map[i] == node_id.get()) {
uorb_index = i;
break;
}
}
if (uorb_index < 0) {
// use next available index
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (_node_status_uorb_index_map[i] == 0) {
_node_status_uorb_index_map[i] = node_id.get();
uorb_index = i;
// advertise
PX4_INFO("advertising node_id %u on index %u", node_id.get(), i);
int instance{0};
_node_status_pub_handles[i] = orb_advertise_multi(ORB_ID(dronecan_node_status), nullptr, &instance);
break;
}
}
}
if (uorb_index >= 0) {
dronecan_node_status_s status{
.timestamp = hrt_absolute_time(),
.uptime_sec = node_status.uptime_sec,
.node_id = node_id.get(),
.vendor_specific_status_code = node_status.vendor_specific_status_code,
.health = node_status.health,
.mode = node_status.mode,
.sub_mode = node_status.sub_mode,
};
(void)orb_publish(ORB_ID(dronecan_node_status), _node_status_pub_handles[uorb_index], &status);
}
});
}
}
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
bool UavcanMixingInterfaceESC::updateOutputs(uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)