fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 03:40:35 +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
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp
+103 -64
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));
}