diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index a46c398801..3fe3dac64c 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -39,10 +39,10 @@ uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::N node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); { - const auto machine_id = uavcan_linux::MachineIDReader().read(); + const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, nid.get()); uavcan::protocol::HardwareVersion hwver; - std::copy(machine_id.begin(), machine_id.end(), hwver.unique_id.begin()); + std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin()); std::cout << hwver << std::endl; node->setHardwareVersion(hwver); @@ -498,7 +498,6 @@ void runForever(const uavcan_linux::NodePtr& node, * Storage backend */ uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend; - ENFORCE(0 <= storage_backend.init(persistent_storage_path.c_str())); /* @@ -506,7 +505,16 @@ void runForever(const uavcan_linux::NodePtr& node, */ uavcan::dynamic_node_id_server::DistributedServer server(*node, storage_backend, event_tracer); - ENFORCE(0 <= server.init(node->getNodeStatusProvider().getHardwareVersion().unique_id, cluster_size)); + const int server_init_res = server.init(node->getNodeStatusProvider().getHardwareVersion().unique_id, cluster_size); + if (server_init_res < 0) + { + throw std::runtime_error("Failed to start the server; error " + std::to_string(server_init_res)); + } + + /* + * Preparing the CLI + */ + std::printf("\x1b[2J"); // Clear entire screen; this will preserve initialization output in the scrollback /* * Spinning the node diff --git a/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp b/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp index 14aa91ad63..b4d6ce0a5c 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp @@ -14,6 +14,7 @@ #include #include #include +#include namespace uavcan_linux { @@ -132,5 +133,45 @@ public: } }; +/** + * This class computes unique ID for a UAVCAN node in a Linux application. + * It takes the following inputs: + * - Unique machine ID + * - Node name string (e.g. "org.uavcan.linux_app.dynamic_node_id_server") + * - Instance ID byte, e.g. node ID + */ +std::array makeApplicationID(const MachineIDReader::MachineID& machine_id, + const std::string& node_name, + const std::uint8_t instance_id) +{ + union HalfID + { + std::uint64_t num; + std::uint8_t bytes[8]; + + HalfID(std::uint64_t arg_num) : num(arg_num) { } + }; + + std::array out; + + // First 8 bytes of the application ID are CRC64 of the machine ID in native byte order + { + uavcan::DataTypeSignatureCRC crc; + crc.add(machine_id.data(), static_cast(machine_id.size())); + HalfID half(crc.get()); + std::copy_n(half.bytes, 8, out.begin()); + } + + // Last 8 bytes of the application ID are CRC64 of the node name and optionally node ID + { + uavcan::DataTypeSignatureCRC crc; + crc.add(reinterpret_cast(node_name.c_str()), static_cast(node_name.length())); + crc.add(instance_id); + HalfID half(crc.get()); + std::copy_n(half.bytes, 8, out.begin() + 8); + } + + return out; +} }