Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-02-06 03:13:00 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

tiny uavcan optimizations (#26344)

Browse Source 
Reduce number of hrt_absolute_time() calls and node_id.get()
This commit is contained in:
Nick
2026-01-27 09:06:18 +01:00
committed by GitHub
parent 01d8113f8b
commit c2490e01a5
1 changed files with 13 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
src/drivers/uavcan/uavcan_main.cpp
View File

@@ -994,9 +994,10 @@ UavcanNode::Run()
void UavcanNode::publish_can_interface_statuses()
{
constexpr hrt_abstime status_pub_interval = 100_ms;
const hrt_abstime now = hrt_absolute_time();
if (hrt_absolute_time() - _last_can_status_pub >= status_pub_interval) {
_last_can_status_pub = hrt_absolute_time();
if (now - _last_can_status_pub >= status_pub_interval) {
_last_can_status_pub = now;
for (int i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
if (i > UAVCAN_NUM_IFACES) {
@@ -1011,7 +1012,7 @@ void UavcanNode::publish_can_interface_statuses()
auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
can_interface_status_s status{
.timestamp = hrt_absolute_time(),
.timestamp = now,
.io_errors = iface_perf_cnt.errors,
.frames_tx = iface_perf_cnt.frames_tx,
.frames_rx = iface_perf_cnt.frames_rx,
@@ -1031,13 +1032,15 @@ void UavcanNode::publish_can_interface_statuses()
void UavcanNode::publish_node_statuses()
{
constexpr hrt_abstime status_pub_interval = 100_ms;
const hrt_abstime now = hrt_absolute_time();
if (hrt_absolute_time() - _last_node_status_pub >= status_pub_interval) {
_last_node_status_pub = hrt_absolute_time();
if (now - _last_node_status_pub >= status_pub_interval) {
_last_node_status_pub = now;
_node_status_monitor.forEachNode([this](uavcan::NodeID node_id, uavcan::NodeStatusMonitor::NodeStatus node_status) {
auto node_id_val = node_id.get();
if (node_id.get() == 0) {
if (node_id_val == 0) {
return;
}
@@ -1045,7 +1048,7 @@ void UavcanNode::publish_node_statuses()
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()) {
if (_node_status_uorb_index_map[i] == node_id_val) {
uorb_index = i;
break;
}
@@ -1055,10 +1058,10 @@ void UavcanNode::publish_node_statuses()
// 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();
_node_status_uorb_index_map[i] = node_id_val;
uorb_index = i;
// advertise
PX4_INFO("advertising node_id %u on index %u", node_id.get(), i);
PX4_INFO("advertising node_id %u on index %u", node_id_val, i);
int instance{0};
_node_status_pub_handles[i] = orb_advertise_multi(ORB_ID(dronecan_node_status), nullptr, &instance);
break;
@@ -1071,7 +1074,7 @@ void UavcanNode::publish_node_statuses()
dronecan_node_status_s status{
.timestamp = hrt_absolute_time(),
.uptime_sec = node_status.uptime_sec,
.node_id = node_id.get(),
.node_id = node_id_val,
.vendor_specific_status_code = node_status.vendor_specific_status_code,
.health = node_status.health,
.mode = node_status.mode,