uc_dynamic_node_id_client.cpp

00001 /*
00002  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <cstdlib>
00006 #include <uavcan/protocol/dynamic_node_id_client.hpp>
00007 
00008 namespace uavcan
00009 {
00010 
00011 void DynamicNodeIDClient::terminate()
00012 {
00013     UAVCAN_TRACE("DynamicNodeIDClient", "Client terminated");
00014     stop();
00015     dnida_sub_.stop();
00016 }
00017 
00018 MonotonicDuration DynamicNodeIDClient::getRandomDuration(uint32_t lower_bound_msec, uint32_t upper_bound_msec)
00019 {
00020     UAVCAN_ASSERT(upper_bound_msec > lower_bound_msec);
00021     // coverity[dont_call]
00022     return MonotonicDuration::fromMSec(lower_bound_msec +
00023                                        static_cast<uint32_t>(std::rand()) % (upper_bound_msec - lower_bound_msec));
00024 }
00025 
00026 void DynamicNodeIDClient::restartTimer(const Mode mode)
00027 {
00028     UAVCAN_ASSERT(mode < NumModes);
00029     UAVCAN_ASSERT((mode == ModeWaitingForTimeSlot) == (size_of_received_unique_id_ == 0));
00030 
00031     const MonotonicDuration delay = (mode == ModeWaitingForTimeSlot) ?
00032         getRandomDuration(protocol::dynamic_node_id::Allocation::MIN_REQUEST_PERIOD_MS,
00033                           protocol::dynamic_node_id::Allocation::MAX_REQUEST_PERIOD_MS) :
00034         getRandomDuration(protocol::dynamic_node_id::Allocation::MIN_FOLLOWUP_DELAY_MS,
00035                           protocol::dynamic_node_id::Allocation::MAX_FOLLOWUP_DELAY_MS);
00036 
00037     startOneShotWithDelay(delay);
00038 
00039     UAVCAN_TRACE("DynamicNodeIDClient", "Restart mode %d, delay %d ms",
00040                  static_cast<int>(mode), static_cast<int>(delay.toMSec()));
00041 }
00042 
00043 void DynamicNodeIDClient::handleTimerEvent(const TimerEvent&)
00044 {
00045     UAVCAN_ASSERT(preferred_node_id_.isValid());
00046     UAVCAN_ASSERT(size_of_received_unique_id_ < protocol::dynamic_node_id::Allocation::FieldTypes::unique_id::MaxSize);
00047 
00048     if (isAllocationComplete())
00049     {
00050         UAVCAN_ASSERT(0);
00051         terminate();
00052         return;
00053     }
00054 
00055     /*
00056      * Filling the message.
00057      */
00058     protocol::dynamic_node_id::Allocation tx;
00059     tx.node_id = preferred_node_id_.get();
00060     tx.first_part_of_unique_id = (size_of_received_unique_id_ == 0);
00061 
00062     const uint8_t size_of_unique_id_in_request =
00063         min(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST,
00064             static_cast<uint8_t>(tx.unique_id.capacity() - size_of_received_unique_id_));
00065 
00066     tx.unique_id.resize(size_of_unique_id_in_request);
00067     copy(unique_id_ + size_of_received_unique_id_,
00068          unique_id_ + size_of_received_unique_id_ + size_of_unique_id_in_request,
00069          tx.unique_id.begin());
00070 
00071     UAVCAN_ASSERT(equal(tx.unique_id.begin(), tx.unique_id.end(), unique_id_ + size_of_received_unique_id_));
00072 
00073     /*
00074      * Resetting the state - this way we can continue with a first stage request on the next attempt.
00075      */
00076     size_of_received_unique_id_ = 0;
00077     restartTimer(ModeWaitingForTimeSlot);
00078 
00079     /*
00080      * Broadcasting the message.
00081      */
00082     UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting; preferred ID %d, size of UID %d",
00083                  static_cast<int>(preferred_node_id_.get()),
00084                  static_cast<int>(tx.unique_id.size()));
00085     const int res = dnida_pub_.broadcast(tx);
00086     if (res < 0)
00087     {
00088         dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed");
00089     }
00090 }
00091 
00092 void DynamicNodeIDClient::handleAllocation(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& msg)
00093 {
00094     UAVCAN_ASSERT(preferred_node_id_.isValid());
00095     if (isAllocationComplete())
00096     {
00097         UAVCAN_ASSERT(0);
00098         terminate();
00099         return;
00100     }
00101 
00102     UAVCAN_TRACE("DynamicNodeIDClient", "Allocation message from %d, %d bytes of unique ID, node ID %d",
00103                  static_cast<int>(msg.getSrcNodeID().get()), static_cast<int>(msg.unique_id.size()),
00104                  static_cast<int>(msg.node_id));
00105 
00106     /*
00107      * Switching to passive state by default; will switch to active state if response matches.
00108      */
00109     size_of_received_unique_id_ = 0;
00110     restartTimer(ModeWaitingForTimeSlot);
00111 
00112     /*
00113      * Filtering out anonymous and invalid messages.
00114      */
00115     const bool full_response = (msg.unique_id.size() == msg.unique_id.capacity());
00116 
00117     if (msg.isAnonymousTransfer() ||
00118         msg.unique_id.empty() ||
00119         (full_response && (msg.node_id == 0)))
00120     {
00121         UAVCAN_TRACE("DynamicNodeIDClient", "Message from %d ignored", static_cast<int>(msg.getSrcNodeID().get()));
00122         return;
00123     }
00124 
00125     /*
00126      * If matches, either switch to active mode or complete the allocation.
00127      */
00128     if (equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_))
00129     {
00130         if (full_response)
00131         {
00132             allocated_node_id_ = msg.node_id;
00133             allocator_node_id_ = msg.getSrcNodeID();
00134             terminate();
00135             UAVCAN_ASSERT(isAllocationComplete());
00136             UAVCAN_TRACE("DynamicNodeIDClient", "Allocation complete, node ID %d provided by %d",
00137                          static_cast<int>(allocated_node_id_.get()), static_cast<int>(allocator_node_id_.get()));
00138         }
00139         else
00140         {
00141             size_of_received_unique_id_ = msg.unique_id.size();
00142             restartTimer(ModeDelayBeforeFollowup);
00143         }
00144     }
00145 }
00146 
00147 int DynamicNodeIDClient::start (const UniqueID& unique_id,
00148                                const NodeID preferred_node_id,
00149                                const TransferPriority transfer_priority)
00150 {
00151     terminate();
00152 
00153     // Allocation is not possible if node ID is already set
00154     if (dnida_pub_.getNode().getNodeID().isUnicast())
00155     {
00156         return -ErrLogic;
00157     }
00158 
00159     // Unique ID initialization & validation
00160     copy(unique_id.begin(), unique_id.end(), unique_id_);
00161     bool unique_id_is_zero = true;
00162     for (uint8_t i = 0; i < sizeof(unique_id_); i++)
00163     {
00164         if (unique_id_[i] != 0)
00165         {
00166             unique_id_is_zero = false;
00167             break;
00168         }
00169     }
00170 
00171     if (unique_id_is_zero)
00172     {
00173         return -ErrInvalidParam;
00174     }
00175 
00176     if (!preferred_node_id.isValid())  // Only broadcast and unicast are allowed
00177     {
00178         return -ErrInvalidParam;
00179     }
00180 
00181     // Initializing the fields
00182     preferred_node_id_ = preferred_node_id;
00183     allocated_node_id_ = NodeID();
00184     allocator_node_id_ = NodeID();
00185     UAVCAN_ASSERT(preferred_node_id_.isValid());
00186     UAVCAN_ASSERT(!allocated_node_id_.isValid());
00187     UAVCAN_ASSERT(!allocator_node_id_.isValid());
00188 
00189     // Initializing node objects - Rule A
00190     int res = dnida_pub_.init();
00191     if (res < 0)
00192     {
00193         return res;
00194     }
00195     dnida_pub_.allowAnonymousTransfers();
00196     dnida_pub_.setPriority(transfer_priority);
00197 
00198     res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDClient::handleAllocation));
00199     if (res < 0)
00200     {
00201         return res;
00202     }
00203     dnida_sub_.allowAnonymousTransfers();
00204 
00205     restartTimer(ModeWaitingForTimeSlot);
00206 
00207     return 0;
00208 }
00209 
00210 }