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

DynamicNodeIDAllocationClient

Browse Source
This commit is contained in:
Pavel Kirienko 2015-04-07 00:47:02 +03:00
parent 012765a796
commit 042aa60773
3 changed files with 386 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp Normal file
View File

@ -0,0 +1,105 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED
#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED
#include <uavcan/node/subscriber.hpp>
#include <uavcan/node/publisher.hpp>
#include <uavcan/node/timer.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/build_config.hpp>
#include <uavcan/protocol/DynamicNodeIDAllocation.hpp>
#include <uavcan/protocol/HardwareVersion.hpp>
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<DynamicNodeIDAllocationClient*,
void (DynamicNodeIDAllocationClient::*)
(const ReceivedDataStructure<protocol::DynamicNodeIDAllocation>&)>
DynamicNodeIDAllocationCallback;
Publisher<protocol::DynamicNodeIDAllocation> dnida_pub_;
Subscriber<protocol::DynamicNodeIDAllocation, DynamicNodeIDAllocationCallback> 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<protocol::DynamicNodeIDAllocation>& 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

138
libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp Normal file
View File

@ -0,0 +1,138 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/protocol/dynamic_node_id_allocation_client.hpp>
#include <uavcan/data_type.hpp> // 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<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");
}
}
void DynamicNodeIDAllocationClient::handleDynamicNodeIDAllocation(
const ReceivedDataStructure<protocol::DynamicNodeIDAllocation>& 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<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");
return;
}
allocated_node_id_ = allocation;
allocator_node_id_ = msg.getSrcNodeID();
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()),
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())
{
// 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<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)
{
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();
}
}

143
libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp Normal file
View File

@ -0,0 +1,143 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/protocol/dynamic_node_id_allocation_client.hpp>
#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<uavcan::protocol::DynamicNodeIDAllocation> _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<uavcan::protocol::DynamicNodeIDAllocation> 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<uavcan::protocol::DynamicNodeIDAllocation> 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<uavcan::protocol::DynamicNodeIDAllocation> _reg1;
ASSERT_LE(-uavcan::ErrLogic, dnidac.start(123456789));
}