From 777eee8ae4ce48e5e2a73b4ced317475a7d6cb6e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 1 Apr 2015 00:48:54 +0300 Subject: [PATCH 01/16] Dynamic Node ID allocation message --- .../559.DynamicNodeIDAllocation.uavcan | 140 ++++++++++++++++++ 1 file changed, 140 insertions(+) create mode 100644 dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan diff --git a/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan b/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan new file mode 100644 index 0000000000..275c58ed62 --- /dev/null +++ b/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan @@ -0,0 +1,140 @@ +# +# This message is used for dynamic Node ID allocation. This algorithm is an +# optional extension to the UAVCAN specification, and it is not mandatory to +# support it. +# +# The dynamic Node ID allocation algorithm works as follows: +# +# - As a prerequisite, the network contains at least one node that has some +# Node ID assigned, that will handle allocation requests from other nodes. +# This node will be later referred to as Allocator. It is recognized that +# since UAVCAN is a decentralized network, there might be more than one +# Allocator, but only one of them should handle allocation requests at any +# given time. This issue is covered below in detail. +# +# - The network may also contain any number of nodes that seek to have +# Node IDs assigned. In order to get Node ID assigned, these nodes request +# the Allocator. Requesting nodes will be referred to as Allocatees. +# +# - In order to get a Node ID allocated, the Allocatee fills this message +# structure with its short unique ID (see below to learn how to compute one) +# and its preferred Node ID (which is basically the Node ID it would like to +# take, if possible). If the Allocatee doesn't have any preference, the +# preferred Node ID can be set to zero. +# +# - The Allocatee broadcasts the message to the bus. Since it doesn't have a +# Node ID at the moment, the field Source Node ID in the broadcasted message +# must be zeroed. Note that it may require a special logic in the transport +# layer to receive messages where Source Node ID is zeroed. +# +# - The Allocator receives the message and refers to its Node ID Table, where +# it looks for any unallocated Node ID value that equals or greater than the +# value that was requested by the Allocatee. If the desired Node ID is set +# to zero, or if all Node IDs that are larger than the preferred one are +# already taken, the Allocator traverses the Node ID Table from top to +# bottom (i.e. from high to low Node ID) until it finds first unallocated +# entry. In case if all available entires are taken, the Allocator ignores +# the request. Please note that it is recommended to restrict the range of +# dynamically assignable Node IDs, as described below. +# +# - The Allocator fills the response message. The short unique ID is set the +# same as in the request, the Node ID field is assigned the value obtained +# on the previous step. When filled, the message is broadcasted as usual, +# i.e. the field Source Node ID must be set to the actual Node ID of the +# Allocator. This allows other Allocators, if any, to understand that this +# message is not an allocation request, otherwise Source Node ID would be +# zeroed. +# +# - The Allocatee receives the response. Sice there can be other nodes trying +# to get a Node ID at the same time, the Allocatee filters other messages +# by means of comparing its own short unique ID with short unique ID in the +# messages passing by. When a message is received such that its short unique +# ID equals the short unique ID of the Allocatee, the Allocatee gets the +# allocated Node ID from the message, accepts it and continues its normal +# operation as a full-fledged UAVCAN node. +# +# - It is recommended that once Allocatee was granted a Node ID, it stops +# listening to other messages of this type and reconfigures its CAN +# acceptance filters accordingly. +# +# It has been mentioned above that a network may accomodate more than one +# Allocator at the time. In this case, the Allocators should agree between +# each other that only one of them is handling allocation requests at the +# moment. A possible way to reach such an agreement is to let one Allocator +# monitor the presence of the second one (e.g. by means of listening to its +# NodeStatus messages), so that the second Allocator will ignore allocation +# requests as long as the first one is alive. +# +# Every Allocator must continuously maintain a table that indicates which +# Node IDs are taken, and which are free. The logic is like that: immediately +# after initialization, only one Node ID is considered to be taken - the one +# that belongs to the Allocator itself. The allocator subscribes to the +# message uavcan.protocol.NodeStatus, which is then used to update the table +# in real time - once a NodeStatus message is received, the table is updated +# to indicate that the Node ID that sent the NodeStatus message is taken. The +# Allocator must not process allocation requests sooner than a few seconds +# after its initialization, in order to ensure that all nodes that exist in +# the network were able to transmit their NodeStatus message at least once. +# The exact duration of initialization delay is defined in a constant below. +# +# In order to minimize chances of Node ID collision between nodes that rely on +# the dynamic allocation and the nodes that use statically allocated Node IDs, +# it is recommended to restrict the range of dynamically allocatable Node IDs. +# Two constants below define a range for dynamically allocated Node ID. +# +# There is a limit that restricts the maximum frequency at which the Allocatee +# is allowed to broadcast allocation requests. It is defined in a constant +# below. +# +# Fields of this message are documented below. +# + +# +# This is the delay that Allocator must take to initialize its Node ID Table. +# +uint8 ALLOCATOR_INITIALIZATION_DELAY_SEC = 30 + +# +# This is the maximum frequency at which the Allocatee can broadcast +# allocation requests. +# +uint8 ALLOCATEE_MAX_BROADCAST_INTERVAL_SEC = 1 + +# +# It is recommended to pick Node IDs from this range only, even if the +# Allocatee requests a Node ID that is outside of this range. +# +uint7 RECOMMENDED_DYNAMIC_NODE_ID_RANGE_MIN = 64 +uint7 RECOMMENDED_DYNAMIC_NODE_ID_RANGE_MAX = 125 + +# +# This field contains 57 bits that uniquely identify the node that requests +# Node ID allocation. The specification does not define exact algorithm for +# computing short unique ID, but it is recommended to stick to the following +# recommendations: +# +# 1. If the node's unique ID is 64 bit large, lower 57 bits should be used. +# +# 2. If the node's unique ID is larger than 64 bits, it should be reduced +# to 64 bit using the CRC-64-WE hash function (the same function that is +# used in DSDL signature computation, see specification for details), and +# the result should be processed as desribed in the first item above. +# +# Many microcontrollers provide vendor-assigned unique product IDs that are +# typically 12 to 16 bytes long (e.g. STM32, LPC11), which should be employed +# as described in the second item above. +# +# A node is not allowed to use this algorithm if it doesn't have a unique ID +# that is 57 or more bits long. +# +truncated uint57 short_unique_id + +# +# In the request, this field should contain Node ID that the requesting node +# would like to take. The allocator then may choose to provide the requested +# Node ID, or to assign a higher one, or to disregard this preference +# completely. It is recommended that the allocator always tries to provide +# Node ID that is equal or higher than requested one, as it would allow nodes +# to get Node ID according to their desired priority on the bus. +# +truncated uint7 node_id From 8bb90afb0fdd58d2beb1c6d8a8808e749f0dca38 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 1 Apr 2015 03:03:19 +0300 Subject: [PATCH 02/16] DynamicNodeIDAllocation updated according to David's input --- .../559.DynamicNodeIDAllocation.uavcan | 32 ++++++++++--------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan b/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan index 275c58ed62..1f2430ff32 100644 --- a/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan +++ b/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan @@ -13,8 +13,9 @@ # given time. This issue is covered below in detail. # # - The network may also contain any number of nodes that seek to have -# Node IDs assigned. In order to get Node ID assigned, these nodes request -# the Allocator. Requesting nodes will be referred to as Allocatees. +# Node IDs assigned to them. In order to get a Node ID assigned, these nodes +# make a request to the Allocator. Requesting nodes will be referred to as +# Allocatees. # # - In order to get a Node ID allocated, the Allocatee fills this message # structure with its short unique ID (see below to learn how to compute one) @@ -24,16 +25,16 @@ # # - The Allocatee broadcasts the message to the bus. Since it doesn't have a # Node ID at the moment, the field Source Node ID in the broadcasted message -# must be zeroed. Note that it may require a special logic in the transport +# must be zero. Note that it may require special logic in the transport # layer to receive messages where Source Node ID is zeroed. # # - The Allocator receives the message and refers to its Node ID Table, where -# it looks for any unallocated Node ID value that equals or greater than the -# value that was requested by the Allocatee. If the desired Node ID is set -# to zero, or if all Node IDs that are larger than the preferred one are +# it looks for any unallocated Node ID value that equal to or greater than +# the value that was requested by the Allocatee. If the desired Node ID is +# set to zero, or if all Node IDs that are larger than the preferred one are # already taken, the Allocator traverses the Node ID Table from top to # bottom (i.e. from high to low Node ID) until it finds first unallocated -# entry. In case if all available entires are taken, the Allocator ignores +# entry. In a case if all available entires are taken, the Allocator ignores # the request. Please note that it is recommended to restrict the range of # dynamically assignable Node IDs, as described below. # @@ -43,7 +44,7 @@ # i.e. the field Source Node ID must be set to the actual Node ID of the # Allocator. This allows other Allocators, if any, to understand that this # message is not an allocation request, otherwise Source Node ID would be -# zeroed. +# zero. # # - The Allocatee receives the response. Sice there can be other nodes trying # to get a Node ID at the same time, the Allocatee filters other messages @@ -69,13 +70,14 @@ # Node IDs are taken, and which are free. The logic is like that: immediately # after initialization, only one Node ID is considered to be taken - the one # that belongs to the Allocator itself. The allocator subscribes to the -# message uavcan.protocol.NodeStatus, which is then used to update the table -# in real time - once a NodeStatus message is received, the table is updated -# to indicate that the Node ID that sent the NodeStatus message is taken. The -# Allocator must not process allocation requests sooner than a few seconds -# after its initialization, in order to ensure that all nodes that exist in -# the network were able to transmit their NodeStatus message at least once. -# The exact duration of initialization delay is defined in a constant below. +# messages uavcan.protocol.NodeStatus and this one, that are then used to +# update the table in real time - once a NodeStatus or this message are +# received, the table is updated to indicate that the Node ID that sent the +# NodeStatus message is taken. The Allocator must not process allocation +# requests sooner than a few seconds after its initialization, in order to +# ensure that all nodes that exist in the network were able to transmit their +# NodeStatus message at least once. The exact duration of initialization delay +# is defined in a constant below. # # In order to minimize chances of Node ID collision between nodes that rely on # the dynamic allocation and the nodes that use statically allocated Node IDs, From c48a22e1b18c1d40dd34520fa8f264621c073e1c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 1 Apr 2015 15:57:10 +0300 Subject: [PATCH 03/16] Minor updates to the DynamicNodeIDAllocation message --- .../protocol/559.DynamicNodeIDAllocation.uavcan | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan b/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan index 1f2430ff32..28dc90697c 100644 --- a/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan +++ b/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan @@ -97,10 +97,10 @@ uint8 ALLOCATOR_INITIALIZATION_DELAY_SEC = 30 # -# This is the maximum frequency at which the Allocatee can broadcast -# allocation requests. +# This is the minimum interval (maximum frequency) at which the Allocatee can +# broadcast allocation requests. # -uint8 ALLOCATEE_MAX_BROADCAST_INTERVAL_SEC = 1 +uint8 ALLOCATEE_MIN_BROADCAST_INTERVAL_SEC = 2 # # It is recommended to pick Node IDs from this range only, even if the @@ -129,6 +129,11 @@ uint7 RECOMMENDED_DYNAMIC_NODE_ID_RANGE_MAX = 125 # A node is not allowed to use this algorithm if it doesn't have a unique ID # that is 57 or more bits long. # +# Regarding the collision probability: the birthday paradox suggests that a +# network with n=30 nodes will experience a short unique ID collision with +# approximate probability of (n^2)/(2*(2^57-1)) = 3.1e-15, assuming that +# the ID reduction function is perfect. +# truncated uint57 short_unique_id # From ec3d5dd8be979493c35c15570a30a1235678521d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 2 Apr 2015 01:34:10 +0300 Subject: [PATCH 04/16] Adjusted priorities of file services --- ....BeginFirmwareUpdate.uavcan => 580.BeginFirmwareUpdate.uavcan} | 0 .../protocol/file/{580.GetInfo.uavcan => 585.GetInfo.uavcan} | 0 ...DirectoryEntryInfo.uavcan => 586.GetDirectoryEntryInfo.uavcan} | 0 .../uavcan/protocol/file/{582.Delete.uavcan => 587.Delete.uavcan} | 0 dsdl/uavcan/protocol/file/{583.Read.uavcan => 588.Read.uavcan} | 0 dsdl/uavcan/protocol/file/{584.Write.uavcan => 589.Write.uavcan} | 0 6 files changed, 0 insertions(+), 0 deletions(-) rename dsdl/uavcan/protocol/file/{589.BeginFirmwareUpdate.uavcan => 580.BeginFirmwareUpdate.uavcan} (100%) rename dsdl/uavcan/protocol/file/{580.GetInfo.uavcan => 585.GetInfo.uavcan} (100%) rename dsdl/uavcan/protocol/file/{581.GetDirectoryEntryInfo.uavcan => 586.GetDirectoryEntryInfo.uavcan} (100%) rename dsdl/uavcan/protocol/file/{582.Delete.uavcan => 587.Delete.uavcan} (100%) rename dsdl/uavcan/protocol/file/{583.Read.uavcan => 588.Read.uavcan} (100%) rename dsdl/uavcan/protocol/file/{584.Write.uavcan => 589.Write.uavcan} (100%) diff --git a/dsdl/uavcan/protocol/file/589.BeginFirmwareUpdate.uavcan b/dsdl/uavcan/protocol/file/580.BeginFirmwareUpdate.uavcan similarity index 100% rename from dsdl/uavcan/protocol/file/589.BeginFirmwareUpdate.uavcan rename to dsdl/uavcan/protocol/file/580.BeginFirmwareUpdate.uavcan diff --git a/dsdl/uavcan/protocol/file/580.GetInfo.uavcan b/dsdl/uavcan/protocol/file/585.GetInfo.uavcan similarity index 100% rename from dsdl/uavcan/protocol/file/580.GetInfo.uavcan rename to dsdl/uavcan/protocol/file/585.GetInfo.uavcan diff --git a/dsdl/uavcan/protocol/file/581.GetDirectoryEntryInfo.uavcan b/dsdl/uavcan/protocol/file/586.GetDirectoryEntryInfo.uavcan similarity index 100% rename from dsdl/uavcan/protocol/file/581.GetDirectoryEntryInfo.uavcan rename to dsdl/uavcan/protocol/file/586.GetDirectoryEntryInfo.uavcan diff --git a/dsdl/uavcan/protocol/file/582.Delete.uavcan b/dsdl/uavcan/protocol/file/587.Delete.uavcan similarity index 100% rename from dsdl/uavcan/protocol/file/582.Delete.uavcan rename to dsdl/uavcan/protocol/file/587.Delete.uavcan diff --git a/dsdl/uavcan/protocol/file/583.Read.uavcan b/dsdl/uavcan/protocol/file/588.Read.uavcan similarity index 100% rename from dsdl/uavcan/protocol/file/583.Read.uavcan rename to dsdl/uavcan/protocol/file/588.Read.uavcan diff --git a/dsdl/uavcan/protocol/file/584.Write.uavcan b/dsdl/uavcan/protocol/file/589.Write.uavcan similarity index 100% rename from dsdl/uavcan/protocol/file/584.Write.uavcan rename to dsdl/uavcan/protocol/file/589.Write.uavcan From 38c6a0d7ac6fec91d13115076989ac23efdb51cc Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 3 Apr 2015 00:26:34 +0300 Subject: [PATCH 05/16] Update 580.BeginFirmwareUpdate.uavcan Add ERROR_IN_PROGRESS --- dsdl/uavcan/protocol/file/580.BeginFirmwareUpdate.uavcan | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dsdl/uavcan/protocol/file/580.BeginFirmwareUpdate.uavcan b/dsdl/uavcan/protocol/file/580.BeginFirmwareUpdate.uavcan index 0f0713a7b2..ecc6228f43 100644 --- a/dsdl/uavcan/protocol/file/580.BeginFirmwareUpdate.uavcan +++ b/dsdl/uavcan/protocol/file/580.BeginFirmwareUpdate.uavcan @@ -1,5 +1,5 @@ # -# This service initiates a firmware update on a remote node. +# This service initiates firmware update on a remote node. # # The node that is being updated (slave) will retrieve the firmware image file 'image_file_remote_path' from the node # 'source_node_id' using the file read service, then it will update the firmware and reboot. Alternatively, this @@ -23,6 +23,7 @@ Path image_file_remote_path # Empty to invoke an alternative bootloader uint8 ERROR_OK = 0 uint8 ERROR_INVALID_MODE = 1 # Cannot perform the update right now (e.g. the vehicle is operating) +uint8 ERROR_IN_PROGRESS = 2 # Firmware update is already in progess, and the slave doesn't want to restart uint8 ERROR_UNKNOWN = 255 uint8 error From b4d93df4506ce5630fd2329605ae033dfdb43ac2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 5 Apr 2015 11:51:58 +0300 Subject: [PATCH 06/16] TransferSender is now capable of broadcasting in passive mode; Frame::isValid() was modified to accept SFT broadcasts with zero SNID --- .../include/uavcan/transport/dispatcher.hpp | 1 + libuavcan/include/uavcan/transport/frame.hpp | 2 +- .../uavcan/transport/transfer_sender.hpp | 9 ++++++ libuavcan/src/transport/uc_frame.cpp | 7 +++-- .../src/transport/uc_transfer_sender.cpp | 28 +++++++++++++++++-- libuavcan/test/transport/frame.cpp | 13 ++++++++- libuavcan/test/transport/transfer_sender.cpp | 15 +++++++++- 7 files changed, 67 insertions(+), 8 deletions(-) diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index ac12bbd2b7..a671f4ac85 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -115,6 +115,7 @@ public: : canio_(driver, allocator, sysclock) , sysclock_(sysclock) , outgoing_transfer_reg_(otr) + , self_node_id_(NodeID::Broadcast) // Default , self_node_id_is_set_(false) { } diff --git a/libuavcan/include/uavcan/transport/frame.hpp b/libuavcan/include/uavcan/transport/frame.hpp index 49d1da2214..5d149ba837 100644 --- a/libuavcan/include/uavcan/transport/frame.hpp +++ b/libuavcan/include/uavcan/transport/frame.hpp @@ -50,7 +50,7 @@ public: { UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast()); UAVCAN_ASSERT(data_type_id.isValid()); - UAVCAN_ASSERT(src_node_id != dst_node_id); + UAVCAN_ASSERT(src_node_id.isUnicast() ? (src_node_id != dst_node_id) : true); UAVCAN_ASSERT(frame_index <= MaxIndex); } diff --git a/libuavcan/include/uavcan/transport/transfer_sender.hpp b/libuavcan/include/uavcan/transport/transfer_sender.hpp index 27ab8a1038..0ffc232ebf 100644 --- a/libuavcan/include/uavcan/transport/transfer_sender.hpp +++ b/libuavcan/include/uavcan/transport/transfer_sender.hpp @@ -25,6 +25,7 @@ class UAVCAN_EXPORT TransferSender const TransferCRC crc_base_; CanIOFlags flags_; uint8_t iface_mask_; + bool allow_broadcasting_in_passive_mode_; Dispatcher& dispatcher_; @@ -46,6 +47,7 @@ public: , crc_base_(data_type.getSignature().toTransferCRC()) , flags_(CanIOFlags(0)) , iface_mask_(AllIfacesMask) + , allow_broadcasting_in_passive_mode_(false) , dispatcher_(dispatcher) { } @@ -59,6 +61,13 @@ public: iface_mask_ = iface_mask; } + /** + * By default, this class will return an error on any attempt to publish a message while the + * dispatcher is configured in passive mode. This method allows to permanently enable sending + * broadcast transfers in passive mode for this class instance. + */ + void allowBroadcastingInPassiveMode() { allow_broadcasting_in_passive_mode_ = true; } + /** * Send with explicit Transfer ID. * Should be used only for service responses, where response TID should match request TID. diff --git a/libuavcan/src/transport/uc_frame.cpp b/libuavcan/src/transport/uc_frame.cpp index 3a3938a01b..3a6f6ac814 100644 --- a/libuavcan/src/transport/uc_frame.cpp +++ b/libuavcan/src/transport/uc_frame.cpp @@ -170,9 +170,12 @@ bool Frame::isValid() const const bool invalid = (frame_index_ > MaxIndex) || ((frame_index_ == MaxIndex) && !last_frame_) || - (!src_node_id_.isUnicast()) || + (!src_node_id_.isValid()) || (!dst_node_id_.isValid()) || - (src_node_id_ == dst_node_id_) || + (src_node_id_.isUnicast() ? (src_node_id_ == dst_node_id_) : false) || + (src_node_id_.isBroadcast() + ? (!last_frame_ || (frame_index_ > 0) || (transfer_type_ != TransferTypeMessageBroadcast)) + : false) || ((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast()) || (transfer_type_ >= NumTransferTypes) || (static_cast(payload_len_) > getMaxPayloadLen()) || diff --git a/libuavcan/src/transport/uc_transfer_sender.cpp b/libuavcan/src/transport/uc_transfer_sender.cpp index 1fda8c6a46..38766ddc9b 100644 --- a/libuavcan/src/transport/uc_transfer_sender.cpp +++ b/libuavcan/src/transport/uc_transfer_sender.cpp @@ -19,15 +19,37 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id, TransferID tid) { + if (payload_len > MaxTransferPayloadLen) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + + Frame frame(data_type_.getID(), transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid); + UAVCAN_TRACE("TransferSender", "%s", frame.toString().c_str()); + + /* + * Checking if we're allowed to send. In passive mode we can send only if: + * - Passive broadcasting is enabled + * - Transfer type is broadcast + * - Transfer payload fits one CAN frame + */ if (dispatcher_.isPassiveMode()) { - return -ErrPassiveMode; + const bool allow = allow_broadcasting_in_passive_mode_ && + (transfer_type == TransferTypeMessageBroadcast) && + (int(payload_len) <= frame.getMaxPayloadLen()); + if (!allow) + { + return -ErrPassiveMode; + } } dispatcher_.getTransferPerfCounter().addTxTransfer(); - Frame frame(data_type_.getID(), transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid); - + /* + * Sending frames + */ if (frame.getMaxPayloadLen() >= int(payload_len)) // Single Frame Transfer { const int res = frame.setPayload(payload, payload_len); diff --git a/libuavcan/test/transport/frame.cpp b/libuavcan/test/transport/frame.cpp index 5f55033f91..719dcf325b 100644 --- a/libuavcan/test/transport/frame.cpp +++ b/libuavcan/test/transport/frame.cpp @@ -162,7 +162,18 @@ TEST(Frame, FrameParsing) can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression (2 << 0) | (1 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageUnicast << 17) | (456 << 19); - ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID + ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID with unicast transfer + + can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression + (2 << 0) | (0 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageBroadcast << 17) | (456 << 19); + ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID with multiframe broadcast transfer + + /* + * Broadcast SNID exceptions + */ + can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression + (2 << 0) | (1 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageBroadcast << 17) | (456 << 19); + ASSERT_TRUE(frame.parse(can)); // Broadcast Src Node ID with single frame broadcast transfer } diff --git a/libuavcan/test/transport/transfer_sender.cpp b/libuavcan/test/transport/transfer_sender.cpp index 5d65943511..4719543ec4 100644 --- a/libuavcan/test/transport/transfer_sender.cpp +++ b/libuavcan/test/transport/transfer_sender.cpp @@ -235,11 +235,24 @@ TEST(TransferSender, PassiveMode) static const uint8_t Payload[] = {1, 2, 3, 4, 5}; + // By default, sending in passive mode is not enabled ASSERT_EQ(-uavcan::ErrPassiveMode, sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast)); + // Overriding the default + sender.allowBroadcastingInPassiveMode(); + + // OK, now we can broadcast in any mode + ASSERT_LE(0, sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), + uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast)); + + // ...but not unicast or anything else + ASSERT_EQ(-uavcan::ErrPassiveMode, + sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), + uavcan::TransferTypeMessageUnicast, uavcan::NodeID(42))); + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount()); - EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(1, dispatcher.getTransferPerfCounter().getTxTransferCount()); EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount()); } From de33cf925055151b90fdc8236c10a65721c6f7e3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 6 Apr 2015 19:02:22 +0300 Subject: [PATCH 07/16] TransferListener now can accept rogue transfers --- .../uavcan/transport/transfer_listener.hpp | 15 +++++ .../src/transport/uc_transfer_listener.cpp | 57 ++++++++++++++----- .../test/transport/transfer_listener.cpp | 35 ++++++++++++ .../test/transport/transfer_test_helpers.hpp | 8 +++ 4 files changed, 101 insertions(+), 14 deletions(-) diff --git a/libuavcan/include/uavcan/transport/transfer_listener.hpp b/libuavcan/include/uavcan/transport/transfer_listener.hpp index e71d50f1e3..99f4efd11e 100644 --- a/libuavcan/include/uavcan/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -50,6 +50,11 @@ public: */ virtual void release() { } + /** + * Whether this is a rogue transfer + */ + virtual bool isRogueTransfer() const { return false; } + MonotonicTime getMonotonicTimestamp() const { return ts_mono_; } UtcTime getUtcTimestamp() const { return ts_utc_; } TransferType getTransferType() const { return transfer_type_; } @@ -68,6 +73,7 @@ class UAVCAN_EXPORT SingleFrameIncomingTransfer : public IncomingTransfer public: explicit SingleFrameIncomingTransfer(const RxFrame& frm); virtual int read(unsigned offset, uint8_t* data, unsigned len) const; + virtual bool isRogueTransfer() const; }; /** @@ -93,6 +99,7 @@ class UAVCAN_EXPORT TransferListenerBase : public LinkedListNode& receivers_; ITransferBufferManager& bufmgr_; TransferPerfCounter& perf_; + bool allow_rogue_transfers_; class TimedOutReceiverPredicate { @@ -119,17 +126,25 @@ protected: , receivers_(receivers) , bufmgr_(bufmgr) , perf_(perf) + , allow_rogue_transfers_(false) { } virtual ~TransferListenerBase() { } void handleReception(TransferReceiver& receiver, const RxFrame& frame, TransferBufferAccessor& tba); + void handleRogueTransferReception(const RxFrame& frame); virtual void handleIncomingTransfer(IncomingTransfer& transfer) = 0; public: const DataTypeDescriptor& getDataTypeDescriptor() const { return data_type_; } + /** + * By default, rogue transfers will be ignored. + * This option allows to enable reception of rogue transfers. + */ + void allowRogueTransfers() { allow_rogue_transfers_ = true; } + void cleanup(MonotonicTime ts); virtual void handleFrame(const RxFrame& frame); diff --git a/libuavcan/src/transport/uc_transfer_listener.cpp b/libuavcan/src/transport/uc_transfer_listener.cpp index d193977390..8cba8d6226 100644 --- a/libuavcan/src/transport/uc_transfer_listener.cpp +++ b/libuavcan/src/transport/uc_transfer_listener.cpp @@ -50,6 +50,11 @@ int SingleFrameIncomingTransfer::read(unsigned offset, uint8_t* data, unsigned l return int(len); } +bool SingleFrameIncomingTransfer::isRogueTransfer() const +{ + return (getTransferType() == TransferTypeMessageBroadcast) && getSrcNodeID().isBroadcast(); +} + /* * MultiFrameIncomingTransfer */ @@ -172,6 +177,16 @@ void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxF } } +void TransferListenerBase::handleRogueTransferReception(const RxFrame& frame) +{ + if (allow_rogue_transfers_) + { + perf_.addRxTransfer(); + SingleFrameIncomingTransfer it(frame); + handleIncomingTransfer(it); + } +} + void TransferListenerBase::cleanup(MonotonicTime ts) { receivers_.removeWhere(TimedOutReceiverPredicate(ts, bufmgr_)); @@ -180,26 +195,40 @@ void TransferListenerBase::cleanup(MonotonicTime ts) void TransferListenerBase::handleFrame(const RxFrame& frame) { - const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType()); - - TransferReceiver* recv = receivers_.access(key); - if (recv == NULL) + if (frame.getSrcNodeID().isUnicast()) // Normal transfer { - if (!frame.isFirst()) - { - return; - } + const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType()); - TransferReceiver new_recv; - recv = receivers_.insert(key, new_recv); + TransferReceiver* recv = receivers_.access(key); if (recv == NULL) { - UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str()); - return; + if (!frame.isFirst()) + { + return; + } + + TransferReceiver new_recv; + recv = receivers_.insert(key, new_recv); + if (recv == NULL) + { + UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str()); + return; + } } + TransferBufferAccessor tba(bufmgr_, key); + handleReception(*recv, frame, tba); + } + else if (frame.getSrcNodeID().isBroadcast() && + frame.isFirst() && + frame.isLast() && + frame.getDstNodeID().isBroadcast()) // Rogue transfer + { + handleRogueTransferReception(frame); + } + else + { + UAVCAN_TRACE("TransferListenerBase", "Invalid frame: %s", frame.toString().c_str()); // Invalid frame } - TransferBufferAccessor tba(bufmgr_, key); - handleReception(*recv, frame, tba); } } diff --git a/libuavcan/test/transport/transfer_listener.cpp b/libuavcan/test/transport/transfer_listener.cpp index 5a4265e168..33ebf82770 100644 --- a/libuavcan/test/transport/transfer_listener.cpp +++ b/libuavcan/test/transport/transfer_listener.cpp @@ -243,3 +243,38 @@ TEST(TransferListener, MaximumTransferLength) ASSERT_TRUE(subscriber.isEmpty()); } + + +TEST(TransferListener, RogueTransfers) +{ + const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); + + uavcan::PoolManager<1> poolmgr; + uavcan::TransferPerfCounter perf; + TestListener<0, 0, 0> subscriber(perf, type, poolmgr); + + TransferListenerEmulator emulator(subscriber, type); + const Transfer transfers[] = + { + emulator.makeTransfer(uavcan::TransferTypeMessageUnicast, 0, "12345678"), // Invalid - not broadcast + emulator.makeTransfer(uavcan::TransferTypeMessageBroadcast, 0, "12345678"), // Valid + emulator.makeTransfer(uavcan::TransferTypeMessageBroadcast, 0, "123456789"), // Invalid - not SFT + emulator.makeTransfer(uavcan::TransferTypeMessageBroadcast, 0, "") // Valid + }; + + emulator.send(transfers); + + // Nothing will be received, because rogue transfers are disabled by default + ASSERT_TRUE(subscriber.isEmpty()); + + subscriber.allowRogueTransfers(); + + // Re-send everything again + emulator.send(transfers); + + // Now the rogue transfers are enabled + ASSERT_TRUE(subscriber.matchAndPop(transfers[1])); // Only SFT broadcast will be accepted + ASSERT_TRUE(subscriber.matchAndPop(transfers[3])); + + ASSERT_TRUE(subscriber.isEmpty()); +} diff --git a/libuavcan/test/transport/transfer_test_helpers.hpp b/libuavcan/test/transport/transfer_test_helpers.hpp index 449c0afb9a..bbb25c4362 100644 --- a/libuavcan/test/transport/transfer_test_helpers.hpp +++ b/libuavcan/test/transport/transfer_test_helpers.hpp @@ -129,6 +129,14 @@ public: const Transfer rx(transfer, Base::getDataTypeDescriptor()); transfers_.push(rx); std::cout << "Received transfer: " << rx.toString() << std::endl; + + const bool single_frame = dynamic_cast(&transfer) != NULL; + + const bool rogue = single_frame && + transfer.getSrcNodeID().isBroadcast() && + (transfer.getTransferType() == uavcan::TransferTypeMessageBroadcast); + + ASSERT_EQ(rogue, transfer.isRogueTransfer()); } bool matchAndPop(const Transfer& reference) From 7aa30e9cdc439b7846ca688c19159202cc4d5d64 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 6 Apr 2015 19:11:21 +0300 Subject: [PATCH 08/16] Naming: TransferSender::allowRogueTransfers() --- .../include/uavcan/transport/transfer_sender.hpp | 14 ++++++++------ libuavcan/src/transport/uc_transfer_sender.cpp | 8 +++----- libuavcan/test/transport/transfer_sender.cpp | 2 +- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/libuavcan/include/uavcan/transport/transfer_sender.hpp b/libuavcan/include/uavcan/transport/transfer_sender.hpp index 0ffc232ebf..0b23b8b405 100644 --- a/libuavcan/include/uavcan/transport/transfer_sender.hpp +++ b/libuavcan/include/uavcan/transport/transfer_sender.hpp @@ -25,7 +25,7 @@ class UAVCAN_EXPORT TransferSender const TransferCRC crc_base_; CanIOFlags flags_; uint8_t iface_mask_; - bool allow_broadcasting_in_passive_mode_; + bool allow_rogue_transfers_; Dispatcher& dispatcher_; @@ -47,7 +47,7 @@ public: , crc_base_(data_type.getSignature().toTransferCRC()) , flags_(CanIOFlags(0)) , iface_mask_(AllIfacesMask) - , allow_broadcasting_in_passive_mode_(false) + , allow_rogue_transfers_(false) , dispatcher_(dispatcher) { } @@ -62,11 +62,13 @@ public: } /** - * By default, this class will return an error on any attempt to publish a message while the - * dispatcher is configured in passive mode. This method allows to permanently enable sending - * broadcast transfers in passive mode for this class instance. + * Rogue transfers (i.e. transfers that don't carry a valid Source Node ID) can be sent if + * the local node is configured in passive mode (i.e. the node doesn't have a valid Node ID). + * By default, this class will return an error if it is asked to send a transfer while the + * node is configured in passive mode. However, if this option is enabled, it will be possible + * to send rogue transfers from passive mode. */ - void allowBroadcastingInPassiveMode() { allow_broadcasting_in_passive_mode_ = true; } + void allowRogueTransfers() { allow_rogue_transfers_ = true; } /** * Send with explicit Transfer ID. diff --git a/libuavcan/src/transport/uc_transfer_sender.cpp b/libuavcan/src/transport/uc_transfer_sender.cpp index 38766ddc9b..bd024a8ff7 100644 --- a/libuavcan/src/transport/uc_transfer_sender.cpp +++ b/libuavcan/src/transport/uc_transfer_sender.cpp @@ -29,14 +29,12 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic UAVCAN_TRACE("TransferSender", "%s", frame.toString().c_str()); /* - * Checking if we're allowed to send. In passive mode we can send only if: - * - Passive broadcasting is enabled - * - Transfer type is broadcast - * - Transfer payload fits one CAN frame + * Checking if we're allowed to send. + * In passive mode we can send only rogue transfers, if they are enabled. */ if (dispatcher_.isPassiveMode()) { - const bool allow = allow_broadcasting_in_passive_mode_ && + const bool allow = allow_rogue_transfers_ && (transfer_type == TransferTypeMessageBroadcast) && (int(payload_len) <= frame.getMaxPayloadLen()); if (!allow) diff --git a/libuavcan/test/transport/transfer_sender.cpp b/libuavcan/test/transport/transfer_sender.cpp index 4719543ec4..baa4929f38 100644 --- a/libuavcan/test/transport/transfer_sender.cpp +++ b/libuavcan/test/transport/transfer_sender.cpp @@ -241,7 +241,7 @@ TEST(TransferSender, PassiveMode) uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast)); // Overriding the default - sender.allowBroadcastingInPassiveMode(); + sender.allowRogueTransfers(); // OK, now we can broadcast in any mode ASSERT_LE(0, sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), From 1a8757e54b86b44d527f38d2c1b483f43e1a2a8d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 6 Apr 2015 19:21:01 +0300 Subject: [PATCH 09/16] allowRogueTransfers() exposed in Publisher and Subscriber --- libuavcan/include/uavcan/node/generic_publisher.hpp | 9 +++++++++ libuavcan/include/uavcan/node/generic_subscriber.hpp | 9 +++++++++ libuavcan/include/uavcan/node/publisher.hpp | 1 + libuavcan/include/uavcan/node/subscriber.hpp | 1 + 4 files changed, 20 insertions(+) diff --git a/libuavcan/include/uavcan/node/generic_publisher.hpp b/libuavcan/include/uavcan/node/generic_publisher.hpp index aece082ddb..9b2f97bc90 100644 --- a/libuavcan/include/uavcan/node/generic_publisher.hpp +++ b/libuavcan/include/uavcan/node/generic_publisher.hpp @@ -60,6 +60,15 @@ public: MonotonicDuration getTxTimeout() const { return tx_timeout_; } void setTxTimeout(MonotonicDuration tx_timeout); + /** + * By default, attempt to send a transfer from passive mode will result in an error @ref ErrPassive. + * This option allows to enable sending rogue transfers from passive mode. + */ + void allowRogueTransfers() + { + sender_->allowRogueTransfers(); + } + INode& getNode() const { return node_; } }; diff --git a/libuavcan/include/uavcan/node/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index 3ec6b87f92..a5d81512b8 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -196,6 +196,15 @@ protected: return genericStart(&Dispatcher::registerServiceResponseListener); } + /** + * By default, rogue transfers will be ignored. + * This option allows to enable reception of rogue transfers. + */ + void allowRogueTransfers() + { + forwarder_->allowRogueTransfers(); + } + /** * Terminate the subscription. * Dispatcher core will remove this instance from the subscribers list. diff --git a/libuavcan/include/uavcan/node/publisher.hpp b/libuavcan/include/uavcan/node/publisher.hpp index 6abb3bd533..9c684b29fc 100644 --- a/libuavcan/include/uavcan/node/publisher.hpp +++ b/libuavcan/include/uavcan/node/publisher.hpp @@ -87,6 +87,7 @@ public: */ using BaseType::init; + using BaseType::allowRogueTransfers; using BaseType::getTransferSender; using BaseType::getMinTxTimeout; using BaseType::getMaxTxTimeout; diff --git a/libuavcan/include/uavcan/node/subscriber.hpp b/libuavcan/include/uavcan/node/subscriber.hpp index 1112a6abc0..1bb8b2c934 100644 --- a/libuavcan/include/uavcan/node/subscriber.hpp +++ b/libuavcan/include/uavcan/node/subscriber.hpp @@ -111,6 +111,7 @@ public: return BaseType::startAsMessageListener(); } + using BaseType::allowRogueTransfers; using BaseType::stop; using BaseType::getFailureCount; }; From 82052fb0987fbdfa29fa6d6bffa105d6c58eb3ed Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 6 Apr 2015 22:30:01 +0300 Subject: [PATCH 10/16] ReceivedDataStructure<>::isRogueTransfer() --- libuavcan/include/uavcan/node/generic_subscriber.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libuavcan/include/uavcan/node/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index a5d81512b8..dde139fe64 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -67,6 +67,7 @@ public: TransferID getTransferID() const { return safeget(); } NodeID getSrcNodeID() const { return safeget(); } uint8_t getIfaceIndex() const { return safeget(); } + bool isRogueTransfer() const { return safeget(); } }; /** From ea2e885c50d36c9c0f0c865dbe5a7e022588dcf5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 7 Apr 2015 00:14:03 +0300 Subject: [PATCH 11/16] DynamicNodeIDAllocation constants updated --- dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan b/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan index 28dc90697c..2d715e8adb 100644 --- a/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan +++ b/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan @@ -94,13 +94,13 @@ # # This is the delay that Allocator must take to initialize its Node ID Table. # -uint8 ALLOCATOR_INITIALIZATION_DELAY_SEC = 30 +uint8 ALLOCATOR_INITIALIZATION_DELAY_SEC = 10 # # This is the minimum interval (maximum frequency) at which the Allocatee can # broadcast allocation requests. # -uint8 ALLOCATEE_MIN_BROADCAST_INTERVAL_SEC = 2 +uint8 ALLOCATEE_MIN_BROADCAST_INTERVAL_SEC = 1 # # It is recommended to pick Node IDs from this range only, even if the From 012765a796803b1ae536fe26868e9edf10ee4fdb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 7 Apr 2015 00:14:44 +0300 Subject: [PATCH 12/16] Typo (missing newline) --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b6b61bd0a1..b889336f0d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,4 +34,4 @@ add_subdirectory(libuavcan) if (${CMAKE_SYSTEM_NAME} MATCHES "Linux") message(STATUS "Adding Linux support library") add_subdirectory(libuavcan_drivers/linux) -endif () \ No newline at end of file +endif () From 042aa60773f81be58a26594febb14632f1a3d7ce Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 7 Apr 2015 00:47:02 +0300 Subject: [PATCH 13/16] DynamicNodeIDAllocationClient --- .../dynamic_node_id_allocation_client.hpp | 105 +++++++++++++ .../uc_dynamic_node_id_allocation_client.cpp | 138 +++++++++++++++++ .../dynamic_node_id_allocation_client.cpp | 143 ++++++++++++++++++ 3 files changed, 386 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp create mode 100644 libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp new file mode 100644 index 0000000000..48b5284732 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class implements client-side logic of dynamic node ID allocation procedure. + * + * Once started, the object will be publishing dynamic node ID allocation requests at the maximum frequency allowed + * by the specification, until a Node ID is granted by the allocator. Note that if there are multiple responses, + * Node ID from the first response will be taken, and all subsequent responses will be ignored. + * + * If the local node is equipped with redundant CAN interfaces, all of them will be used for publishing requests + * and listening for responses. + * + * Once dynamic allocation is complete (or not needed anymore), the object can be deleted. + */ +class DynamicNodeIDAllocationClient : private TimerBase +{ + typedef MethodBinder&)> + DynamicNodeIDAllocationCallback; + + Publisher dnida_pub_; + Subscriber dnida_sub_; + + uint64_t short_unique_id_; + NodeID preferred_node_id_; + NodeID allocated_node_id_; + NodeID allocator_node_id_; + + virtual void handleTimerEvent(const TimerEvent&); + + void handleDynamicNodeIDAllocation(const ReceivedDataStructure& msg); + + int startImpl(); + +public: + DynamicNodeIDAllocationClient(INode& node) + : TimerBase(node) + , dnida_pub_(node) + , dnida_sub_(node) + , short_unique_id_(0) + { } + + /** + * Starts the client with a pre-computed short unique Node ID. + * @param short_unique_node_id The unique ID, only lower 57 bits of it will be used. + * @param preferred_node_id Node ID that the application would like to take; default is any. + * @return Zero on success + * Negative error code on failure + * -ErrLogic if the node is not in passive mode (i.e. allocation is meaningless) + * -ErrInvalidParam if the supplied short unique ID is invalid + */ + int start(uint64_t short_unique_node_id, NodeID preferred_node_id = NodeID::Broadcast); + + /** + * This overload computes a short unique Node ID using data from uavcan.protocol.HardwareVersion structure. + * @param hardware_version Hardware version information, where unique_id must be set correctly. + * @param preferred_node_id Node ID that the application would like to take; default is any. + * @return Refer to the overload for details + * -ErrInvalidParam if short unique ID could not be computed + */ + int start(const protocol::HardwareVersion& hardware_version, NodeID preferred_node_id = NodeID::Broadcast); + + /** + * This method allows to retrieve the value that was allocated to the local node. + * If no value was allocated yet, the returned Node ID will be invalid (non-unicast). + * Tip: use getAllocatedNodeID().isUnicast() to check if allocation is complete. + * @return If allocation is complete, a valid unicast Node ID will be returned. + * If allocation is not complete yet, an invalid Node ID will be returned. + */ + NodeID getAllocatedNodeID() const { return allocated_node_id_; } + + /** + * This method allows to retrieve node ID of the allocator that granted our Node ID. + * If no Node ID was allocated yet, the returned Node ID will be invalid (non-unicast). + * @return If allocation is complete, a valid unicast Node ID will be returned. + * If allocation is not complete yet, an invalid Node ID will be returned. + */ + NodeID getAllocatorNodeID() const { return allocator_node_id_; } + + /** + * This utility method simply returns the short node ID used in the allocation request messages. + * Returned value will be zero if the short unique node ID has not been initialized yet. + */ + uint64_t getShortUniqueNodeID() const { return short_unique_id_; } +}; + +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp new file mode 100644 index 0000000000..7a989c13a8 --- /dev/null +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp @@ -0,0 +1,138 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include // For CRC64 + +namespace uavcan +{ + +void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&) +{ + UAVCAN_ASSERT(!allocated_node_id_.isUnicast()); + UAVCAN_ASSERT(preferred_node_id_.isUnicast()); + + protocol::DynamicNodeIDAllocation msg; + msg.short_unique_id = short_unique_id_; + msg.node_id = preferred_node_id_.get(); + + UAVCAN_TRACE("DynamicNodeIDAllocation", "Broadcasting a request: short unique ID 0x%016llx, preferred ID %d", + static_cast(short_unique_id_), + static_cast(preferred_node_id_.get())); + + const int res = dnida_pub_.broadcast(msg); + if (res < 0) + { + dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocation pub failed"); + } +} + +void DynamicNodeIDAllocationClient::handleDynamicNodeIDAllocation( + const ReceivedDataStructure& msg) +{ + if ((msg.short_unique_id != short_unique_id_) || msg.isRogueTransfer()) + { + return; // Not ours + } + + if (allocated_node_id_.isUnicast()) + { + UAVCAN_TRACE("DynamicNodeIDAllocation", "Redundant response from %d", + static_cast(msg.getSrcNodeID().get())); + return; // Allocation is already done + } + + const NodeID allocation(msg.node_id); + if (!allocation.isUnicast()) + { + dnida_sub_.getNode().registerInternalFailure("DynamicNodeIDAllocation bad node ID allocated"); + return; + } + + allocated_node_id_ = allocation; + allocator_node_id_ = msg.getSrcNodeID(); + + UAVCAN_TRACE("DynamicNodeIDAllocation", "Allocation done: requested %d, received %d from %d", + static_cast(preferred_node_id_.get()), + static_cast(allocated_node_id_.get()), + static_cast(msg.getSrcNodeID().get())); + + TimerBase::stop(); +} + +int DynamicNodeIDAllocationClient::startImpl() +{ + short_unique_id_ &= protocol::DynamicNodeIDAllocation::FieldTypes::short_unique_id::mask(); + + if ((short_unique_id_ == 0) || !preferred_node_id_.isUnicast()) + { + // It's not like a zero unique ID is not unique enough, but it's surely suspicious + return -ErrInvalidParam; + } + + UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Short unique node ID: 0x%016llx, preferred node ID: %d", + static_cast(short_unique_id_), static_cast(preferred_node_id_.get())); + + allocated_node_id_ = NodeID(); + UAVCAN_ASSERT(!allocated_node_id_.isUnicast()); + UAVCAN_ASSERT(!allocated_node_id_.isValid()); + + int res = dnida_pub_.init(); + if (res < 0) + { + return res; + } + dnida_pub_.allowRogueTransfers(); + + res = dnida_sub_.start( + DynamicNodeIDAllocationCallback(this, &DynamicNodeIDAllocationClient::handleDynamicNodeIDAllocation)); + if (res < 0) + { + return res; + } + dnida_sub_.allowRogueTransfers(); + + startPeriodic( + MonotonicDuration::fromMSec(protocol::DynamicNodeIDAllocation::ALLOCATEE_MIN_BROADCAST_INTERVAL_SEC * 1000)); + + return 0; +} + +int DynamicNodeIDAllocationClient::start(const uint64_t short_unique_node_id, const NodeID preferred_node_id) +{ + short_unique_id_ = short_unique_node_id; + preferred_node_id_ = preferred_node_id; + return startImpl(); +} + +int DynamicNodeIDAllocationClient::start(const protocol::HardwareVersion& hardware_version, + const NodeID preferred_node_id) +{ + // Checking if unique ID is set + bool unique_id_is_zero = true; + for (uint8_t i = 0; i < hardware_version.unique_id.size(); i++) + { + if (hardware_version.unique_id[i] != 0) + { + unique_id_is_zero = false; + break; + } + } + + if (unique_id_is_zero) + { + return -ErrInvalidParam; + } + + // Reducing the ID to 64 bits according to the specification + DataTypeSignatureCRC crc; + crc.add(hardware_version.unique_id.begin(), hardware_version.unique_id.size()); + short_unique_id_ = crc.get(); + + preferred_node_id_ = preferred_node_id; + + return startImpl(); +} + +} diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp new file mode 100644 index 0000000000..0e86d84e54 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp @@ -0,0 +1,143 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +TEST(DynamicNodeIDAllocationClient, Basic) +{ + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + uavcan::protocol::HardwareVersion hwver; + + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver)); // Empty hardware version is not allowed + + /* + * Initializing. + * Reduced signature was calculated as follows: + * >>> crc = pyuavcan.dsdl.signature.Signature() + * >>> crc.add(range(16)) + * >>> crc.get_value() + * 4539764000456687298L + * >>> hex(crc.get_value()) + * '0x3f007b4e4353bec2L' + */ + const uavcan::uint64_t UniqueID64Bit = 0x3f007b4e4353bec2ULL; + const uavcan::uint64_t UniqueID57Bit = UniqueID64Bit & ((1ULL << 57) - 1U); + for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++) + { + hwver.unique_id[i] = i; + } + + // More incorrect inputs + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(0)); + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver, uavcan::NodeID::Broadcast)); // Bad node ID + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver, uavcan::NodeID())); // Ditto + + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, dnidac.start(hwver, PreferredNodeID)); + + // Making sure the signature reduction was performed correctly + ASSERT_EQ(UniqueID57Bit, dnidac.getShortUniqueNodeID()); + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + + /* + * Initializing subscriber + * Rogue transfers must be enabled + */ + SubscriberWithCollector dynid_sub(nodes.a); + ASSERT_LE(0, dynid_sub.start()); + dynid_sub.subscriber.allowRogueTransfers(); + + /* + * Monitoring requests at 1Hz + * First request will be sent immediately + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + ASSERT_TRUE(dynid_sub.collector.msg.get()); + ASSERT_EQ(UniqueID57Bit, dynid_sub.collector.msg->short_unique_id); + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + dynid_sub.collector.msg.reset(); + + // Rate validation + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(dynid_sub.collector.msg.get()); + + // Second - rate is 1 Hz + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(900)); + ASSERT_TRUE(dynid_sub.collector.msg.get()); + ASSERT_EQ(UniqueID57Bit, dynid_sub.collector.msg->short_unique_id); + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + dynid_sub.collector.msg.reset(); + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + + /* + * Sending a bunch of responses + * Note that response transfers are NOT rogue + */ + uavcan::Publisher dynid_pub(nodes.a); + ASSERT_LE(0, dynid_pub.init()); + + uavcan::protocol::DynamicNodeIDAllocation msg; + + msg.short_unique_id = 123; // garbage + msg.node_id = 100; // oh whatever + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); + + msg.short_unique_id = 0; // garbage + msg.node_id = 101; + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); + + msg.short_unique_id = UniqueID57Bit; // correct ID + msg.node_id = 102; // THIS NODE ID WILL BE USED + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); + + msg.short_unique_id = UniqueID57Bit; // repeating, will be ignored + msg.node_id = 103; + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); + + /* + * Validating the results + */ + ASSERT_FALSE(dynid_sub.collector.msg.get()); + + ASSERT_TRUE(dnidac.getAllocatedNodeID().isUnicast()); + ASSERT_TRUE(dnidac.getAllocatorNodeID().isUnicast()); + + ASSERT_EQ(102, dnidac.getAllocatedNodeID().get()); + ASSERT_EQ(10, dnidac.getAllocatorNodeID().get()); + + // Making sure requests have stopped + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + ASSERT_FALSE(dynid_sub.collector.msg.get()); +} + + +TEST(DynamicNodeIDAllocationClient, NonPassiveMode) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + ASSERT_LE(-uavcan::ErrLogic, dnidac.start(123456789)); +} From df2a38c217a101bba77260e8de51deeba2903dac Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 8 Apr 2015 02:40:28 +0300 Subject: [PATCH 14/16] Better name for anonymous transfers (automatic renaming) --- .../include/uavcan/node/generic_publisher.hpp | 6 +++--- .../include/uavcan/node/generic_subscriber.hpp | 10 +++++----- libuavcan/include/uavcan/node/publisher.hpp | 2 +- libuavcan/include/uavcan/node/subscriber.hpp | 2 +- .../uavcan/transport/transfer_listener.hpp | 18 +++++++++--------- .../uavcan/transport/transfer_sender.hpp | 10 +++++----- .../uc_dynamic_node_id_allocation_client.cpp | 6 +++--- .../src/transport/uc_transfer_listener.cpp | 10 +++++----- libuavcan/src/transport/uc_transfer_sender.cpp | 4 ++-- .../dynamic_node_id_allocation_client.cpp | 6 +++--- libuavcan/test/transport/transfer_listener.cpp | 8 ++++---- libuavcan/test/transport/transfer_sender.cpp | 2 +- .../test/transport/transfer_test_helpers.hpp | 8 ++++---- 13 files changed, 46 insertions(+), 46 deletions(-) diff --git a/libuavcan/include/uavcan/node/generic_publisher.hpp b/libuavcan/include/uavcan/node/generic_publisher.hpp index 9b2f97bc90..7dde549c00 100644 --- a/libuavcan/include/uavcan/node/generic_publisher.hpp +++ b/libuavcan/include/uavcan/node/generic_publisher.hpp @@ -62,11 +62,11 @@ public: /** * By default, attempt to send a transfer from passive mode will result in an error @ref ErrPassive. - * This option allows to enable sending rogue transfers from passive mode. + * This option allows to enable sending anonymous transfers from passive mode. */ - void allowRogueTransfers() + void allowAnonymousTransfers() { - sender_->allowRogueTransfers(); + sender_->allowAnonymousTransfers(); } INode& getNode() const { return node_; } diff --git a/libuavcan/include/uavcan/node/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index dde139fe64..545e2a7489 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -67,7 +67,7 @@ public: TransferID getTransferID() const { return safeget(); } NodeID getSrcNodeID() const { return safeget(); } uint8_t getIfaceIndex() const { return safeget(); } - bool isRogueTransfer() const { return safeget(); } + bool isAnonymousTransfer() const { return safeget(); } }; /** @@ -198,12 +198,12 @@ protected: } /** - * By default, rogue transfers will be ignored. - * This option allows to enable reception of rogue transfers. + * By default, anonymous transfers will be ignored. + * This option allows to enable reception of anonymous transfers. */ - void allowRogueTransfers() + void allowAnonymousTransfers() { - forwarder_->allowRogueTransfers(); + forwarder_->allowAnonymousTransfers(); } /** diff --git a/libuavcan/include/uavcan/node/publisher.hpp b/libuavcan/include/uavcan/node/publisher.hpp index 9c684b29fc..2b720f7ee1 100644 --- a/libuavcan/include/uavcan/node/publisher.hpp +++ b/libuavcan/include/uavcan/node/publisher.hpp @@ -87,7 +87,7 @@ public: */ using BaseType::init; - using BaseType::allowRogueTransfers; + using BaseType::allowAnonymousTransfers; using BaseType::getTransferSender; using BaseType::getMinTxTimeout; using BaseType::getMaxTxTimeout; diff --git a/libuavcan/include/uavcan/node/subscriber.hpp b/libuavcan/include/uavcan/node/subscriber.hpp index 1bb8b2c934..f5c19f4c78 100644 --- a/libuavcan/include/uavcan/node/subscriber.hpp +++ b/libuavcan/include/uavcan/node/subscriber.hpp @@ -111,7 +111,7 @@ public: return BaseType::startAsMessageListener(); } - using BaseType::allowRogueTransfers; + using BaseType::allowAnonymousTransfers; using BaseType::stop; using BaseType::getFailureCount; }; diff --git a/libuavcan/include/uavcan/transport/transfer_listener.hpp b/libuavcan/include/uavcan/transport/transfer_listener.hpp index 99f4efd11e..73491be3b1 100644 --- a/libuavcan/include/uavcan/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -51,9 +51,9 @@ public: virtual void release() { } /** - * Whether this is a rogue transfer + * Whether this is a anonymous transfer */ - virtual bool isRogueTransfer() const { return false; } + virtual bool isAnonymousTransfer() const { return false; } MonotonicTime getMonotonicTimestamp() const { return ts_mono_; } UtcTime getUtcTimestamp() const { return ts_utc_; } @@ -73,7 +73,7 @@ class UAVCAN_EXPORT SingleFrameIncomingTransfer : public IncomingTransfer public: explicit SingleFrameIncomingTransfer(const RxFrame& frm); virtual int read(unsigned offset, uint8_t* data, unsigned len) const; - virtual bool isRogueTransfer() const; + virtual bool isAnonymousTransfer() const; }; /** @@ -99,7 +99,7 @@ class UAVCAN_EXPORT TransferListenerBase : public LinkedListNode& receivers_; ITransferBufferManager& bufmgr_; TransferPerfCounter& perf_; - bool allow_rogue_transfers_; + bool allow_anonymous_transfers_; class TimedOutReceiverPredicate { @@ -126,13 +126,13 @@ protected: , receivers_(receivers) , bufmgr_(bufmgr) , perf_(perf) - , allow_rogue_transfers_(false) + , allow_anonymous_transfers_(false) { } virtual ~TransferListenerBase() { } void handleReception(TransferReceiver& receiver, const RxFrame& frame, TransferBufferAccessor& tba); - void handleRogueTransferReception(const RxFrame& frame); + void handleAnonymousTransferReception(const RxFrame& frame); virtual void handleIncomingTransfer(IncomingTransfer& transfer) = 0; @@ -140,10 +140,10 @@ public: const DataTypeDescriptor& getDataTypeDescriptor() const { return data_type_; } /** - * By default, rogue transfers will be ignored. - * This option allows to enable reception of rogue transfers. + * By default, anonymous transfers will be ignored. + * This option allows to enable reception of anonymous transfers. */ - void allowRogueTransfers() { allow_rogue_transfers_ = true; } + void allowAnonymousTransfers() { allow_anonymous_transfers_ = true; } void cleanup(MonotonicTime ts); diff --git a/libuavcan/include/uavcan/transport/transfer_sender.hpp b/libuavcan/include/uavcan/transport/transfer_sender.hpp index 0b23b8b405..b67edf23a7 100644 --- a/libuavcan/include/uavcan/transport/transfer_sender.hpp +++ b/libuavcan/include/uavcan/transport/transfer_sender.hpp @@ -25,7 +25,7 @@ class UAVCAN_EXPORT TransferSender const TransferCRC crc_base_; CanIOFlags flags_; uint8_t iface_mask_; - bool allow_rogue_transfers_; + bool allow_anonymous_transfers_; Dispatcher& dispatcher_; @@ -47,7 +47,7 @@ public: , crc_base_(data_type.getSignature().toTransferCRC()) , flags_(CanIOFlags(0)) , iface_mask_(AllIfacesMask) - , allow_rogue_transfers_(false) + , allow_anonymous_transfers_(false) , dispatcher_(dispatcher) { } @@ -62,13 +62,13 @@ public: } /** - * Rogue transfers (i.e. transfers that don't carry a valid Source Node ID) can be sent if + * Anonymous transfers (i.e. transfers that don't carry a valid Source Node ID) can be sent if * the local node is configured in passive mode (i.e. the node doesn't have a valid Node ID). * By default, this class will return an error if it is asked to send a transfer while the * node is configured in passive mode. However, if this option is enabled, it will be possible - * to send rogue transfers from passive mode. + * to send anonymous transfers from passive mode. */ - void allowRogueTransfers() { allow_rogue_transfers_ = true; } + void allowAnonymousTransfers() { allow_anonymous_transfers_ = true; } /** * Send with explicit Transfer ID. diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp index 7a989c13a8..f948d7b42e 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp @@ -31,7 +31,7 @@ void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&) void DynamicNodeIDAllocationClient::handleDynamicNodeIDAllocation( const ReceivedDataStructure& msg) { - if ((msg.short_unique_id != short_unique_id_) || msg.isRogueTransfer()) + if ((msg.short_unique_id != short_unique_id_) || msg.isAnonymousTransfer()) { return; // Not ours } @@ -83,7 +83,7 @@ int DynamicNodeIDAllocationClient::startImpl() { return res; } - dnida_pub_.allowRogueTransfers(); + dnida_pub_.allowAnonymousTransfers(); res = dnida_sub_.start( DynamicNodeIDAllocationCallback(this, &DynamicNodeIDAllocationClient::handleDynamicNodeIDAllocation)); @@ -91,7 +91,7 @@ int DynamicNodeIDAllocationClient::startImpl() { return res; } - dnida_sub_.allowRogueTransfers(); + dnida_sub_.allowAnonymousTransfers(); startPeriodic( MonotonicDuration::fromMSec(protocol::DynamicNodeIDAllocation::ALLOCATEE_MIN_BROADCAST_INTERVAL_SEC * 1000)); diff --git a/libuavcan/src/transport/uc_transfer_listener.cpp b/libuavcan/src/transport/uc_transfer_listener.cpp index 8cba8d6226..6a5d38fbec 100644 --- a/libuavcan/src/transport/uc_transfer_listener.cpp +++ b/libuavcan/src/transport/uc_transfer_listener.cpp @@ -50,7 +50,7 @@ int SingleFrameIncomingTransfer::read(unsigned offset, uint8_t* data, unsigned l return int(len); } -bool SingleFrameIncomingTransfer::isRogueTransfer() const +bool SingleFrameIncomingTransfer::isAnonymousTransfer() const { return (getTransferType() == TransferTypeMessageBroadcast) && getSrcNodeID().isBroadcast(); } @@ -177,9 +177,9 @@ void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxF } } -void TransferListenerBase::handleRogueTransferReception(const RxFrame& frame) +void TransferListenerBase::handleAnonymousTransferReception(const RxFrame& frame) { - if (allow_rogue_transfers_) + if (allow_anonymous_transfers_) { perf_.addRxTransfer(); SingleFrameIncomingTransfer it(frame); @@ -221,9 +221,9 @@ void TransferListenerBase::handleFrame(const RxFrame& frame) else if (frame.getSrcNodeID().isBroadcast() && frame.isFirst() && frame.isLast() && - frame.getDstNodeID().isBroadcast()) // Rogue transfer + frame.getDstNodeID().isBroadcast()) // Anonymous transfer { - handleRogueTransferReception(frame); + handleAnonymousTransferReception(frame); } else { diff --git a/libuavcan/src/transport/uc_transfer_sender.cpp b/libuavcan/src/transport/uc_transfer_sender.cpp index bd024a8ff7..0e603a4b54 100644 --- a/libuavcan/src/transport/uc_transfer_sender.cpp +++ b/libuavcan/src/transport/uc_transfer_sender.cpp @@ -30,11 +30,11 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic /* * Checking if we're allowed to send. - * In passive mode we can send only rogue transfers, if they are enabled. + * In passive mode we can send only anonymous transfers, if they are enabled. */ if (dispatcher_.isPassiveMode()) { - const bool allow = allow_rogue_transfers_ && + const bool allow = allow_anonymous_transfers_ && (transfer_type == TransferTypeMessageBroadcast) && (int(payload_len) <= frame.getMaxPayloadLen()); if (!allow) diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp index 0e86d84e54..5806008291 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp @@ -54,11 +54,11 @@ TEST(DynamicNodeIDAllocationClient, Basic) /* * Initializing subscriber - * Rogue transfers must be enabled + * Anonymous transfers must be enabled */ SubscriberWithCollector dynid_sub(nodes.a); ASSERT_LE(0, dynid_sub.start()); - dynid_sub.subscriber.allowRogueTransfers(); + dynid_sub.subscriber.allowAnonymousTransfers(); /* * Monitoring requests at 1Hz @@ -86,7 +86,7 @@ TEST(DynamicNodeIDAllocationClient, Basic) /* * Sending a bunch of responses - * Note that response transfers are NOT rogue + * Note that response transfers are NOT anonymous */ uavcan::Publisher dynid_pub(nodes.a); ASSERT_LE(0, dynid_pub.init()); diff --git a/libuavcan/test/transport/transfer_listener.cpp b/libuavcan/test/transport/transfer_listener.cpp index 33ebf82770..fd42c1f118 100644 --- a/libuavcan/test/transport/transfer_listener.cpp +++ b/libuavcan/test/transport/transfer_listener.cpp @@ -245,7 +245,7 @@ TEST(TransferListener, MaximumTransferLength) } -TEST(TransferListener, RogueTransfers) +TEST(TransferListener, AnonymousTransfers) { const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); @@ -264,15 +264,15 @@ TEST(TransferListener, RogueTransfers) emulator.send(transfers); - // Nothing will be received, because rogue transfers are disabled by default + // Nothing will be received, because anonymous transfers are disabled by default ASSERT_TRUE(subscriber.isEmpty()); - subscriber.allowRogueTransfers(); + subscriber.allowAnonymousTransfers(); // Re-send everything again emulator.send(transfers); - // Now the rogue transfers are enabled + // Now the anonymous transfers are enabled ASSERT_TRUE(subscriber.matchAndPop(transfers[1])); // Only SFT broadcast will be accepted ASSERT_TRUE(subscriber.matchAndPop(transfers[3])); diff --git a/libuavcan/test/transport/transfer_sender.cpp b/libuavcan/test/transport/transfer_sender.cpp index baa4929f38..1fe2e22dbc 100644 --- a/libuavcan/test/transport/transfer_sender.cpp +++ b/libuavcan/test/transport/transfer_sender.cpp @@ -241,7 +241,7 @@ TEST(TransferSender, PassiveMode) uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast)); // Overriding the default - sender.allowRogueTransfers(); + sender.allowAnonymousTransfers(); // OK, now we can broadcast in any mode ASSERT_LE(0, sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), diff --git a/libuavcan/test/transport/transfer_test_helpers.hpp b/libuavcan/test/transport/transfer_test_helpers.hpp index bbb25c4362..f27a603c49 100644 --- a/libuavcan/test/transport/transfer_test_helpers.hpp +++ b/libuavcan/test/transport/transfer_test_helpers.hpp @@ -132,11 +132,11 @@ public: const bool single_frame = dynamic_cast(&transfer) != NULL; - const bool rogue = single_frame && - transfer.getSrcNodeID().isBroadcast() && - (transfer.getTransferType() == uavcan::TransferTypeMessageBroadcast); + const bool anonymous = single_frame && + transfer.getSrcNodeID().isBroadcast() && + (transfer.getTransferType() == uavcan::TransferTypeMessageBroadcast); - ASSERT_EQ(rogue, transfer.isRogueTransfer()); + ASSERT_EQ(anonymous, transfer.isAnonymousTransfer()); } bool matchAndPop(const Transfer& reference) From b70d32a2acee17b64de6ca9d9af01043ed821b6e Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 23 Apr 2015 16:43:42 -1000 Subject: [PATCH 15/16] Compile error std.hpp:70:37: error: 'std::size_t' has not been declared extern int snprintf(char out, std::size_t maxlen, const char --- libuavcan/include/uavcan/std.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libuavcan/include/uavcan/std.hpp b/libuavcan/include/uavcan/std.hpp index c04ced0c89..a1e63ea469 100644 --- a/libuavcan/include/uavcan/std.hpp +++ b/libuavcan/include/uavcan/std.hpp @@ -7,6 +7,7 @@ #include #include +#include #if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) # error UAVCAN_CPP_VERSION From abea24eeec0d2eb91cc178e26b0af7fe17e12d89 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 26 Apr 2015 07:42:26 +0300 Subject: [PATCH 16/16] New dynamic node ID allocation client --- .../559.DynamicNodeIDAllocation.uavcan | 147 ------------ .../dynamic_node_id/559.Allocation.uavcan | 64 +++++ .../dynamic_node_id_allocation_client.hpp | 68 +++--- .../uc_dynamic_node_id_allocation_client.cpp | 219 +++++++++++------- .../dynamic_node_id_allocation_client.cpp | 167 ++++++++----- 5 files changed, 323 insertions(+), 342 deletions(-) delete mode 100644 dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan create mode 100644 dsdl/uavcan/protocol/dynamic_node_id/559.Allocation.uavcan diff --git a/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan b/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan deleted file mode 100644 index 2d715e8adb..0000000000 --- a/dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan +++ /dev/null @@ -1,147 +0,0 @@ -# -# This message is used for dynamic Node ID allocation. This algorithm is an -# optional extension to the UAVCAN specification, and it is not mandatory to -# support it. -# -# The dynamic Node ID allocation algorithm works as follows: -# -# - As a prerequisite, the network contains at least one node that has some -# Node ID assigned, that will handle allocation requests from other nodes. -# This node will be later referred to as Allocator. It is recognized that -# since UAVCAN is a decentralized network, there might be more than one -# Allocator, but only one of them should handle allocation requests at any -# given time. This issue is covered below in detail. -# -# - The network may also contain any number of nodes that seek to have -# Node IDs assigned to them. In order to get a Node ID assigned, these nodes -# make a request to the Allocator. Requesting nodes will be referred to as -# Allocatees. -# -# - In order to get a Node ID allocated, the Allocatee fills this message -# structure with its short unique ID (see below to learn how to compute one) -# and its preferred Node ID (which is basically the Node ID it would like to -# take, if possible). If the Allocatee doesn't have any preference, the -# preferred Node ID can be set to zero. -# -# - The Allocatee broadcasts the message to the bus. Since it doesn't have a -# Node ID at the moment, the field Source Node ID in the broadcasted message -# must be zero. Note that it may require special logic in the transport -# layer to receive messages where Source Node ID is zeroed. -# -# - The Allocator receives the message and refers to its Node ID Table, where -# it looks for any unallocated Node ID value that equal to or greater than -# the value that was requested by the Allocatee. If the desired Node ID is -# set to zero, or if all Node IDs that are larger than the preferred one are -# already taken, the Allocator traverses the Node ID Table from top to -# bottom (i.e. from high to low Node ID) until it finds first unallocated -# entry. In a case if all available entires are taken, the Allocator ignores -# the request. Please note that it is recommended to restrict the range of -# dynamically assignable Node IDs, as described below. -# -# - The Allocator fills the response message. The short unique ID is set the -# same as in the request, the Node ID field is assigned the value obtained -# on the previous step. When filled, the message is broadcasted as usual, -# i.e. the field Source Node ID must be set to the actual Node ID of the -# Allocator. This allows other Allocators, if any, to understand that this -# message is not an allocation request, otherwise Source Node ID would be -# zero. -# -# - The Allocatee receives the response. Sice there can be other nodes trying -# to get a Node ID at the same time, the Allocatee filters other messages -# by means of comparing its own short unique ID with short unique ID in the -# messages passing by. When a message is received such that its short unique -# ID equals the short unique ID of the Allocatee, the Allocatee gets the -# allocated Node ID from the message, accepts it and continues its normal -# operation as a full-fledged UAVCAN node. -# -# - It is recommended that once Allocatee was granted a Node ID, it stops -# listening to other messages of this type and reconfigures its CAN -# acceptance filters accordingly. -# -# It has been mentioned above that a network may accomodate more than one -# Allocator at the time. In this case, the Allocators should agree between -# each other that only one of them is handling allocation requests at the -# moment. A possible way to reach such an agreement is to let one Allocator -# monitor the presence of the second one (e.g. by means of listening to its -# NodeStatus messages), so that the second Allocator will ignore allocation -# requests as long as the first one is alive. -# -# Every Allocator must continuously maintain a table that indicates which -# Node IDs are taken, and which are free. The logic is like that: immediately -# after initialization, only one Node ID is considered to be taken - the one -# that belongs to the Allocator itself. The allocator subscribes to the -# messages uavcan.protocol.NodeStatus and this one, that are then used to -# update the table in real time - once a NodeStatus or this message are -# received, the table is updated to indicate that the Node ID that sent the -# NodeStatus message is taken. The Allocator must not process allocation -# requests sooner than a few seconds after its initialization, in order to -# ensure that all nodes that exist in the network were able to transmit their -# NodeStatus message at least once. The exact duration of initialization delay -# is defined in a constant below. -# -# In order to minimize chances of Node ID collision between nodes that rely on -# the dynamic allocation and the nodes that use statically allocated Node IDs, -# it is recommended to restrict the range of dynamically allocatable Node IDs. -# Two constants below define a range for dynamically allocated Node ID. -# -# There is a limit that restricts the maximum frequency at which the Allocatee -# is allowed to broadcast allocation requests. It is defined in a constant -# below. -# -# Fields of this message are documented below. -# - -# -# This is the delay that Allocator must take to initialize its Node ID Table. -# -uint8 ALLOCATOR_INITIALIZATION_DELAY_SEC = 10 - -# -# This is the minimum interval (maximum frequency) at which the Allocatee can -# broadcast allocation requests. -# -uint8 ALLOCATEE_MIN_BROADCAST_INTERVAL_SEC = 1 - -# -# It is recommended to pick Node IDs from this range only, even if the -# Allocatee requests a Node ID that is outside of this range. -# -uint7 RECOMMENDED_DYNAMIC_NODE_ID_RANGE_MIN = 64 -uint7 RECOMMENDED_DYNAMIC_NODE_ID_RANGE_MAX = 125 - -# -# This field contains 57 bits that uniquely identify the node that requests -# Node ID allocation. The specification does not define exact algorithm for -# computing short unique ID, but it is recommended to stick to the following -# recommendations: -# -# 1. If the node's unique ID is 64 bit large, lower 57 bits should be used. -# -# 2. If the node's unique ID is larger than 64 bits, it should be reduced -# to 64 bit using the CRC-64-WE hash function (the same function that is -# used in DSDL signature computation, see specification for details), and -# the result should be processed as desribed in the first item above. -# -# Many microcontrollers provide vendor-assigned unique product IDs that are -# typically 12 to 16 bytes long (e.g. STM32, LPC11), which should be employed -# as described in the second item above. -# -# A node is not allowed to use this algorithm if it doesn't have a unique ID -# that is 57 or more bits long. -# -# Regarding the collision probability: the birthday paradox suggests that a -# network with n=30 nodes will experience a short unique ID collision with -# approximate probability of (n^2)/(2*(2^57-1)) = 3.1e-15, assuming that -# the ID reduction function is perfect. -# -truncated uint57 short_unique_id - -# -# In the request, this field should contain Node ID that the requesting node -# would like to take. The allocator then may choose to provide the requested -# Node ID, or to assign a higher one, or to disregard this preference -# completely. It is recommended that the allocator always tries to provide -# Node ID that is equal or higher than requested one, as it would allow nodes -# to get Node ID according to their desired priority on the bus. -# -truncated uint7 node_id diff --git a/dsdl/uavcan/protocol/dynamic_node_id/559.Allocation.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/559.Allocation.uavcan new file mode 100644 index 0000000000..c4ecd05bf2 --- /dev/null +++ b/dsdl/uavcan/protocol/dynamic_node_id/559.Allocation.uavcan @@ -0,0 +1,64 @@ +# +# This message is used for dynamic Node ID allocation. This algorithm is an optional extension to the UAVCAN +# specification, and it is not mandatory to support it. +# +# On the client's side the protocol is defined through the following set of rules: +# +# Rule A. On initialization: +# 1. The client subscribes to this message. +# 2. The client starts the Request Timer with interval of DEFAULT_REQUEST_INTERVAL_MS. +# +# Rule B. On expiration of Request Timer: +# 1. Request Timer restarts. +# 2. The client broadcasts a first-stage Allocation request message, where the fields are assigned following values: +# node_id - preferred node ID, or zero if the client doesn't have any preference +# first_part_of_unique_id - true +# unique_id - first MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST bytes of unique ID +# +# Rule C. On any Allocation message, even if other rules also match: +# 1. Request Timer restarts. +# +# Rule D. On an Allocation message WHERE (source node ID is non-anonymous) AND (client's unique ID starts with the +# bytes available in the field unique_id) AND (unique_id is less than 16 bytes long): +# 1. The client broadcasts a second-stage Allocation request message, where the fields are assigned following values: +# node_id - same value as in the first-stage +# first_part_of_unique_id - false +# unique_id - at most MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST bytes of local unique ID with an offset +# equal to number of bytes in the received unique ID +# +# Rule E. On an Allocation message WHERE (source node ID is non-anonymous) AND (unique_id fully matches client's +# unique ID) AND (node_id in the received message is not zero): +# 1. Request Timer stops. +# 2. The client initializes its node_id with the received value. +# 3. The client terminates subscription to Allocation messages. +# 4. Exit. +# + +# +# Recommended request transmission period. +# +uint16 DEFAULT_REQUEST_PERIOD_MS = 1000 + +# +# Any request message can accommodate no more than this number of bytes of unique ID. +# This limitation is needed to ensure that all request transfers are single-frame. +# +uint8 MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST = 7 + +# +# If transfer is anonymous, this is the preferred ID. +# If transfer is non-anonymous, this is allocated ID. +# +uint7 node_id + +# +# If transfer is anonymous, this field indicates first-stage request. +# If transfer is non-anonymous, this field should be ignored. +# +bool first_part_of_unique_id + +# +# If transfer is anonymous, this array must not contain more than MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST items. +# Note that array is tail-optimized, i.e. it will not be prepended with length field. +# +uint8[<=16] unique_id diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp index 48b5284732..1f11f46663 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include namespace uavcan @@ -18,9 +18,8 @@ namespace uavcan /** * This class implements client-side logic of dynamic node ID allocation procedure. * - * Once started, the object will be publishing dynamic node ID allocation requests at the maximum frequency allowed - * by the specification, until a Node ID is granted by the allocator. Note that if there are multiple responses, - * Node ID from the first response will be taken, and all subsequent responses will be ignored. + * Once started, the object will be publishing dynamic node ID allocation requests at the default frequency defined + * by the specification, until a Node ID is granted by the allocator. * * If the local node is equipped with redundant CAN interfaces, all of them will be used for publishing requests * and listening for responses. @@ -31,73 +30,60 @@ class DynamicNodeIDAllocationClient : private TimerBase { typedef MethodBinder&)> - DynamicNodeIDAllocationCallback; + (const ReceivedDataStructure&)> + AllocationCallback; - Publisher dnida_pub_; - Subscriber dnida_sub_; + Publisher dnida_pub_; + Subscriber dnida_sub_; - uint64_t short_unique_id_; + uint8_t unique_id_[protocol::HardwareVersion::FieldTypes::unique_id::MaxSize]; NodeID preferred_node_id_; NodeID allocated_node_id_; NodeID allocator_node_id_; + void terminate(); + virtual void handleTimerEvent(const TimerEvent&); - void handleDynamicNodeIDAllocation(const ReceivedDataStructure& msg); - - int startImpl(); + void handleAllocation(const ReceivedDataStructure& msg); public: DynamicNodeIDAllocationClient(INode& node) : TimerBase(node) , dnida_pub_(node) , dnida_sub_(node) - , short_unique_id_(0) { } /** - * Starts the client with a pre-computed short unique Node ID. - * @param short_unique_node_id The unique ID, only lower 57 bits of it will be used. - * @param preferred_node_id Node ID that the application would like to take; default is any. - * @return Zero on success - * Negative error code on failure - * -ErrLogic if the node is not in passive mode (i.e. allocation is meaningless) - * -ErrInvalidParam if the supplied short unique ID is invalid + * @param hardware_version Hardware version information, where unique_id must be set correctly. + * @param preferred_node_id Node ID that the application would like to take; set to broadcast (zero) if + * the application doesn't have any preference (this is default). + * @return Zero on success + * Negative error code on failure + * -ErrLogic if 1. the node is not in passive mode or 2. the client is already started */ - int start(uint64_t short_unique_node_id, NodeID preferred_node_id = NodeID::Broadcast); + int start(const protocol::HardwareVersion& hardware_version, const NodeID preferred_node_id = NodeID::Broadcast); /** - * This overload computes a short unique Node ID using data from uavcan.protocol.HardwareVersion structure. - * @param hardware_version Hardware version information, where unique_id must be set correctly. - * @param preferred_node_id Node ID that the application would like to take; default is any. - * @return Refer to the overload for details - * -ErrInvalidParam if short unique ID could not be computed + * Use this method to determine when allocation is complete. */ - int start(const protocol::HardwareVersion& hardware_version, NodeID preferred_node_id = NodeID::Broadcast); + bool isAllocationComplete() const { return getAllocatedNodeID().isUnicast(); } /** - * This method allows to retrieve the value that was allocated to the local node. - * If no value was allocated yet, the returned Node ID will be invalid (non-unicast). - * Tip: use getAllocatedNodeID().isUnicast() to check if allocation is complete. - * @return If allocation is complete, a valid unicast Node ID will be returned. - * If allocation is not complete yet, an invalid Node ID will be returned. + * This method allows to retrieve the node ID that was allocated to the local node. + * If no node ID was allocated yet, the returned node ID will be invalid (non-unicast). + * @return If allocation is complete, a valid unicast node ID will be returned. + * If allocation is not complete yet, a non-unicast node ID will be returned. */ NodeID getAllocatedNodeID() const { return allocated_node_id_; } /** * This method allows to retrieve node ID of the allocator that granted our Node ID. - * If no Node ID was allocated yet, the returned Node ID will be invalid (non-unicast). - * @return If allocation is complete, a valid unicast Node ID will be returned. - * If allocation is not complete yet, an invalid Node ID will be returned. + * If no node ID was allocated yet, the returned node ID will be invalid (non-unicast). + * @return If allocation is complete, a valid unicast node ID will be returned. + * If allocation is not complete yet, an non-unicast node ID will be returned. */ NodeID getAllocatorNodeID() const { return allocator_node_id_; } - - /** - * This utility method simply returns the short node ID used in the allocation request messages. - * Returned value will be zero if the short unique node ID has not been initialized yet. - */ - uint64_t getShortUniqueNodeID() const { return short_unique_id_; } }; } diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp index f948d7b42e..5b3b4e571c 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp @@ -3,117 +3,133 @@ */ #include -#include // For CRC64 namespace uavcan { -void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&) +void DynamicNodeIDAllocationClient::terminate() { - UAVCAN_ASSERT(!allocated_node_id_.isUnicast()); - UAVCAN_ASSERT(preferred_node_id_.isUnicast()); - - protocol::DynamicNodeIDAllocation msg; - msg.short_unique_id = short_unique_id_; - msg.node_id = preferred_node_id_.get(); - - UAVCAN_TRACE("DynamicNodeIDAllocation", "Broadcasting a request: short unique ID 0x%016llx, preferred ID %d", - static_cast(short_unique_id_), - static_cast(preferred_node_id_.get())); - - const int res = dnida_pub_.broadcast(msg); - if (res < 0) - { - dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocation pub failed"); - } + UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Client terminated"); + stop(); + dnida_sub_.stop(); } -void DynamicNodeIDAllocationClient::handleDynamicNodeIDAllocation( - const ReceivedDataStructure& msg) +void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&) { - if ((msg.short_unique_id != short_unique_id_) || msg.isAnonymousTransfer()) + // This method implements Rule B + UAVCAN_ASSERT(preferred_node_id_.isValid()); + if (isAllocationComplete()) { - return; // Not ours - } - - if (allocated_node_id_.isUnicast()) - { - UAVCAN_TRACE("DynamicNodeIDAllocation", "Redundant response from %d", - static_cast(msg.getSrcNodeID().get())); - return; // Allocation is already done - } - - const NodeID allocation(msg.node_id); - if (!allocation.isUnicast()) - { - dnida_sub_.getNode().registerInternalFailure("DynamicNodeIDAllocation bad node ID allocated"); + UAVCAN_ASSERT(0); + terminate(); return; } - allocated_node_id_ = allocation; - allocator_node_id_ = msg.getSrcNodeID(); + // Filling the message + protocol::dynamic_node_id::Allocation msg; + msg.node_id = preferred_node_id_.get(); + msg.first_part_of_unique_id = true; - UAVCAN_TRACE("DynamicNodeIDAllocation", "Allocation done: requested %d, received %d from %d", - static_cast(preferred_node_id_.get()), - static_cast(allocated_node_id_.get()), + msg.unique_id.resize(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST); + copy(unique_id_, unique_id_ + msg.unique_id.size(), msg.unique_id.begin()); + UAVCAN_ASSERT(equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_)); + + // Broadcasting + UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Broadcasting 1st stage: preferred ID: %d", + static_cast(preferred_node_id_.get())); + const int res = dnida_pub_.broadcast(msg); + if (res < 0) + { + dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocationClient request failed"); + } +} + +void DynamicNodeIDAllocationClient::handleAllocation( + const ReceivedDataStructure& msg) +{ + /* + * TODO This method can blow the stack easily + */ + UAVCAN_ASSERT(preferred_node_id_.isValid()); + if (isAllocationComplete()) + { + UAVCAN_ASSERT(0); + terminate(); + return; + } + + startPeriodic(getPeriod()); // Restarting the timer - Rule C + UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Request timer reset because of Allocation message from %d", static_cast(msg.getSrcNodeID().get())); - TimerBase::stop(); -} - -int DynamicNodeIDAllocationClient::startImpl() -{ - short_unique_id_ &= protocol::DynamicNodeIDAllocation::FieldTypes::short_unique_id::mask(); - - if ((short_unique_id_ == 0) || !preferred_node_id_.isUnicast()) + // Rule D + if (!msg.isAnonymousTransfer() && + msg.unique_id.size() > 0 && + msg.unique_id.size() < msg.unique_id.capacity() && + equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_)) { - // It's not like a zero unique ID is not unique enough, but it's surely suspicious - return -ErrInvalidParam; + // Filling the response message + const uint8_t size_of_unique_id_in_response = + min(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST, + static_cast(msg.unique_id.capacity() - msg.unique_id.size())); + + protocol::dynamic_node_id::Allocation second_stage; + second_stage.node_id = preferred_node_id_.get(); + second_stage.first_part_of_unique_id = false; + + second_stage.unique_id.resize(size_of_unique_id_in_response); + + copy(unique_id_ + msg.unique_id.size(), + unique_id_ + msg.unique_id.size() + size_of_unique_id_in_response, + second_stage.unique_id.begin()); + + UAVCAN_ASSERT(equal(second_stage.unique_id.begin(), + second_stage.unique_id.end(), + unique_id_ + msg.unique_id.size())); + + // Broadcasting the response + UAVCAN_TRACE("DynamicNodeIDAllocationClient", + "Broadcasting 2nd stage: preferred ID: %d, size of unique ID: %d", + static_cast(preferred_node_id_.get()), static_cast(second_stage.unique_id.size())); + const int res = dnida_pub_.broadcast(second_stage); + if (res < 0) + { + dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocationClient request failed"); + } } - UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Short unique node ID: 0x%016llx, preferred node ID: %d", - static_cast(short_unique_id_), static_cast(preferred_node_id_.get())); - - allocated_node_id_ = NodeID(); - UAVCAN_ASSERT(!allocated_node_id_.isUnicast()); - UAVCAN_ASSERT(!allocated_node_id_.isValid()); - - int res = dnida_pub_.init(); - if (res < 0) + // Rule E + if (!msg.isAnonymousTransfer() && + msg.unique_id.size() == msg.unique_id.capacity() && + equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_) && + msg.node_id > 0) { - return res; + allocated_node_id_ = msg.node_id; + allocator_node_id_ = msg.getSrcNodeID(); + UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Allocation complete, node ID %d provided by %d", + static_cast(allocated_node_id_.get()), static_cast(allocator_node_id_.get())); + terminate(); + UAVCAN_ASSERT(isAllocationComplete()); } - dnida_pub_.allowAnonymousTransfers(); - - res = dnida_sub_.start( - DynamicNodeIDAllocationCallback(this, &DynamicNodeIDAllocationClient::handleDynamicNodeIDAllocation)); - if (res < 0) - { - return res; - } - dnida_sub_.allowAnonymousTransfers(); - - startPeriodic( - MonotonicDuration::fromMSec(protocol::DynamicNodeIDAllocation::ALLOCATEE_MIN_BROADCAST_INTERVAL_SEC * 1000)); - - return 0; -} - -int DynamicNodeIDAllocationClient::start(const uint64_t short_unique_node_id, const NodeID preferred_node_id) -{ - short_unique_id_ = short_unique_node_id; - preferred_node_id_ = preferred_node_id; - return startImpl(); } int DynamicNodeIDAllocationClient::start(const protocol::HardwareVersion& hardware_version, const NodeID preferred_node_id) { - // Checking if unique ID is set - bool unique_id_is_zero = true; - for (uint8_t i = 0; i < hardware_version.unique_id.size(); i++) + terminate(); + + // Allocation is not possible if node ID is already set + if (dnida_pub_.getNode().getNodeID().isUnicast()) { - if (hardware_version.unique_id[i] != 0) + return -ErrLogic; + } + + // Unique ID initialization & validation + copy(hardware_version.unique_id.begin(), hardware_version.unique_id.end(), unique_id_); + bool unique_id_is_zero = true; + for (uint8_t i = 0; i < sizeof(unique_id_); i++) + { + if (unique_id_[i] != 0) { unique_id_is_zero = false; break; @@ -125,14 +141,37 @@ int DynamicNodeIDAllocationClient::start(const protocol::HardwareVersion& hardwa return -ErrInvalidParam; } - // Reducing the ID to 64 bits according to the specification - DataTypeSignatureCRC crc; - crc.add(hardware_version.unique_id.begin(), hardware_version.unique_id.size()); - short_unique_id_ = crc.get(); + if (!preferred_node_id.isValid()) // Only broadcast and unicast are allowed + { + return -ErrInvalidParam; + } + // Initializing the fields preferred_node_id_ = preferred_node_id; + allocated_node_id_ = NodeID(); + allocator_node_id_ = NodeID(); + UAVCAN_ASSERT(preferred_node_id_.isValid()); + UAVCAN_ASSERT(!allocated_node_id_.isValid()); + UAVCAN_ASSERT(!allocator_node_id_.isValid()); - return startImpl(); + // Initializing node objects - Rule A + int res = dnida_pub_.init(); + if (res < 0) + { + return res; + } + dnida_pub_.allowAnonymousTransfers(); + + res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDAllocationClient::handleAllocation)); + if (res < 0) + { + return res; + } + dnida_sub_.allowAnonymousTransfers(); + + startPeriodic(MonotonicDuration::fromMSec(protocol::dynamic_node_id::Allocation::DEFAULT_REQUEST_PERIOD_MS)); + + return 0; } } diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp index 5806008291..ecf84ec493 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp @@ -15,118 +15,150 @@ TEST(DynamicNodeIDAllocationClient, Basic) uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b); uavcan::GlobalDataTypeRegistry::instance().reset(); - uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg1; + (void)_reg1; + /* + * Client initialization + */ uavcan::protocol::HardwareVersion hwver; ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver)); // Empty hardware version is not allowed - /* - * Initializing. - * Reduced signature was calculated as follows: - * >>> crc = pyuavcan.dsdl.signature.Signature() - * >>> crc.add(range(16)) - * >>> crc.get_value() - * 4539764000456687298L - * >>> hex(crc.get_value()) - * '0x3f007b4e4353bec2L' - */ - const uavcan::uint64_t UniqueID64Bit = 0x3f007b4e4353bec2ULL; - const uavcan::uint64_t UniqueID57Bit = UniqueID64Bit & ((1ULL << 57) - 1U); for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++) { hwver.unique_id[i] = i; } - // More incorrect inputs - ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(0)); - ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver, uavcan::NodeID::Broadcast)); // Bad node ID - ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver, uavcan::NodeID())); // Ditto + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver, uavcan::NodeID())); const uavcan::NodeID PreferredNodeID = 42; ASSERT_LE(0, dnidac.start(hwver, PreferredNodeID)); - // Making sure the signature reduction was performed correctly - ASSERT_EQ(UniqueID57Bit, dnidac.getShortUniqueNodeID()); - ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + ASSERT_FALSE(dnidac.isAllocationComplete()); /* - * Initializing subscriber - * Anonymous transfers must be enabled + * Subscriber (server emulation) */ - SubscriberWithCollector dynid_sub(nodes.a); + SubscriberWithCollector dynid_sub(nodes.a); ASSERT_LE(0, dynid_sub.start()); dynid_sub.subscriber.allowAnonymousTransfers(); /* * Monitoring requests at 1Hz - * First request will be sent immediately */ nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); ASSERT_TRUE(dynid_sub.collector.msg.get()); - ASSERT_EQ(UniqueID57Bit, dynid_sub.collector.msg->short_unique_id); + std::cout << "First-stage request:\n" << *dynid_sub.collector.msg << std::endl; ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + ASSERT_TRUE(dynid_sub.collector.msg->first_part_of_unique_id); + ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), + dynid_sub.collector.msg->unique_id.end(), + hwver.unique_id.begin())); dynid_sub.collector.msg.reset(); // Rate validation - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500)); ASSERT_FALSE(dynid_sub.collector.msg.get()); // Second - rate is 1 Hz - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(900)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500)); ASSERT_TRUE(dynid_sub.collector.msg.get()); - ASSERT_EQ(UniqueID57Bit, dynid_sub.collector.msg->short_unique_id); - ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); dynid_sub.collector.msg.reset(); ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + ASSERT_FALSE(dnidac.isAllocationComplete()); /* - * Sending a bunch of responses - * Note that response transfers are NOT anonymous + * Publisher (server emulation) */ - uavcan::Publisher dynid_pub(nodes.a); + uavcan::Publisher dynid_pub(nodes.a); ASSERT_LE(0, dynid_pub.init()); - uavcan::protocol::DynamicNodeIDAllocation msg; - - msg.short_unique_id = 123; // garbage - msg.node_id = 100; // oh whatever - ASSERT_LE(0, dynid_pub.broadcast(msg)); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); - - msg.short_unique_id = 0; // garbage - msg.node_id = 101; - ASSERT_LE(0, dynid_pub.broadcast(msg)); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); - - msg.short_unique_id = UniqueID57Bit; // correct ID - msg.node_id = 102; // THIS NODE ID WILL BE USED - ASSERT_LE(0, dynid_pub.broadcast(msg)); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); - - msg.short_unique_id = UniqueID57Bit; // repeating, will be ignored - msg.node_id = 103; - ASSERT_LE(0, dynid_pub.broadcast(msg)); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); + /* + * Sending some some Allocation messages - the timer will keep restarting + */ + for (int i = 0; i < 10; i++) + { + uavcan::protocol::dynamic_node_id::Allocation msg; // Contents of the message doesn't matter + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(210)); + ASSERT_FALSE(dynid_sub.collector.msg.get()); + } /* - * Validating the results + * Responding with partially matching unique ID - the client will respond with second-stage request immediately */ - ASSERT_FALSE(dynid_sub.collector.msg.get()); + { + uavcan::protocol::dynamic_node_id::Allocation msg; + msg.unique_id.resize(7); + uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.begin() + 7, msg.unique_id.begin()); - ASSERT_TRUE(dnidac.getAllocatedNodeID().isUnicast()); - ASSERT_TRUE(dnidac.getAllocatorNodeID().isUnicast()); + std::cout << "First-stage offer:\n" << msg << std::endl; - ASSERT_EQ(102, dnidac.getAllocatedNodeID().get()); - ASSERT_EQ(10, dnidac.getAllocatorNodeID().get()); + ASSERT_FALSE(dynid_sub.collector.msg.get()); + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); - // Making sure requests have stopped - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); - ASSERT_FALSE(dynid_sub.collector.msg.get()); + ASSERT_TRUE(dynid_sub.collector.msg.get()); + std::cout << "Second-stage request:\n" << *dynid_sub.collector.msg << std::endl; + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id); + ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), + dynid_sub.collector.msg->unique_id.end(), + hwver.unique_id.begin() + 7)); + dynid_sub.collector.msg.reset(); + } + + /* + * Responding with second-stage offer, expecting the last request back + */ + { + uavcan::protocol::dynamic_node_id::Allocation msg; + msg.unique_id.resize(14); + uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.begin() + 14, msg.unique_id.begin()); + + std::cout << "Second-stage offer:\n" << msg << std::endl; + + ASSERT_FALSE(dynid_sub.collector.msg.get()); + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + + ASSERT_TRUE(dynid_sub.collector.msg.get()); + std::cout << "Last request:\n" << *dynid_sub.collector.msg << std::endl; + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id); + ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), + dynid_sub.collector.msg->unique_id.end(), + hwver.unique_id.begin() + 14)); + dynid_sub.collector.msg.reset(); + } + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + ASSERT_FALSE(dnidac.isAllocationComplete()); + + /* + * Now we have full unique ID for this client received, and it is possible to grant allocation + */ + { + uavcan::protocol::dynamic_node_id::Allocation msg; + msg.unique_id.resize(16); + msg.node_id = 72; + uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.end(), msg.unique_id.begin()); + + ASSERT_FALSE(dynid_sub.collector.msg.get()); + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + ASSERT_FALSE(dynid_sub.collector.msg.get()); + } + + ASSERT_EQ(uavcan::NodeID(72), dnidac.getAllocatedNodeID()); + ASSERT_EQ(uavcan::NodeID(10), dnidac.getAllocatorNodeID()); + ASSERT_TRUE(dnidac.isAllocationComplete()); } @@ -137,7 +169,14 @@ TEST(DynamicNodeIDAllocationClient, NonPassiveMode) uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b); uavcan::GlobalDataTypeRegistry::instance().reset(); - uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg1; + (void)_reg1; - ASSERT_LE(-uavcan::ErrLogic, dnidac.start(123456789)); + uavcan::protocol::HardwareVersion hwver; + for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++) + { + hwver.unique_id[i] = i; + } + + ASSERT_LE(-uavcan::ErrLogic, dnidac.start(hwver)); }