fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-08 10:20: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/src/protocol/uc_dynamic_node_id_allocation_client.cpp
+129 -90
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;
}
}