node_id_selector.hpp

00001 /*
00002  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED
00006 #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED
00007 
00008 #include <uavcan/build_config.hpp>
00009 #include <cassert>
00010 
00011 namespace uavcan
00012 {
00013 namespace dynamic_node_id_server
00014 {
00015 /**
00016  * Node ID allocation logic
00017  */
00018 template <typename Owner>
00019 class NodeIDSelector
00020 {
00021     typedef bool (Owner::*IsNodeIDTakenMethod)(const NodeID node_id) const;
00022 
00023     const Owner* const owner_;
00024     const IsNodeIDTakenMethod is_node_id_taken_;
00025 
00026 public:
00027     NodeIDSelector(const Owner* owner, IsNodeIDTakenMethod is_node_id_taken)
00028         : owner_(owner)
00029         , is_node_id_taken_(is_node_id_taken)
00030     {
00031         UAVCAN_ASSERT(owner_ != UAVCAN_NULLPTR);
00032         UAVCAN_ASSERT(is_node_id_taken_ != UAVCAN_NULLPTR);
00033     }
00034 
00035     /**
00036      * Reutrns a default-constructed (invalid) node ID if a free one could not be found.
00037      */
00038     NodeID findFreeNodeID(const NodeID preferred) const
00039     {
00040         uint8_t candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes;
00041 
00042         // Up
00043         while (candidate <= NodeID::MaxRecommendedForRegularNodes)
00044         {
00045             if (!(owner_->*is_node_id_taken_)(candidate))
00046             {
00047                 return candidate;
00048             }
00049             candidate++;
00050         }
00051 
00052         candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes;
00053 
00054         // Down
00055         while (candidate > 0)
00056         {
00057             if (!(owner_->*is_node_id_taken_)(candidate))
00058             {
00059                 return candidate;
00060             }
00061             candidate--;
00062         }
00063 
00064         return NodeID();
00065     }
00066 };
00067 
00068 }
00069 }
00070 
00071 #endif // Include guard