fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 19:54:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

New dynamic node ID allocation client

Browse Source
This commit is contained in:
Pavel Kirienko 2015-04-26 07:42:26 +03:00
parent edf556a9f2
commit abea24eeec
5 changed files with 323 additions and 342 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

147
dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan
View File

@ -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

64
dsdl/uavcan/protocol/dynamic_node_id/559.Allocation.uavcan Normal file
View File

@ -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

68
libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp
View File

@ -10,7 +10,7 @@
#include <uavcan/node/timer.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/build_config.hpp>
#include <uavcan/protocol/DynamicNodeIDAllocation.hpp>
#include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
#include <uavcan/protocol/HardwareVersion.hpp>
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<DynamicNodeIDAllocationClient*,
void (DynamicNodeIDAllocationClient::*)
(const ReceivedDataStructure<protocol::DynamicNodeIDAllocation>&)>
DynamicNodeIDAllocationCallback;
(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>&)>
AllocationCallback;
Publisher<protocol::DynamicNodeIDAllocation> dnida_pub_;
Subscriber<protocol::DynamicNodeIDAllocation, DynamicNodeIDAllocationCallback> dnida_sub_;
Publisher<protocol::dynamic_node_id::Allocation> dnida_pub_;
Subscriber<protocol::dynamic_node_id::Allocation, AllocationCallback> 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<protocol::DynamicNodeIDAllocation>& msg);
int startImpl();
void handleAllocation(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& 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_; }
};
}

219
libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp
View File

@ -3,117 +3,133 @@
*/
#include <uavcan/protocol/dynamic_node_id_allocation_client.hpp>
#include <uavcan/data_type.hpp> // 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<unsigned long long>(short_unique_id_),
static_cast<int>(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<protocol::DynamicNodeIDAllocation>& 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<int>(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<int>(preferred_node_id_.get()),
static_cast<int>(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<int>(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<protocol::dynamic_node_id::Allocation>& 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<int>(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<uint8_t>(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<int>(preferred_node_id_.get()), static_cast<int>(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<unsigned long long>(short_unique_id_), static_cast<int>(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<int>(allocated_node_id_.get()), static_cast<int>(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;
}
}

167
libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp
View File

@ -15,118 +15,150 @@ TEST(DynamicNodeIDAllocationClient, Basic)
uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b);
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::DynamicNodeIDAllocation> _reg1;
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::Allocation> _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<uavcan::protocol::DynamicNodeIDAllocation> dynid_sub(nodes.a);
SubscriberWithCollector<uavcan::protocol::dynamic_node_id::Allocation> 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<uavcan::protocol::DynamicNodeIDAllocation> dynid_pub(nodes.a);
uavcan::Publisher<uavcan::protocol::dynamic_node_id::Allocation> 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<uavcan::protocol::DynamicNodeIDAllocation> _reg1;
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::Allocation> _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));
}