diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 505bf4272c..8961b70955 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -824,6 +824,27 @@ extern "C" int uavcannode_start(int argc, char *argv[]) } } + // Read the static node ID parameter and use it if no valid node_id from shared memory + int32_t cannode_node_id = 0; + param_get(param_find("CANNODE_NODE_ID"), &cannode_node_id); + + // Check if the static node ID is in range + if (cannode_node_id < 0 || cannode_node_id > 127) { + PX4_ERR("Invalid static node ID %ld, using dynamic allocation", cannode_node_id); + cannode_node_id = 0; + } + + // Assign the static node ID if no dynamic allocation is used. Do wen't override a valid node ID from the bootloader? + if (node_id == 0 && cannode_node_id > 0 && cannode_node_id <= 127) { + node_id = cannode_node_id; + } + + // Persist the node ID for the bootloader + bootloader_app_shared_t shared_write = {}; + shared_write.node_id = node_id; + shared_write.bus_speed = bitrate; + bootloader_app_shared_write(&shared_write, BootLoader); + if ( #if defined(SUPPORT_ALT_CAN_BOOTLOADER) board_booted_by_px4() && diff --git a/src/drivers/uavcannode/uavcannode_params.c b/src/drivers/uavcannode/uavcannode_params.c index 8546989954..6e7dc0bf28 100644 --- a/src/drivers/uavcannode/uavcannode_params.c +++ b/src/drivers/uavcannode/uavcannode_params.c @@ -31,6 +31,15 @@ * ****************************************************************************/ +/** + * UAVCAN CAN node ID (0 for dynamic allocation). + * + * @min 0 + * @max 127 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(CANNODE_NODE_ID, 0); + /** * UAVCAN CAN bus bitrate. *