diff --git a/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl b/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl index 305d715dc5..12bd65817e 100644 --- a/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl +++ b/libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl @@ -33,19 +33,11 @@ ${line} #undef ${a.name} % endfor -#ifndef UAVCAN_PACK_STRUCTS -# error UAVCAN_PACK_STRUCTS -#endif - % for nsc in t.cpp_namespace_components: namespace ${nsc} { % endfor -#if UAVCAN_PACK_STRUCTS -UAVCAN_PACKED_BEGIN -#endif - % if t.kind != t.KIND_SERVICE: template % endif @@ -178,10 +170,6 @@ private: % endif }; -#if UAVCAN_PACK_STRUCTS -UAVCAN_PACKED_END -#endif - /* * Out of line struct method definitions */ diff --git a/libuavcan/include/uavcan/build_config.hpp b/libuavcan/include/uavcan/build_config.hpp index 0ce6575bf1..6b8768c3e5 100644 --- a/libuavcan/include/uavcan/build_config.hpp +++ b/libuavcan/include/uavcan/build_config.hpp @@ -82,21 +82,6 @@ # endif #endif -/** - * Struct layout control. - * Set UAVCAN_PACK_STRUCTS=1 and define UAVCAN_PACKED_BEGIN and UAVCAN_PACKED_END to reduce memory usage. - * THIS MAY BREAK THE CODE. - */ -#ifndef UAVCAN_PACK_STRUCTS -# define UAVCAN_PACK_STRUCTS 0 -#endif -#ifndef UAVCAN_PACKED_BEGIN -# define UAVCAN_PACKED_BEGIN -#endif -#ifndef UAVCAN_PACKED_END -# define UAVCAN_PACKED_END -#endif - /** * Declaration visibility * http://gcc.gnu.org/wiki/Visibility @@ -228,6 +213,35 @@ static const unsigned FloatComparisonEpsilonMult = UAVCAN_FLOAT_COMPARISON_EPSIL static const unsigned FloatComparisonEpsilonMult = 10; #endif +/** + * Maximum number of CAN acceptance filters available on the platform + */ +#ifdef UAVCAN_MAX_CAN_ACCEPTANCE_FILTERS +/// Explicitly specified by the user. +static const unsigned MaxCanAcceptanceFilters = UAVCAN_MAX_CAN_ACCEPTANCE_FILTERS; +#else +/// Default that should be OK for any platform. +static const unsigned MaxCanAcceptanceFilters = 32; +#endif + +/** + * This constant defines how many receiver objects will be statically pre-allocated by classes that listen to messages + * of type uavcan.protocol.NodeStatus from other nodes. If the number of publishers exceeds this value, extra + * listeners will be allocated in the dynamic memory (one block per listener). + * + * The default value is safe to use in any application (since extra memory can always be retrieved from the pool). + * Applications that are extremely memory sensitive and are not expected to operate in large networks can override + * the default with a lower value. + * + * Note that nodes that aren't using classes that monitor the network (e.g. NodeStatusMonitor, dynamic node ID + * allocation servers) do not depend on this configuration parameter in any way. + */ +#ifdef UAVCAN_MAX_NETWORK_SIZE_HINT +static const unsigned MaxNetworkSizeHint = UAVCAN_MAX_NETWORK_SIZE_HINT; +#else +static const unsigned MaxNetworkSizeHint = 64; ///< Rest will go into the dynamic memory +#endif + } #endif // UAVCAN_BUILD_CONFIG_HPP_INCLUDED diff --git a/libuavcan/include/uavcan/data_type.hpp b/libuavcan/include/uavcan/data_type.hpp index f8f752ac8d..c9598ee6d1 100644 --- a/libuavcan/include/uavcan/data_type.hpp +++ b/libuavcan/include/uavcan/data_type.hpp @@ -131,24 +131,24 @@ public: */ class UAVCAN_EXPORT DataTypeDescriptor { - DataTypeKind kind_; - DataTypeID id_; DataTypeSignature signature_; const char* full_name_; + DataTypeKind kind_; + DataTypeID id_; public: static const unsigned MaxFullNameLen = 80; - DataTypeDescriptor() - : kind_(DataTypeKind(0)) - , full_name_("") + DataTypeDescriptor() : + full_name_(""), + kind_(DataTypeKind(0)) { } - DataTypeDescriptor(DataTypeKind kind, DataTypeID id, const DataTypeSignature& signature, const char* name) - : kind_(kind) - , id_(id) - , signature_(signature) - , full_name_(name) + DataTypeDescriptor(DataTypeKind kind, DataTypeID id, const DataTypeSignature& signature, const char* name) : + signature_(signature), + full_name_(name), + kind_(kind), + id_(id) { UAVCAN_ASSERT(kind < NumDataTypeKinds); UAVCAN_ASSERT(name); diff --git a/libuavcan/include/uavcan/driver/can.hpp b/libuavcan/include/uavcan/driver/can.hpp index 04a9363b5b..64fab68599 100644 --- a/libuavcan/include/uavcan/driver/can.hpp +++ b/libuavcan/include/uavcan/driver/can.hpp @@ -16,7 +16,6 @@ namespace uavcan /** * Raw CAN frame, as passed to/from the CAN driver. */ -UAVCAN_PACKED_BEGIN struct UAVCAN_EXPORT CanFrame { static const uint32_t MaskStdID = 0x000007FFU; @@ -31,16 +30,16 @@ struct UAVCAN_EXPORT CanFrame uint8_t data[MaxDataLen]; uint8_t dlc; ///< Data Length Code - CanFrame() - : id(0) - , dlc(0) + CanFrame() : + id(0), + dlc(0) { fill(data, data + MaxDataLen, uint8_t(0)); } - CanFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len) - : id(can_id) - , dlc((data_len > MaxDataLen) ? MaxDataLen : data_len) + CanFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len) : + id(can_id), + dlc((data_len > MaxDataLen) ? MaxDataLen : data_len) { UAVCAN_ASSERT(can_data != NULL); UAVCAN_ASSERT(data_len == dlc); @@ -63,7 +62,9 @@ struct UAVCAN_EXPORT CanFrame StrTight, ///< Minimum string length (default) StrAligned ///< Fixed formatting for any frame }; + std::string toString(StringRepresentation mode = StrTight) const; + #endif /** @@ -74,7 +75,6 @@ struct UAVCAN_EXPORT CanFrame bool priorityHigherThan(const CanFrame& rhs) const; bool priorityLowerThan(const CanFrame& rhs) const { return rhs.priorityHigherThan(*this); } }; -UAVCAN_PACKED_END /** * CAN hardware filter config struct. @@ -86,15 +86,14 @@ struct UAVCAN_EXPORT CanFilterConfig uint32_t id; uint32_t mask; - //bool operator == (const CanFilterConfig&) const; bool operator==(const CanFilterConfig& rhs) const { return rhs.id == id && rhs.mask == mask; } - CanFilterConfig() - : id(0) - , mask(0) + CanFilterConfig() : + id(0), + mask(0) { } }; @@ -107,9 +106,9 @@ struct UAVCAN_EXPORT CanSelectMasks uint8_t read; uint8_t write; - CanSelectMasks() - : read(0) - , write(0) + CanSelectMasks() : + read(0), + write(0) { } }; @@ -178,7 +177,11 @@ public: * Returns an interface by index, or null pointer if the index is out of range. */ virtual ICanIface* getIface(uint8_t iface_index) = 0; - virtual const ICanIface* getIface(uint8_t iface_index) const = 0; + + virtual const ICanIface* getIface(uint8_t iface_index) const + { + return const_cast(this)->getIface(iface_index); + } /** * Total number of available CAN interfaces. diff --git a/libuavcan/include/uavcan/dynamic_memory.hpp b/libuavcan/include/uavcan/dynamic_memory.hpp index 813d5480be..5d440724d6 100644 --- a/libuavcan/include/uavcan/dynamic_memory.hpp +++ b/libuavcan/include/uavcan/dynamic_memory.hpp @@ -26,55 +26,13 @@ public: virtual void* allocate(std::size_t size) = 0; virtual void deallocate(const void* ptr) = 0; - virtual bool isInPool(const void* ptr) const = 0; - - virtual std::size_t getBlockSize() const = 0; - virtual std::size_t getNumBlocks() const = 0; -}; - -/** - * Pool manager contains multiple pool allocators of different block sizes and - * finds the most suitable allocator for every allocation request. - */ -template -class UAVCAN_EXPORT PoolManager : public IPoolAllocator, Noncopyable -{ - IPoolAllocator* pools_[MaxPools]; - - static int qsortComparePoolAllocators(const void* raw_a, const void* raw_b) - { - const IPoolAllocator* const a = *static_cast(raw_a); - const IPoolAllocator* const b = *static_cast(raw_b); - const std::size_t a_size = a ? a->getBlockSize() : NumericTraits::max(); - const std::size_t b_size = b ? b->getBlockSize() : NumericTraits::max(); - if (a_size != b_size) - { - return (a_size > b_size) ? 1 : -1; - } - return 0; - } - -public: - PoolManager() - { - (void)std::memset(pools_, 0, sizeof(pools_)); - } - - bool addPool(IPoolAllocator* pool); - - virtual void* allocate(std::size_t size); - virtual void deallocate(const void* ptr); - - virtual bool isInPool(const void* ptr) const; - - virtual std::size_t getBlockSize() const; - virtual std::size_t getNumBlocks() const; + virtual uint16_t getNumBlocks() const = 0; }; /** * Classic implementation of a pool allocator (Meyers). */ -template +template class UAVCAN_EXPORT PoolAllocator : public IPoolAllocator, Noncopyable { union Node @@ -92,21 +50,29 @@ class UAVCAN_EXPORT PoolAllocator : public IPoolAllocator, Noncopyable Node _aligner3; } pool_; + uint16_t used_; + uint16_t max_used_; + public: - static const unsigned NumBlocks = unsigned(PoolSize / BlockSize); + static const uint16_t NumBlocks = PoolSize / BlockSize; PoolAllocator(); virtual void* allocate(std::size_t size); virtual void deallocate(const void* ptr); - virtual bool isInPool(const void* ptr) const; + virtual uint16_t getNumBlocks() const { return NumBlocks; } - virtual std::size_t getBlockSize() const { return BlockSize; } - virtual std::size_t getNumBlocks() const { return NumBlocks; } + /** + * Return the number of blocks that are currently allocated/unallocated. + */ + uint16_t getNumUsedBlocks() const { return used_; } + uint16_t getNumFreeBlocks() const { return static_cast(NumBlocks - used_); } - unsigned getNumFreeBlocks() const; - unsigned getNumUsedBlocks() const { return NumBlocks - getNumFreeBlocks(); } + /** + * Returns the maximum number of blocks that were ever allocated at the same time. + */ + uint16_t getPeakNumUsedBlocks() const { return max_used_; } }; /** @@ -115,13 +81,13 @@ public: class LimitedPoolAllocator : public IPoolAllocator { IPoolAllocator& allocator_; - const std::size_t max_blocks_; - std::size_t used_blocks_; + const uint16_t max_blocks_; + uint16_t used_blocks_; public: LimitedPoolAllocator(IPoolAllocator& allocator, std::size_t max_blocks) : allocator_(allocator) - , max_blocks_(max_blocks) + , max_blocks_(static_cast(min(max_blocks, 0xFFFFU))) , used_blocks_(0) { UAVCAN_ASSERT(max_blocks_ > 0); @@ -130,118 +96,26 @@ public: virtual void* allocate(std::size_t size); virtual void deallocate(const void* ptr); - virtual bool isInPool(const void* ptr) const; - - virtual std::size_t getBlockSize() const; - virtual std::size_t getNumBlocks() const; + virtual uint16_t getNumBlocks() const; }; // ---------------------------------------------------------------------------- -/* - * PoolManager<> - */ -template -bool PoolManager::addPool(IPoolAllocator* pool) -{ - UAVCAN_ASSERT(pool); - bool retval = false; - for (unsigned i = 0; i < MaxPools; i++) - { - UAVCAN_ASSERT(pools_[i] != pool); - if (pools_[i] == NULL || pools_[i] == pool) - { - pools_[i] = pool; - retval = true; - break; - } - } - // We need to keep the pools in order, so that smallest blocks go first - qsort(pools_, MaxPools, sizeof(IPoolAllocator*), &PoolManager::qsortComparePoolAllocators); - return retval; -} - -template -void* PoolManager::allocate(std::size_t size) -{ - for (unsigned i = 0; i < MaxPools; i++) - { - if (pools_[i] == NULL) - { - break; - } - void* const pmem = pools_[i]->allocate(size); - if (pmem != NULL) - { - return pmem; - } - } - return NULL; -} - -template -void PoolManager::deallocate(const void* ptr) -{ - for (unsigned i = 0; i < MaxPools; i++) - { - if (pools_[i] == NULL) - { - UAVCAN_ASSERT(0); - break; - } - if (pools_[i]->isInPool(ptr)) - { - pools_[i]->deallocate(ptr); - break; - } - } -} - -template -bool PoolManager::isInPool(const void* ptr) const -{ - for (unsigned i = 0; i < MaxPools; i++) - { - if (pools_[i] == NULL) - { - break; - } - if (pools_[i]->isInPool(ptr)) - { - return true; - } - } - return false; -} - -template -std::size_t PoolManager::getBlockSize() const -{ - return 0; -} - -template -std::size_t PoolManager::getNumBlocks() const -{ - std::size_t ret = 0; - for (unsigned i = 0; i < MaxPools; i++) - { - if (pools_[i] == NULL) - { - break; - } - ret += pools_[i]->getNumBlocks(); - } - return ret; -} - /* * PoolAllocator<> */ -template -PoolAllocator::PoolAllocator() - : free_list_(reinterpret_cast(pool_.bytes)) +template +const uint16_t PoolAllocator::NumBlocks; + +template +PoolAllocator::PoolAllocator() : + free_list_(reinterpret_cast(pool_.bytes)), + used_(0), + max_used_(0) { + // The limit is imposed by the width of the pool usage tracking variables. + StaticAssert<((PoolSize / BlockSize) <= 0xFFFFU)>::check(); + (void)std::memset(pool_.bytes, 0, PoolSize); for (unsigned i = 0; (i + 1) < (NumBlocks - 1 + 1); i++) // -Werror=type-limits { @@ -251,49 +125,43 @@ PoolAllocator::PoolAllocator() free_list_[NumBlocks - 1].next = NULL; } -template +template void* PoolAllocator::allocate(std::size_t size) { if (free_list_ == NULL || size > BlockSize) { return NULL; } + void* pmem = free_list_; free_list_ = free_list_->next; + + // Statistics + UAVCAN_ASSERT(used_ < NumBlocks); + used_++; + if (used_ > max_used_) + { + max_used_ = used_; + } + return pmem; } -template +template void PoolAllocator::deallocate(const void* ptr) { if (ptr == NULL) { return; } + Node* p = static_cast(const_cast(ptr)); p->next = free_list_; free_list_ = p; -} -template -bool PoolAllocator::isInPool(const void* ptr) const -{ - return ptr >= pool_.bytes && - ptr < (pool_.bytes + PoolSize); -} - -template -unsigned PoolAllocator::getNumFreeBlocks() const -{ - unsigned num = 0; - Node* p = free_list_; - while (p) - { - num++; - UAVCAN_ASSERT(num <= NumBlocks); - p = p->next; - } - return num; + // Statistics + UAVCAN_ASSERT(used_ > 0); + used_--; } } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp index de9b99ac61..6d8c37acd1 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp @@ -42,12 +42,12 @@ class Server : IAllocationRequestHandler */ bool isNodeIDTaken(const NodeID node_id) const { - return storage_.findByNodeID(node_id) != NULL; + return storage_.isNodeIDOccupied(node_id); } - void tryPublishAllocationResult(const Storage::Entry& entry) + void tryPublishAllocationResult(const NodeID node_id, const UniqueID& unique_id) { - const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id); + const int res = allocation_request_manager_.broadcastAllocationResponse(unique_id, node_id); if (res < 0) { tracer_.onEvent(TraceError, res); @@ -65,10 +65,10 @@ class Server : IAllocationRequestHandler virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id) { - const Storage::Entry* const e = storage_.findByUniqueID(unique_id); - if (e != NULL) + const NodeID existing_node_id = storage_.getNodeIDForUniqueID(unique_id); + if (existing_node_id.isValid()) { - tryPublishAllocationResult(*e); + tryPublishAllocationResult(existing_node_id, unique_id); } else { @@ -80,7 +80,7 @@ class Server : IAllocationRequestHandler const int res = storage_.add(allocated_node_id, unique_id); if (res >= 0) { - tryPublishAllocationResult(Storage::Entry(allocated_node_id, unique_id)); + tryPublishAllocationResult(allocated_node_id, unique_id); } else { @@ -105,12 +105,12 @@ class Server : IAllocationRequestHandler virtual NodeAwareness checkNodeAwareness(NodeID node_id) const { - return (storage_.findByNodeID(node_id) == NULL) ? NodeAwarenessUnknown : NodeAwarenessKnownAndCommitted; + return storage_.isNodeIDOccupied(node_id) ? NodeAwarenessKnownAndCommitted : NodeAwarenessUnknown; } virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) { - if (storage_.findByNodeID(node_id) != NULL) + if (storage_.isNodeIDOccupied(node_id)) { UAVCAN_ASSERT(0); // Such node is already known, the class that called this method should have known that return; @@ -152,25 +152,15 @@ public: own_unique_id_ = own_unique_id; { - const Storage::Entry* e = storage_.findByNodeID(node_.getNodeID()); - if (e != NULL) + const NodeID stored_own_node_id = storage_.getNodeIDForUniqueID(own_unique_id_); + if (stored_own_node_id.isValid()) { - if (e->unique_id != own_unique_id_) + if (stored_own_node_id != node_.getNodeID()) { return -ErrInvalidConfiguration; } } - - e = storage_.findByUniqueID(own_unique_id_); - if (e != NULL) - { - if (e->node_id != node_.getNodeID()) - { - return -ErrInvalidConfiguration; - } - } - - if (e == NULL) + else { res = storage_.add(node_.getNodeID(), own_unique_id_); if (res < 0) @@ -198,7 +188,7 @@ public: return 0; } - Storage::Size getNumAllocations() const { return storage_.getSize(); } + uint8_t getNumAllocations() const { return storage_.getSize(); } }; } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp index c03957d14f..e2f1685403 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp @@ -9,6 +9,7 @@ #include #include #include +#include namespace uavcan { @@ -22,148 +23,59 @@ namespace centralized */ class Storage { -public: - typedef uint8_t Size; + typedef BitSet OccupationMask; + typedef Array, ArrayModeStatic, + BitLenToByteLen::Result> + OccupationMaskArray; - enum { Capacity = NodeID::Max }; - - struct Entry - { - UniqueID unique_id; - NodeID node_id; - - Entry() { } - - Entry(const NodeID nid, const UniqueID& uid) - : unique_id(uid) - , node_id(nid) - { } - - bool operator==(const Entry& rhs) const - { - return unique_id == rhs.unique_id && - node_id == rhs.node_id; - } - }; - -private: IStorageBackend& storage_; - Entry entries_[Capacity]; - Size size_; + OccupationMask occupation_mask_; - static IStorageBackend::String getSizeKey() { return "size"; } + static IStorageBackend::String getOccupationMaskKey() { return "occupation_mask"; } - static IStorageBackend::String makeEntryKey(Size index, const char* postfix) + static OccupationMask maskFromArray(const OccupationMaskArray& array) { - IStorageBackend::String str; - // "0_foobar" - str.appendFormatted("%d", int(index)); - str += "_"; - str += postfix; - return str; + OccupationMask mask; + for (uint8_t byte = 0; byte < array.size(); byte++) + { + for (uint8_t bit = 0; bit < 8; bit++) + { + mask[byte * 8U + bit] = (array[byte] & (1U << bit)) != 0; + } + } + return mask; } - int readEntryFromStorage(Size index, Entry& out_entry) + static OccupationMaskArray maskToArray(const OccupationMask& mask) { - const StorageMarshaller io(storage_); - - // Unique ID - if (io.get(makeEntryKey(index, "unique_id"), out_entry.unique_id) < 0) + OccupationMaskArray array; + for (uint8_t byte = 0; byte < array.size(); byte++) { - return -ErrFailure; + for (uint8_t bit = 0; bit < 8; bit++) + { + if (mask[byte * 8U + bit]) + { + array[byte] |= static_cast(1U << bit); + } + } } - - // Node ID - uint32_t node_id = 0; - if (io.get(makeEntryKey(index, "node_id"), node_id) < 0) - { - return -ErrFailure; - } - if (node_id > NodeID::Max || node_id == 0) - { - return -ErrFailure; - } - out_entry.node_id = NodeID(static_cast(node_id)); - - return 0; - } - - int writeEntryToStorage(Size index, const Entry& entry) - { - Entry temp = entry; - - StorageMarshaller io(storage_); - - // Unique ID - if (io.setAndGetBack(makeEntryKey(index, "unique_id"), temp.unique_id) < 0) - { - return -ErrFailure; - } - - // Node ID - uint32_t node_id = entry.node_id.get(); - if (io.setAndGetBack(makeEntryKey(index, "node_id"), node_id) < 0) - { - return -ErrFailure; - } - temp.node_id = NodeID(static_cast(node_id)); - - return (temp == entry) ? 0 : -ErrFailure; + return array; } public: - Storage(IStorageBackend& storage) - : storage_(storage) - , size_(0) + Storage(IStorageBackend& storage) : + storage_(storage) { } /** - * This method reads all entries from the storage. + * This method reads only the occupation mask from the storage. */ int init() { StorageMarshaller io(storage_); - - // Reading size - size_ = 0; - { - uint32_t value = 0; - if (io.get(getSizeKey(), value) < 0) - { - if (storage_.get(getSizeKey()).empty()) - { - int res = io.setAndGetBack(getSizeKey(), value); - if (res < 0) - { - return res; - } - return (value == 0) ? 0 : -ErrFailure; - } - else - { - // There's some data in the storage, but it cannot be parsed - reporting an error - return -ErrFailure; - } - } - - if (value > Capacity) - { - return -ErrFailure; - } - - size_ = Size(value); - } - - // Restoring entries - for (Size index = 0; index < size_; index++) - { - const int result = readEntryFromStorage(index, entries_[index]); - if (result < 0) - { - return result; - } - } - + OccupationMaskArray array; + io.get(getOccupationMaskKey(), array); + occupation_mask_ = maskFromArray(array); return 0; } @@ -173,77 +85,62 @@ public: */ int add(const NodeID node_id, const UniqueID& unique_id) { - if (size_ == Capacity) - { - return -ErrLogic; - } - if (!node_id.isUnicast()) { return -ErrInvalidParam; } - Entry entry; - entry.node_id = node_id; - entry.unique_id = unique_id; + StorageMarshaller io(storage_); // If next operations fail, we'll get a dangling entry, but it's absolutely OK. - int res = writeEntryToStorage(size_, entry); - if (res < 0) { - return res; + uint32_t node_id_int = node_id.get(); + int res = io.setAndGetBack(StorageMarshaller::convertUniqueIDToHex(unique_id), node_id_int); + if (res < 0) + { + return res; + } + if (node_id_int != node_id.get()) + { + return -ErrFailure; + } } - // Updating the size - StorageMarshaller io(storage_); - uint32_t new_size_index = size_ + 1U; - res = io.setAndGetBack(getSizeKey(), new_size_index); + // Updating the mask in the storage + OccupationMask new_occupation_mask = occupation_mask_; + new_occupation_mask[node_id.get()] = true; + OccupationMaskArray occupation_array = maskToArray(new_occupation_mask); + + int res = io.setAndGetBack(getOccupationMaskKey(), occupation_array); if (res < 0) { return res; } - if (new_size_index != size_ + 1U) + if (occupation_array != maskToArray(new_occupation_mask)) { return -ErrFailure; } - entries_[size_] = entry; - size_++; + // Updating the cached mask only if the storage was updated successfully + occupation_mask_ = new_occupation_mask; return 0; } /** - * Returns nullptr if there's no such entry. + * Returns an invalid node ID if there's no such allocation. */ - const Entry* findByNodeID(const NodeID node_id) const + NodeID getNodeIDForUniqueID(const UniqueID& unique_id) const { - for (Size i = 0; i < size_; i++) - { - if (entries_[i].node_id == node_id) - { - return &entries_[i]; - } - } - return NULL; + StorageMarshaller io(storage_); + uint32_t node_id = 0; + io.get(StorageMarshaller::convertUniqueIDToHex(unique_id), node_id); + return (node_id > 0 && node_id <= NodeID::Max) ? NodeID(static_cast(node_id)) : NodeID(); } - /** - * Returns nullptr if there's no such entry. - */ - const Entry* findByUniqueID(const UniqueID& unique_id) const - { - for (Size i = 0; i < size_; i++) - { - if (entries_[i].unique_id == unique_id) - { - return &entries_[i]; - } - } - return NULL; - } + bool isNodeIDOccupied(NodeID node_id) const { return occupation_mask_[node_id.get()]; } - Size getSize() const { return size_; } + uint8_t getSize() const { return static_cast(occupation_mask_.count()); } }; } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index e5e4da5e1f..e9c58c12d8 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -94,8 +94,6 @@ class NodeDiscoverer : TimerBase enum { TimerPollIntervalMs = 170 }; // ~ ceil(500 ms service timeout / 3) - enum { NumNodeStatusStaticReceivers = 64 }; - /* * States */ @@ -106,7 +104,7 @@ class NodeDiscoverer : TimerBase NodeMap node_map_; ///< Will not work in UAVCAN_TINY ServiceClient get_node_info_client_; - Subscriber node_status_sub_; + Subscriber node_status_sub_; /* * Methods diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp index 09870d9192..296ff1e066 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp @@ -38,6 +38,20 @@ public: : storage_(storage) { } + /** + * Turns a unique ID into a hex string that can be used as a key or as a value. + */ + static IStorageBackend::String convertUniqueIDToHex(const UniqueID& key) + { + IStorageBackend::String serialized; + for (uint8_t i = 0; i < UniqueID::MaxSize; i++) + { + serialized.appendFormatted("%02x", key.at(i)); + } + UAVCAN_ASSERT(serialized.size() == UniqueID::MaxSize * 2); + return serialized; + } + /** * These methods set the value and then immediately read it back. * 1. Serialize the value. @@ -58,12 +72,7 @@ public: int setAndGetBack(const IStorageBackend::String& key, UniqueID& inout_value) { - IStorageBackend::String serialized; - for (uint8_t i = 0; i < UniqueID::MaxSize; i++) - { - serialized.appendFormatted("%02x", inout_value.at(i)); - } - UAVCAN_ASSERT(serialized.size() == UniqueID::MaxSize * 2); + const IStorageBackend::String serialized = convertUniqueIDToHex(inout_value); UAVCAN_TRACE("StorageMarshaller", "Set %s = %s", key.c_str(), serialized.c_str()); storage_.set(key, serialized); diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 017102fa40..00458efbea 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -365,6 +365,22 @@ public: return 0; } + /** + * This method forces the class to re-request uavcan.protocol.GetNodeInfo from all nodes as if they + * have just appeared in the network. + */ + void invalidateAll() + { + NodeStatusMonitor::forgetAllNodes(); + get_node_info_client_.cancelAllCalls(); + + for (unsigned i = 0; i < (sizeof(entries_) / sizeof(entries_[0])); i++) + { + entries_[i] = Entry(); + } + // It is not necessary to reset the last picked node index + } + /** * Adds one listener. Does nothing if such listener already exists. * May return -ErrMemory if there's no space to add the listener. diff --git a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp index 06f7bb7605..0e5c80aa68 100644 --- a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp +++ b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp @@ -51,12 +51,7 @@ private: typedef MethodBinder TimerCallback; - /* - * We'll be able to handle this many nodes in the network without any dynamic memory. - */ - enum { NumStaticReceivers = 64 }; - - Subscriber sub_; + Subscriber sub_; TimerEventForwarder timer_; @@ -200,6 +195,17 @@ public: } } + /** + * Make all nodes unknown. + */ + void forgetAllNodes() + { + for (unsigned i = 0; i < (sizeof(entries_) / sizeof(entries_[0])); i++) + { + entries_[i] = Entry(); + } + } + /** * Returns status of a given node. * Unknown nodes are considered offline. diff --git a/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp b/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp new file mode 100644 index 0000000000..2a0decf265 --- /dev/null +++ b/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2014 Pavel Kirienko , + * Ilia Sheremet + */ + +#ifndef UAVCAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED +#define UAVCAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +/** + * This class configures hardware acceptance filters (if this feature is present on the particular CAN driver) to + * preclude reception of irrelevant CAN frames on the hardware level. + * + * Configuration starts by creating an object of class @ref CanAcceptanceFilterConfigurator on the stack. + * Through the method configureFilters() it determines the number of available HW filters and the number + * of listeners. In case the number of listeners is higher than the number of available HW filters, the function + * automatically merges configs in the most efficient way until their number is reduced to the number of + * available HW filters. Subsequently obtained configurations are then loaded into the CAN driver. + * + * The maximum number of CAN acceptance filters is predefined in uavcan/build_config.hpp through a constant + * @ref MaxCanAcceptanceFilters. The algorithm doesn't allow to have higher number of HW filters configurations than + * defined by MaxCanAcceptanceFilters. You can change this value according to the number specified in your CAN driver + * datasheet. + */ +class CanAcceptanceFilterConfigurator +{ + /** + * Below constants based on UAVCAN transport layer specification. Masks and ID's depends on message Priority, + * TypeID, TransferID (RequestNotResponse - for service types, BroadcastNotUnicast - for message types). + * For more details refer to uavcan.org/CAN_bus_transport_layer_specification. + * For clarity let's represent "i" as Data Type ID + * DefaultFilterMsgMask = 00111111111110000000000000000 + * DefaultFilterMsgID = 00iiiiiiiiiii0000000000000000, no need to explicitly define, since MsgID initialized as 0. + * DefaultFilterServiceRequestMask = 11111111111100000000000000000 + * DefaultFilterServiceRequestID = 101iiiiiiiii00000000000000000 + * ServiceRespFrameMask = 11100000000000000000000000000 + * ServiceRespFrameID = 10000000000000000000000000000, all Service Response Frames are accepted by HW filters. + */ + static const unsigned DefaultFilterMsgMask = 0x7FF0000; + static const unsigned DefaultFilterServiceRequestID = 0x14000000; + static const unsigned DefaultFilterServiceRequestMask = 0x1FFE0000; + static const unsigned ServiceRespFrameID = 0x10000000; + static const unsigned ServiceRespFrameMask = 0x1C000000; + + typedef uavcan::Multiset MultisetConfigContainer; + + static CanFilterConfig mergeFilters(CanFilterConfig &a_, CanFilterConfig &b_); + static uint8_t countBits(uint32_t n_); + uint16_t getNumFilters() const; + + /** + * Fills the multiset_configs_ to proceed it with computeConfiguration() + */ + int16_t loadInputConfiguration(); + + /** + * This method merges several listeners's filter configurations by predetermined algorithm + * if number of available hardware acceptance filters less than number of listeners + */ + int16_t computeConfiguration(); + + /** + * This method loads the configuration computed with computeConfiguration() to the CAN driver. + */ + int16_t applyConfiguration(); + + INode& node_; //< Node reference is needed for access to ICanDriver and Dispatcher + MultisetConfigContainer multiset_configs_; + +public: + explicit CanAcceptanceFilterConfigurator(INode& node) + : node_(node) + , multiset_configs_(node.getAllocator()) + { } + + /** + * This method invokes loadInputConfiguration(), computeConfiguration() and applyConfiguration() consequently, so that + * optimal acceptance filter configuration will be computed and loaded through CanDriver::configureFilters() + * @return 0 = success, negative for error. + */ + int configureFilters(); + + /** + * Returns the configuration computed with computeConfiguration(). + * If computeConfiguration() has not been called yet, an empty configuration will be returned. + */ + const MultisetConfigContainer& getConfiguration() const + { + return multiset_configs_; + } +}; + +} + +#endif // UAVCAN_BUILD_CONFIG_HPP_INCLUDED diff --git a/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp b/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp index 2dbc508118..1a85d0b681 100644 --- a/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp +++ b/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp @@ -16,7 +16,6 @@ namespace uavcan { -UAVCAN_PACKED_BEGIN class UAVCAN_EXPORT OutgoingTransferRegistryKey { DataTypeID data_type_id_; @@ -56,7 +55,6 @@ public: std::string toString() const; #endif }; -UAVCAN_PACKED_END /** * Outgoing transfer registry keeps track of Transfer ID values for all currently existing local transfer senders. @@ -76,13 +74,11 @@ public: template class UAVCAN_EXPORT OutgoingTransferRegistry : public IOutgoingTransferRegistry, Noncopyable { - UAVCAN_PACKED_BEGIN struct Value { MonotonicTime deadline; TransferID tid; }; - UAVCAN_PACKED_END class DeadlineExpiredPredicate { diff --git a/libuavcan/include/uavcan/transport/transfer_buffer.hpp b/libuavcan/include/uavcan/transport/transfer_buffer.hpp index 0911f106e4..977bd3e368 100644 --- a/libuavcan/include/uavcan/transport/transfer_buffer.hpp +++ b/libuavcan/include/uavcan/transport/transfer_buffer.hpp @@ -16,7 +16,6 @@ namespace uavcan { -UAVCAN_PACKED_BEGIN /** * Internal for TransferBufferManager */ @@ -136,7 +135,6 @@ public: virtual int read(unsigned offset, uint8_t* data, unsigned len) const; virtual int write(unsigned offset, const uint8_t* data, unsigned len); }; -UAVCAN_PACKED_END /** * Standalone static buffer diff --git a/libuavcan/include/uavcan/transport/transfer_receiver.hpp b/libuavcan/include/uavcan/transport/transfer_receiver.hpp index 737c9bc2db..f36083260a 100644 --- a/libuavcan/include/uavcan/transport/transfer_receiver.hpp +++ b/libuavcan/include/uavcan/transport/transfer_receiver.hpp @@ -13,37 +13,46 @@ namespace uavcan { -UAVCAN_PACKED_BEGIN class UAVCAN_EXPORT TransferReceiver { public: enum ResultCode { ResultNotComplete, ResultComplete, ResultSingleFrame }; - static const uint32_t MinTransferIntervalUSec = 1 * 1000UL; - static const uint32_t MaxTransferIntervalUSec = 10 * 1000 * 1000UL; - static const uint32_t DefaultTransferIntervalUSec = 1 * 1000 * 1000UL; + static const uint16_t MinTransferIntervalMSec = 1; + static const uint16_t MaxTransferIntervalMSec = 0xFFFF; + static const uint16_t DefaultTransferIntervalMSec = 1000; static MonotonicDuration getDefaultTransferInterval() { - return MonotonicDuration::fromUSec(DefaultTransferIntervalUSec); + return MonotonicDuration::fromMSec(DefaultTransferIntervalMSec); } - static MonotonicDuration getMinTransferInterval() { return MonotonicDuration::fromUSec(MinTransferIntervalUSec); } - static MonotonicDuration getMaxTransferInterval() { return MonotonicDuration::fromUSec(MaxTransferIntervalUSec); } + static MonotonicDuration getMinTransferInterval() { return MonotonicDuration::fromMSec(MinTransferIntervalMSec); } + static MonotonicDuration getMaxTransferInterval() { return MonotonicDuration::fromMSec(MaxTransferIntervalMSec); } private: enum TidRelation { TidSame, TidRepeat, TidFuture }; - static const uint8_t IfaceIndexNotSet = 0xFF; + + enum { IfaceIndexNotSet = MaxCanIfaces }; + + enum { BufferWritePosMask = 4095 }; + enum { ErrorCntMask = 15 }; + enum { IfaceIndexMask = MaxCanIfaces }; MonotonicTime prev_transfer_ts_; MonotonicTime this_transfer_ts_; UtcTime first_frame_ts_; - uint32_t transfer_interval_usec_; + uint16_t transfer_interval_msec_; uint16_t this_transfer_crc_; - uint16_t buffer_write_pos_; - TransferID tid_; - uint8_t iface_index_; - uint8_t next_frame_index_; - mutable uint8_t error_cnt_; + + // 2 byte aligned bitfields: + uint16_t buffer_write_pos_ : 12; + mutable uint16_t error_cnt_ : 4; + + TransferID tid_; // 1 byte field + + // 1 byte aligned bitfields: + uint8_t next_frame_index_ : 6; + uint8_t iface_index_ : 2; bool isInitialized() const { return iface_index_ != IfaceIndexNotSet; } @@ -59,14 +68,18 @@ private: ResultCode receive(const RxFrame& frame, TransferBufferAccessor& tba); public: - TransferReceiver() - : transfer_interval_usec_(DefaultTransferIntervalUSec) - , this_transfer_crc_(0) - , buffer_write_pos_(0) - , iface_index_(IfaceIndexNotSet) - , next_frame_index_(0) - , error_cnt_(0) - { } + TransferReceiver() : + transfer_interval_msec_(DefaultTransferIntervalMSec), + this_transfer_crc_(0), + buffer_write_pos_(0), + error_cnt_(0), + next_frame_index_(0), + iface_index_(IfaceIndexNotSet) + { +#if UAVCAN_DEBUG + StaticAssert::check(); +#endif + } bool isTimedOut(MonotonicTime current_ts) const; @@ -79,9 +92,8 @@ public: uint16_t getLastTransferCrc() const { return this_transfer_crc_; } - MonotonicDuration getInterval() const { return MonotonicDuration::fromUSec(transfer_interval_usec_); } + MonotonicDuration getInterval() const { return MonotonicDuration::fromMSec(transfer_interval_msec_); } }; -UAVCAN_PACKED_END } diff --git a/libuavcan/include/uavcan/util/map.hpp b/libuavcan/include/uavcan/util/map.hpp index ece759af0e..9c43394ba3 100644 --- a/libuavcan/include/uavcan/util/map.hpp +++ b/libuavcan/include/uavcan/util/map.hpp @@ -36,22 +36,22 @@ class UAVCAN_EXPORT MapBase : Noncopyable { template friend class Map; - UAVCAN_PACKED_BEGIN public: struct KVPair { + Value value; // Key and value are swapped because this may allow to reduce padding (depending on types) Key key; - Value value; - KVPair() - : key() - , value() + KVPair() : + value(), + key() { } - KVPair(const Key& arg_key, const Value& arg_value) - : key(arg_key) - , value(arg_value) + KVPair(const Key& arg_key, const Value& arg_value) : + value(arg_value), + key(arg_key) { } + bool match(const Key& rhs) const { return rhs == key; } }; @@ -99,7 +99,6 @@ private: return NULL; } }; - UAVCAN_PACKED_END LinkedListRoot list_; IPoolAllocator& allocator_; diff --git a/libuavcan/src/node/uc_scheduler.cpp b/libuavcan/src/node/uc_scheduler.cpp index 5901fe75c5..529d94ccc6 100644 --- a/libuavcan/src/node/uc_scheduler.cpp +++ b/libuavcan/src/node/uc_scheduler.cpp @@ -132,7 +132,7 @@ MonotonicTime Scheduler::computeDispatcherSpinDeadline(MonotonicTime spin_deadli const MonotonicTime ts = getMonotonicTime(); if (earliest > ts) { - if (ts - earliest > deadline_resolution_) + if (earliest - ts > deadline_resolution_) { return ts + deadline_resolution_; } diff --git a/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp b/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp new file mode 100644 index 0000000000..0637f4dbbd --- /dev/null +++ b/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp @@ -0,0 +1,228 @@ +/* + * Copyright (C) 2014 Pavel Kirienko , + * Ilia Sheremet + */ + +#include +#include + +namespace uavcan +{ +const unsigned CanAcceptanceFilterConfigurator::DefaultFilterMsgMask; +const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceRequestID; +const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceRequestMask; +const unsigned CanAcceptanceFilterConfigurator::ServiceRespFrameID; +const unsigned CanAcceptanceFilterConfigurator::ServiceRespFrameMask; + +int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration() +{ + multiset_configs_.clear(); + + CanFilterConfig service_resp_cfg; + service_resp_cfg.id = ServiceRespFrameID; + service_resp_cfg.mask = ServiceRespFrameMask; + if (multiset_configs_.emplace(service_resp_cfg) == NULL) + { + return -ErrMemory; + } + + const TransferListenerBase* p = node_.getDispatcher().getListOfMessageListeners().get(); + while (p) + { + CanFilterConfig cfg; + cfg.id = static_cast(p->getDataTypeDescriptor().getID().get()) << 16; + cfg.id |= static_cast(p->getDataTypeDescriptor().getKind()) << 8; + cfg.mask = DefaultFilterMsgMask; + if (multiset_configs_.emplace(cfg) == NULL) + { + return -ErrMemory; + } + p = p->getNextListNode(); + } + + const TransferListenerBase* p1 = node_.getDispatcher().getListOfServiceRequestListeners().get(); + while (p1) + { + CanFilterConfig cfg; + cfg.id = DefaultFilterServiceRequestID; + cfg.id |= static_cast(p1->getDataTypeDescriptor().getID().get()) << 17; + cfg.mask = DefaultFilterServiceRequestMask; + if (multiset_configs_.emplace(cfg) == NULL) + { + return -ErrMemory; + } + p1 = p1->getNextListNode(); + } + + if (multiset_configs_.getSize() == 0) + { + return -ErrLogic; + } + +#if UAVCAN_DEBUG + for (uint16_t i = 0; i < multiset_configs_.getSize(); i++) + { + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::loadInputConfiguration()", "cfg.ID [%u] = %d", i, + multiset_configs_.getByIndex(i)->id); + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::loadInputConfiguration()", "cfg.MK [%u] = %d", i, + multiset_configs_.getByIndex(i)->mask); + } +#endif + + return 0; +} + +int16_t CanAcceptanceFilterConfigurator::computeConfiguration() +{ + const uint16_t acceptance_filters_number = getNumFilters(); + UAVCAN_ASSERT(multiset_configs_.getSize() != 0); + + while (acceptance_filters_number < multiset_configs_.getSize()) + { + uint16_t i_rank = 0, j_rank = 0; + uint8_t best_rank = 0; + + const uint16_t multiset_array_size = static_cast(multiset_configs_.getSize()); + + for (uint16_t i_ind = 0; i_ind < multiset_array_size - 1; i_ind++) + { + for (uint16_t j_ind = static_cast(i_ind + 1); j_ind < multiset_array_size; j_ind++) + { + CanFilterConfig temp_config = mergeFilters(*multiset_configs_.getByIndex(i_ind), + *multiset_configs_.getByIndex(j_ind)); + uint8_t rank = countBits(temp_config.mask); + if (rank > best_rank) + { + best_rank = rank; + i_rank = i_ind; + j_rank = j_ind; + } + } + } + + *multiset_configs_.getByIndex(j_rank) = mergeFilters(*multiset_configs_.getByIndex(i_rank), + *multiset_configs_.getByIndex(j_rank)); + multiset_configs_.removeFirst(*multiset_configs_.getByIndex(i_rank)); + } + + UAVCAN_ASSERT(acceptance_filters_number >= multiset_configs_.getSize()); + + return 0; +} + +int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void) +{ + CanFilterConfig filter_conf_array[MaxCanAcceptanceFilters]; + const unsigned int filter_array_size = multiset_configs_.getSize(); + + if (filter_array_size > MaxCanAcceptanceFilters) + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + + for (uint16_t i = 0; i < filter_array_size; i++) + { + CanFilterConfig temp_filter_config = *multiset_configs_.getByIndex(i); + + filter_conf_array[i] = temp_filter_config; + } + + ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver(); + for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) + { + ICanIface* iface = can_driver.getIface(i); + if (iface == NULL) + { + return -ErrDriver; + } + int16_t num = iface->configureFilters(reinterpret_cast(&filter_conf_array), + static_cast(filter_array_size)); + if (num < 0) + { + return -ErrDriver; + } + } + + return 0; +} + +int CanAcceptanceFilterConfigurator::configureFilters() +{ + if (getNumFilters() == 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available"); + return -ErrDriver; + } + + int16_t fill_array_error = loadInputConfiguration(); + if (fill_array_error != 0) + { + UAVCAN_TRACE("CanAcceptanceFilter::loadInputConfiguration", "Failed to execute loadInputConfiguration()"); + return fill_array_error; + } + + int16_t compute_configuration_error = computeConfiguration(); + if (compute_configuration_error != 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to compute optimal acceptance fliter's configuration"); + return compute_configuration_error; + } + + if (applyConfiguration() != 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); + return -ErrDriver; + } + + return 0; +} + +uint16_t CanAcceptanceFilterConfigurator::getNumFilters() const +{ + + static const uint16_t InvalidOut = 0xFFFF; + uint16_t out = InvalidOut; + ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver(); + + for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) + { + const ICanIface* iface = can_driver.getIface(i); + if (iface == NULL) + { + UAVCAN_ASSERT(0); + out = 0; + break; + } + const uint16_t num = iface->getNumFilters(); + out = min(out, num); + if (out > MaxCanAcceptanceFilters) + { + out = MaxCanAcceptanceFilters; + } + } + + return (out == InvalidOut) ? 0 : out; +} + +CanFilterConfig CanAcceptanceFilterConfigurator::mergeFilters(CanFilterConfig &a_, CanFilterConfig &b_) +{ + CanFilterConfig temp_arr; + temp_arr.mask = a_.mask & b_.mask & ~(a_.id ^ b_.id); + temp_arr.id = a_.id & temp_arr.mask; + + return temp_arr; +} + +uint8_t CanAcceptanceFilterConfigurator::countBits(uint32_t n_) +{ + uint8_t c_; // c accumulates the total bits set in v + for (c_ = 0; n_; c_++) + { + n_ &= n_ - 1; // clear the least significant bit set + } + + return c_; +} + +} diff --git a/libuavcan/src/transport/uc_transfer_receiver.cpp b/libuavcan/src/transport/uc_transfer_receiver.cpp index 7d85a0b749..9b20ace8e5 100644 --- a/libuavcan/src/transport/uc_transfer_receiver.cpp +++ b/libuavcan/src/transport/uc_transfer_receiver.cpp @@ -11,21 +11,13 @@ namespace uavcan { -const uint32_t TransferReceiver::MinTransferIntervalUSec; -const uint32_t TransferReceiver::MaxTransferIntervalUSec; -const uint32_t TransferReceiver::DefaultTransferIntervalUSec; -const uint8_t TransferReceiver::IfaceIndexNotSet; +const uint16_t TransferReceiver::MinTransferIntervalMSec; +const uint16_t TransferReceiver::MaxTransferIntervalMSec; +const uint16_t TransferReceiver::DefaultTransferIntervalMSec; void TransferReceiver::registerError() const { - if (error_cnt_ < 0xFF) - { - error_cnt_ = static_cast(error_cnt_ + 1); - } - else - { - UAVCAN_ASSERT(0); - } + error_cnt_ = static_cast(error_cnt_ + 1) & ErrorCntMask; } TransferReceiver::TidRelation TransferReceiver::getTidRelation(const RxFrame& frame) const @@ -51,10 +43,10 @@ void TransferReceiver::updateTransferTimings() if ((!prev_prev_ts.isZero()) && (!prev_transfer_ts_.isZero()) && (prev_transfer_ts_ >= prev_prev_ts)) { - uint64_t interval_usec = uint64_t((prev_transfer_ts_ - prev_prev_ts).toUSec()); - interval_usec = min(interval_usec, uint64_t(MaxTransferIntervalUSec)); - interval_usec = max(interval_usec, uint64_t(MinTransferIntervalUSec)); - transfer_interval_usec_ = static_cast((uint64_t(transfer_interval_usec_) * 7 + interval_usec) / 8); + uint64_t interval_msec = uint64_t((prev_transfer_ts_ - prev_prev_ts).toMSec()); + interval_msec = min(interval_msec, uint64_t(MaxTransferIntervalMSec)); + interval_msec = max(interval_msec, uint64_t(MinTransferIntervalMSec)); + transfer_interval_msec_ = static_cast((uint64_t(transfer_interval_msec_) * 7U + interval_msec) / 8U); } } @@ -118,7 +110,7 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf) const bool success = res == static_cast(effective_payload_len); if (success) { - buffer_write_pos_ = static_cast(buffer_write_pos_ + effective_payload_len); + buffer_write_pos_ = static_cast(buffer_write_pos_ + effective_payload_len) & BufferWritePosMask; } return success; } @@ -128,7 +120,7 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf) const bool success = res == static_cast(payload_len); if (success) { - buffer_write_pos_ = static_cast(buffer_write_pos_ + payload_len); + buffer_write_pos_ = static_cast(buffer_write_pos_ + payload_len) & BufferWritePosMask; } return success; } @@ -186,12 +178,12 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, Tra bool TransferReceiver::isTimedOut(MonotonicTime current_ts) const { - static const int64_t INTERVAL_MULT = (1 << TransferID::BitLen) / 2 + 1; + static const int64_t IntervalMult = (1 << TransferID::BitLen) / 2 + 1; if (current_ts <= this_transfer_ts_) { return false; } - return (current_ts - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_usec_) * INTERVAL_MULT); + return (current_ts - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_msec_) * 1000 * IntervalMult); } TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba) @@ -209,7 +201,7 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr const bool first_fame = frame.isFirst(); const TidRelation tid_rel = getTidRelation(frame); const bool iface_timed_out = - (frame.getMonotonicTimestamp() - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_usec_) * 2); + (frame.getMonotonicTimestamp() - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_msec_) * 1000 * 2); // FSM, the hard way const bool need_restart = @@ -230,7 +222,7 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr int(not_initialized), int(iface_timed_out), int(receiver_timed_out), int(same_iface), int(first_fame), int(tid_rel), frame.toString().c_str()); tba.remove(); - iface_index_ = frame.getIfaceIndex(); + iface_index_ = frame.getIfaceIndex() & IfaceIndexMask; tid_ = frame.getTransferID(); next_frame_index_ = 0; buffer_write_pos_ = 0; diff --git a/libuavcan/src/uc_dynamic_memory.cpp b/libuavcan/src/uc_dynamic_memory.cpp index d85eca5317..68925f1d11 100644 --- a/libuavcan/src/uc_dynamic_memory.cpp +++ b/libuavcan/src/uc_dynamic_memory.cpp @@ -33,17 +33,7 @@ void LimitedPoolAllocator::deallocate(const void* ptr) } } -bool LimitedPoolAllocator::isInPool(const void* ptr) const -{ - return allocator_.isInPool(ptr); -} - -std::size_t LimitedPoolAllocator::getBlockSize() const -{ - return allocator_.getBlockSize(); -} - -std::size_t LimitedPoolAllocator::getNumBlocks() const +uint16_t LimitedPoolAllocator::getNumBlocks() const { return min(max_blocks_, allocator_.getNumBlocks()); } diff --git a/libuavcan/test/dynamic_memory.cpp b/libuavcan/test/dynamic_memory.cpp index b179211518..21ba7e4487 100644 --- a/libuavcan/test/dynamic_memory.cpp +++ b/libuavcan/test/dynamic_memory.cpp @@ -8,91 +8,40 @@ TEST(DynamicMemory, Basic) { uavcan::PoolAllocator<128, 32> pool32; - uavcan::PoolAllocator<256, 64> pool64; - uavcan::PoolAllocator<512, 128> pool128; - EXPECT_EQ(4, pool32.getNumFreeBlocks()); - EXPECT_EQ(4, pool64.getNumFreeBlocks()); - EXPECT_EQ(4, pool128.getNumFreeBlocks()); - - uavcan::PoolManager<2> poolmgr; - EXPECT_TRUE(poolmgr.addPool(&pool64)); // Order of insertion shall not matter - EXPECT_TRUE(poolmgr.addPool(&pool32)); - EXPECT_FALSE(poolmgr.addPool(&pool128)); - - EXPECT_EQ(4 * 2, poolmgr.getNumBlocks()); - - const void* ptr1 = poolmgr.allocate(16); - EXPECT_TRUE(ptr1); - - const void* ptr2 = poolmgr.allocate(32); - EXPECT_TRUE(ptr2); - - const void* ptr3 = poolmgr.allocate(64); - EXPECT_TRUE(ptr3); - - EXPECT_FALSE(poolmgr.allocate(120)); - - EXPECT_EQ(2, pool32.getNumUsedBlocks()); - EXPECT_EQ(1, pool64.getNumUsedBlocks()); - EXPECT_EQ(0, pool128.getNumUsedBlocks()); - - poolmgr.deallocate(ptr1); + EXPECT_EQ(0, pool32.getPeakNumUsedBlocks()); + const void* ptr1 = pool32.allocate(16); + ASSERT_TRUE(ptr1); EXPECT_EQ(1, pool32.getNumUsedBlocks()); - poolmgr.deallocate(ptr2); + EXPECT_FALSE(pool32.allocate(120)); + EXPECT_EQ(1, pool32.getNumUsedBlocks()); + pool32.deallocate(ptr1); EXPECT_EQ(0, pool32.getNumUsedBlocks()); - EXPECT_EQ(1, pool64.getNumUsedBlocks()); - poolmgr.deallocate(ptr3); - EXPECT_EQ(0, pool64.getNumUsedBlocks()); - EXPECT_EQ(0, pool128.getNumUsedBlocks()); + EXPECT_EQ(1, pool32.getPeakNumUsedBlocks()); } TEST(DynamicMemory, OutOfMemory) { uavcan::PoolAllocator<64, 32> pool32; - uavcan::PoolAllocator<128, 64> pool64; EXPECT_EQ(2, pool32.getNumFreeBlocks()); - EXPECT_EQ(2, pool64.getNumFreeBlocks()); + EXPECT_EQ(0, pool32.getNumUsedBlocks()); + EXPECT_EQ(0, pool32.getPeakNumUsedBlocks()); - uavcan::PoolManager<4> poolmgr; - EXPECT_TRUE(poolmgr.addPool(&pool64)); - EXPECT_TRUE(poolmgr.addPool(&pool32)); - - const void* ptr1 = poolmgr.allocate(32); - EXPECT_TRUE(ptr1); - EXPECT_TRUE(pool32.isInPool(ptr1)); - EXPECT_FALSE(pool64.isInPool(ptr1)); - - const void* ptr2 = poolmgr.allocate(32); - EXPECT_TRUE(ptr2); - EXPECT_TRUE(pool32.isInPool(ptr2)); - EXPECT_FALSE(pool64.isInPool(ptr2)); - - const void* ptr3 = poolmgr.allocate(32); - EXPECT_TRUE(ptr3); - EXPECT_FALSE(pool32.isInPool(ptr3)); - EXPECT_TRUE(pool64.isInPool(ptr3)); // One block went to the next pool + const void* ptr1 = pool32.allocate(32); + ASSERT_TRUE(ptr1); + EXPECT_EQ(1, pool32.getNumUsedBlocks()); + EXPECT_EQ(1, pool32.getPeakNumUsedBlocks()); + const void* ptr2 = pool32.allocate(32); + ASSERT_TRUE(ptr2); EXPECT_EQ(2, pool32.getNumUsedBlocks()); - EXPECT_EQ(1, pool64.getNumUsedBlocks()); + EXPECT_EQ(2, pool32.getPeakNumUsedBlocks()); - poolmgr.deallocate(ptr2); - EXPECT_EQ(1, pool32.getNumUsedBlocks()); - EXPECT_EQ(1, pool64.getNumUsedBlocks()); - - const void* ptr4 = poolmgr.allocate(64); - EXPECT_TRUE(ptr4); - EXPECT_EQ(1, pool32.getNumUsedBlocks()); - EXPECT_EQ(2, pool64.getNumUsedBlocks()); // Top pool is 100% used - - EXPECT_FALSE(poolmgr.allocate(64)); // No free blocks left --> NULL - EXPECT_EQ(1, pool32.getNumUsedBlocks()); - EXPECT_EQ(2, pool64.getNumUsedBlocks()); - - poolmgr.deallocate(ptr3); // This was small chunk allocated in big pool - EXPECT_EQ(1, pool32.getNumUsedBlocks()); - EXPECT_EQ(1, pool64.getNumUsedBlocks()); // Make sure it was properly deallocated + ASSERT_FALSE(pool32.allocate(32)); // No free blocks left --> NULL + EXPECT_EQ(2, pool32.getNumUsedBlocks()); + EXPECT_EQ(0, pool32.getNumFreeBlocks()); + EXPECT_EQ(2, pool32.getPeakNumUsedBlocks()); } TEST(DynamicMemory, LimitedPoolAllocator) @@ -101,6 +50,7 @@ TEST(DynamicMemory, LimitedPoolAllocator) uavcan::LimitedPoolAllocator lim(pool32, 2); EXPECT_EQ(2, lim.getNumBlocks()); + EXPECT_EQ(0, pool32.getPeakNumUsedBlocks()); const void* ptr1 = lim.allocate(1); const void* ptr2 = lim.allocate(1); @@ -119,4 +69,6 @@ TEST(DynamicMemory, LimitedPoolAllocator) EXPECT_TRUE(ptr4); EXPECT_TRUE(ptr5); EXPECT_FALSE(ptr6); + + EXPECT_EQ(2, pool32.getPeakNumUsedBlocks()); } diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index 939251f154..fd01b82d96 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -20,17 +20,15 @@ struct TestNode : public uavcan::INode { uavcan::PoolAllocator pool; - uavcan::PoolManager<1> poolmgr; uavcan::OutgoingTransferRegistry<8> otr; uavcan::Scheduler scheduler; uint64_t internal_failure_count; TestNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock_driver, uavcan::NodeID self_node_id) - : otr(poolmgr) - , scheduler(can_driver, poolmgr, clock_driver, otr) + : otr(pool) + , scheduler(can_driver, pool, clock_driver, otr) , internal_failure_count(0) { - poolmgr.addPool(&pool); setNodeID(self_node_id); } @@ -40,7 +38,7 @@ struct TestNode : public uavcan::INode internal_failure_count++; } - virtual uavcan::PoolManager<1>& getAllocator() { return poolmgr; } + virtual uavcan::IPoolAllocator& getAllocator() { return pool; } virtual uavcan::Scheduler& getScheduler() { return scheduler; } virtual const uavcan::Scheduler& getScheduler() const { return scheduler; } }; @@ -75,15 +73,6 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface return NULL; } - virtual const uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) const - { - if (iface_index == 0) - { - return this; - } - return NULL; - } - virtual uavcan::uint8_t getNumIfaces() const { return 1; } virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline) diff --git a/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp index 51ada6d104..c41115653d 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp @@ -11,6 +11,7 @@ TEST(dynamic_node_id_server_centralized_Storage, Initialization) { using namespace uavcan::dynamic_node_id_server::centralized; + using namespace uavcan::dynamic_node_id_server; // No data in the storage - initializing empty { @@ -20,95 +21,51 @@ TEST(dynamic_node_id_server_centralized_Storage, Initialization) ASSERT_EQ(0, storage.getNumKeys()); ASSERT_LE(0, stor.init()); - ASSERT_EQ(1, storage.getNumKeys()); + ASSERT_EQ(0, storage.getNumKeys()); ASSERT_EQ(0, stor.getSize()); - ASSERT_FALSE(stor.findByNodeID(1)); - ASSERT_FALSE(stor.findByNodeID(0)); - } - // Nonempty storage, one item - { - MemoryStorageBackend storage; - Storage stor(storage); - - storage.set("size", "1"); - ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Expected one entry, none found - ASSERT_EQ(1, storage.getNumKeys()); - - storage.set("0_unique_id", "00000000000000000000000000000000"); - storage.set("0_node_id", "0"); - ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Invalid entry - zero Node ID - - storage.set("0_node_id", "1"); - ASSERT_LE(0, stor.init()); // OK now - - ASSERT_EQ(3, storage.getNumKeys()); - ASSERT_EQ(1, stor.getSize()); - - ASSERT_TRUE(stor.findByNodeID(1)); - ASSERT_FALSE(stor.findByNodeID(0)); - } - // Nonempty storage, broken data - { - MemoryStorageBackend storage; - Storage stor(storage); - - storage.set("size", "foobar"); - ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Bad value - - storage.set("size", "128"); - ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Bad value - - storage.set("size", "1"); - ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // No items - ASSERT_EQ(1, storage.getNumKeys()); - - storage.set("0_unique_id", "00000000000000000000000000000000"); - storage.set("0_node_id", "128"); // Bad value (127 max) - ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Failed - - storage.set("0_node_id", "127"); - ASSERT_LE(0, stor.init()); // OK now - ASSERT_EQ(1, stor.getSize()); - - ASSERT_TRUE(stor.findByNodeID(127)); - ASSERT_FALSE(stor.findByNodeID(0)); - - ASSERT_EQ(3, storage.getNumKeys()); + ASSERT_FALSE(stor.isNodeIDOccupied(1)); + ASSERT_FALSE(stor.isNodeIDOccupied(0)); } // Nonempty storage, many items { MemoryStorageBackend storage; Storage stor(storage); - storage.set("size", "2"); - storage.set("0_unique_id", "00000000000000000000000000000000"); - storage.set("0_node_id", "1"); - storage.set("1_unique_id", "0123456789abcdef0123456789abcdef"); - storage.set("1_node_id", "127"); + storage.set("occupation_mask", "0e000000000000000000000000000000"); // node ID 1, 2, 3 + ASSERT_LE(0, stor.init()); // OK - ASSERT_LE(0, stor.init()); // OK now - ASSERT_EQ(5, storage.getNumKeys()); - ASSERT_EQ(2, stor.getSize()); + ASSERT_EQ(1, storage.getNumKeys()); + ASSERT_EQ(3, stor.getSize()); - ASSERT_TRUE(stor.findByNodeID(1)); - ASSERT_TRUE(stor.findByNodeID(127)); - ASSERT_FALSE(stor.findByNodeID(0)); + ASSERT_TRUE(stor.isNodeIDOccupied(1)); + ASSERT_TRUE(stor.isNodeIDOccupied(2)); + ASSERT_TRUE(stor.isNodeIDOccupied(3)); + ASSERT_FALSE(stor.isNodeIDOccupied(0)); + ASSERT_FALSE(stor.isNodeIDOccupied(4)); - uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id uid; - uid[0] = 0x01; - uid[1] = 0x23; - uid[2] = 0x45; - uid[3] = 0x67; - uid[4] = 0x89; - uid[5] = 0xab; - uid[6] = 0xcd; - uid[7] = 0xef; - uavcan::copy(uid.begin(), uid.begin() + 8, uid.begin() + 8); + UniqueID uid_1; + uid_1[0] = 1; - ASSERT_TRUE(stor.findByUniqueID(uid)); - ASSERT_EQ(127, stor.findByUniqueID(uid)->node_id.get()); - ASSERT_EQ(uid, stor.findByNodeID(127)->unique_id); + UniqueID uid_2; + uid_2[0] = 2; + + UniqueID uid_3; + uid_3[0] = 3; + + ASSERT_FALSE(stor.getNodeIDForUniqueID(uid_1).isValid()); + ASSERT_FALSE(stor.getNodeIDForUniqueID(uid_2).isValid()); + ASSERT_FALSE(stor.getNodeIDForUniqueID(uid_3).isValid()); + ASSERT_FALSE(stor.isNodeIDOccupied(127)); + + storage.set("01000000000000000000000000000000", "1"); + storage.set("02000000000000000000000000000000", "2"); + storage.set("03000000000000000000000000000000", "3"); + + ASSERT_EQ(1, stor.getNodeIDForUniqueID(uid_1).get()); + ASSERT_EQ(2, stor.getNodeIDForUniqueID(uid_2).get()); + ASSERT_EQ(3, stor.getNodeIDForUniqueID(uid_3).get()); + ASSERT_FALSE(stor.isNodeIDOccupied(127)); } } @@ -123,21 +80,19 @@ TEST(dynamic_node_id_server_centralized_Storage, Basic) ASSERT_EQ(0, storage.getNumKeys()); ASSERT_LE(0, stor.init()); storage.print(); - ASSERT_EQ(1, storage.getNumKeys()); + ASSERT_EQ(0, storage.getNumKeys()); /* * Adding one entry to the log, making sure it appears in the storage */ - Storage::Entry entry; - entry.node_id = 1; - entry.unique_id[0] = 1; - ASSERT_LE(0, stor.add(entry.node_id, entry.unique_id)); + uavcan::dynamic_node_id_server::UniqueID unique_id; + unique_id[0] = 1; + ASSERT_LE(0, stor.add(1, unique_id)); - ASSERT_EQ("1", storage.get("size")); - ASSERT_EQ("01000000000000000000000000000000", storage.get("0_unique_id")); - ASSERT_EQ("1", storage.get("0_node_id")); + ASSERT_EQ("02000000000000000000000000000000", storage.get("occupation_mask")); + ASSERT_EQ("1", storage.get("01000000000000000000000000000000")); - ASSERT_EQ(3, storage.getNumKeys()); + ASSERT_EQ(2, storage.getNumKeys()); ASSERT_EQ(1, stor.getSize()); /* @@ -145,30 +100,31 @@ TEST(dynamic_node_id_server_centralized_Storage, Basic) */ storage.failOnSetCalls(true); - ASSERT_EQ(3, storage.getNumKeys()); + ASSERT_EQ(2, storage.getNumKeys()); - entry.node_id = 2; - entry.unique_id[0] = 2; - ASSERT_GT(0, stor.add(entry.node_id, entry.unique_id)); + unique_id[0] = 2; + ASSERT_GT(0, stor.add(2, unique_id)); - ASSERT_EQ(3, storage.getNumKeys()); // No new entries, we failed + ASSERT_EQ(2, storage.getNumKeys()); // No new entries, we failed ASSERT_EQ(1, stor.getSize()); /* - * Making sure add() fails when the log is full + * Adding a lot of entries */ storage.failOnSetCalls(false); - while (stor.getSize() < stor.Capacity) + uavcan::NodeID node_id(2); + while (stor.getSize() < 127) { - ASSERT_LE(0, stor.add(entry.node_id, entry.unique_id)); + ASSERT_LE(0, stor.add(node_id, unique_id)); - entry.node_id = uint8_t(uavcan::min(entry.node_id.get() + 1U, 127U)); - entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); + ASSERT_TRUE(stor.getNodeIDForUniqueID(unique_id).isValid()); + ASSERT_TRUE(stor.isNodeIDOccupied(node_id)); + + node_id = uint8_t(uavcan::min(node_id.get() + 1U, 127U)); + unique_id[0] = uint8_t(unique_id[0] + 1U); } - ASSERT_GT(0, stor.add(123, entry.unique_id)); // Failing because full - storage.print(); } diff --git a/libuavcan/test/protocol/node_info_retriever.cpp b/libuavcan/test/protocol/node_info_retriever.cpp index 88fc4aace8..f5bbb25a7e 100644 --- a/libuavcan/test/protocol/node_info_retriever.cpp +++ b/libuavcan/test/protocol/node_info_retriever.cpp @@ -203,6 +203,26 @@ TEST(NodeInfoRetriever, Basic) EXPECT_EQ(13, listener.status_message_cnt); EXPECT_EQ(7, listener.status_change_cnt); // node 2 online/offline + 2 test nodes above online/offline + 1 EXPECT_EQ(3, listener.info_unavailable_cnt); + + /* + * Forcing the class to forget everything + */ + std::cout << "Invalidation" << std::endl; + + retr.invalidateAll(); + + ASSERT_FALSE(retr.isRetrievingInProgress()); + ASSERT_EQ(0, retr.getNumPendingRequests()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 0, 60, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 0, 60, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 60, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); + + ASSERT_TRUE(retr.isRetrievingInProgress()); + ASSERT_EQ(3, retr.getNumPendingRequests()); } diff --git a/libuavcan/test/transport/can/can.hpp b/libuavcan/test/transport/can/can.hpp index 89bf7f1dbe..2d5a781e1d 100644 --- a/libuavcan/test/transport/can/can.hpp +++ b/libuavcan/test/transport/can/can.hpp @@ -232,7 +232,6 @@ public: } virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) { return &ifaces.at(iface_index); } - virtual const uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) const { return &ifaces.at(iface_index); } virtual uavcan::uint8_t getNumIfaces() const { return uavcan::uint8_t(ifaces.size()); } }; diff --git a/libuavcan/test/transport/can/io.cpp b/libuavcan/test/transport/can/io.cpp index cef7a6f6de..e28ae640bd 100644 --- a/libuavcan/test/transport/can/io.cpp +++ b/libuavcan/test/transport/can/io.cpp @@ -23,15 +23,13 @@ TEST(CanIOManager, Reception) { // Memory uavcan::PoolAllocator pool; - uavcan::PoolManager<2> poolmgr; - poolmgr.addPool(&pool); // Platform interface SystemClockMock clockmock; CanDriverMock driver(2, clockmock); // IO Manager - uavcan::CanIOManager iomgr(driver, poolmgr, clockmock); + uavcan::CanIOManager iomgr(driver, pool, clockmock); ASSERT_EQ(2, iomgr.getNumIfaces()); /* @@ -120,15 +118,13 @@ TEST(CanIOManager, Transmission) // Memory uavcan::PoolAllocator pool; - uavcan::PoolManager<2> poolmgr; - poolmgr.addPool(&pool); // Platform interface SystemClockMock clockmock; CanDriverMock driver(2, clockmock); // IO Manager - CanIOManager iomgr(driver, poolmgr, clockmock, 9999); + CanIOManager iomgr(driver, pool, clockmock, 9999); ASSERT_EQ(2, iomgr.getNumIfaces()); const int ALL_IFACES_MASK = 3; @@ -312,15 +308,13 @@ TEST(CanIOManager, Loopback) // Memory uavcan::PoolAllocator pool; - uavcan::PoolManager<2> poolmgr; - poolmgr.addPool(&pool); // Platform interface SystemClockMock clockmock; CanDriverMock driver(2, clockmock); // IO Manager - CanIOManager iomgr(driver, poolmgr, clockmock); + CanIOManager iomgr(driver, pool, clockmock); ASSERT_EQ(2, iomgr.getNumIfaces()); CanFrame fr1; diff --git a/libuavcan/test/transport/can/tx_queue.cpp b/libuavcan/test/transport/can/tx_queue.cpp index 425329b0d8..da5aa58bb2 100644 --- a/libuavcan/test/transport/can/tx_queue.cpp +++ b/libuavcan/test/transport/can/tx_queue.cpp @@ -68,12 +68,10 @@ TEST(CanTxQueue, TxQueue) ASSERT_GE(40, sizeof(CanTxQueue::Entry)); // should be true for any platforms, though not required uavcan::PoolAllocator<40 * 4, 40> pool; - uavcan::PoolManager<2> poolmgr; - poolmgr.addPool(&pool); SystemClockMock clockmock; - CanTxQueue queue(poolmgr, clockmock, 99999); + CanTxQueue queue(pool, clockmock, 99999); EXPECT_TRUE(queue.isEmpty()); const uavcan::CanIOFlags flags = 0; diff --git a/libuavcan/test/transport/can_acceptance_filter_configurator.cpp b/libuavcan/test/transport/can_acceptance_filter_configurator.cpp new file mode 100644 index 0000000000..7418c12f51 --- /dev/null +++ b/libuavcan/test/transport/can_acceptance_filter_configurator.cpp @@ -0,0 +1,157 @@ +/* + * Copyright (C) 2014 Pavel Kirienko , + * Ilia Sheremet + */ + +#include +#include + +#include +#include "../node/test_node.hpp" +#include "uavcan/node/subscriber.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +template +struct SubscriptionListener +{ + typedef uavcan::ReceivedDataStructure ReceivedDataStructure; + + struct ReceivedDataStructureCopy + { + uavcan::MonotonicTime ts_monotonic; + uavcan::UtcTime ts_utc; + uavcan::TransferType transfer_type; + uavcan::TransferID transfer_id; + uavcan::NodeID src_node_id; + uavcan::uint8_t iface_index; + DataType msg; + + ReceivedDataStructureCopy(const ReceivedDataStructure& s) : + ts_monotonic(s.getMonotonicTimestamp()), + ts_utc(s.getUtcTimestamp()), + transfer_type(s.getTransferType()), + transfer_id(s.getTransferID()), + src_node_id(s.getSrcNodeID()), + iface_index(s.getIfaceIndex()), + msg(s) + { } + }; + + std::vector simple; + std::vector extended; + + void receiveExtended(ReceivedDataStructure& msg) + { + extended.push_back(msg); + } + + void receiveSimple(DataType& msg) + { + simple.push_back(msg); + } + + typedef SubscriptionListener SelfType; + typedef uavcan::MethodBinder ExtendedBinder; + typedef uavcan::MethodBinder SimpleBinder; + + ExtendedBinder bindExtended() { return ExtendedBinder(this, &SelfType::receiveExtended); } + SimpleBinder bindSimple() { return SimpleBinder(this, &SelfType::receiveSimple); } +}; + +static void writeServiceServerCallback( + const uavcan::ReceivedDataStructure& req, + uavcan::protocol::file::BeginFirmwareUpdate::Response& rsp) +{ + std::cout << req << std::endl; + rsp.error = rsp.ERROR_UNKNOWN; +} + +TEST(CanAcceptanceFilter, Basic_test) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + uavcan::DefaultDataTypeRegistrator _reg6; + uavcan::DefaultDataTypeRegistrator _reg7; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(1, clock_driver); + TestNode node(can_driver, clock_driver, 24); + + uavcan::Subscriber::ExtendedBinder> sub_1(node); + uavcan::Subscriber::ExtendedBinder> sub_2(node); + uavcan::Subscriber::ExtendedBinder> sub_3(node); + uavcan::Subscriber::ExtendedBinder> sub_4(node); + uavcan::Subscriber::ExtendedBinder> sub_5(node); + uavcan::Subscriber::ExtendedBinder> sub_6(node); + uavcan::Subscriber::ExtendedBinder> sub_6_1(node); + uavcan::ServiceServer server(node); + + SubscriptionListener listener_1; + SubscriptionListener listener_2; + SubscriptionListener listener_3; + SubscriptionListener listener_4; + SubscriptionListener listener_5; + SubscriptionListener listener_6; + + sub_1.start(listener_1.bindExtended()); + sub_2.start(listener_2.bindExtended()); + sub_3.start(listener_3.bindExtended()); + sub_4.start(listener_4.bindExtended()); + sub_5.start(listener_5.bindExtended()); + sub_6.start(listener_6.bindExtended()); + sub_6_1.start(listener_6.bindExtended()); + server.start(writeServiceServerCallback); + std::cout << "Subscribers are initialized ..." << std::endl; + + uavcan::CanAcceptanceFilterConfigurator test_configurator(node); + int configure_filters_assert = test_configurator.configureFilters(); + if (configure_filters_assert == 0) + { + std::cout << "Filters are configured ..." << std::endl; + } + + const auto& configure_array = test_configurator.getConfiguration(); + uint32_t configure_array_size = configure_array.getSize(); + + ASSERT_EQ(configure_filters_assert, 0); + ASSERT_EQ(configure_array_size, 4); + + for (uint16_t i = 0; i poolmgr; // We don't need dynamic memory + NullAllocator poolmgr; // We don't need dynamic memory uavcan::TransferBufferManager<256, 1> bufmgr(poolmgr); const RxFrame frame = makeFrame(); diff --git a/libuavcan/test/transport/outgoing_transfer_registry.cpp b/libuavcan/test/transport/outgoing_transfer_registry.cpp index 2456df5030..9df1678052 100644 --- a/libuavcan/test/transport/outgoing_transfer_registry.cpp +++ b/libuavcan/test/transport/outgoing_transfer_registry.cpp @@ -6,12 +6,13 @@ #include #include #include "../clock.hpp" +#include "transfer_test_helpers.hpp" TEST(OutgoingTransferRegistry, Basic) { using uavcan::OutgoingTransferRegistryKey; - uavcan::PoolManager<1> poolmgr; // Empty + NullAllocator poolmgr; // Empty uavcan::OutgoingTransferRegistry<4> otr(poolmgr); otr.cleanup(tsMono(1000)); diff --git a/libuavcan/test/transport/transfer_buffer.cpp b/libuavcan/test/transport/transfer_buffer.cpp index 355da224cd..77f3c55399 100644 --- a/libuavcan/test/transport/transfer_buffer.cpp +++ b/libuavcan/test/transport/transfer_buffer.cpp @@ -133,10 +133,8 @@ TEST(DynamicTransferBufferManagerEntry, Basic) static const int MAX_SIZE = TEST_BUFFER_SIZE; static const int POOL_BLOCKS = 8; uavcan::PoolAllocator pool; - uavcan::PoolManager<2> poolmgr; - poolmgr.addPool(&pool); - DynamicTransferBufferManagerEntry buf(poolmgr, MAX_SIZE); + DynamicTransferBufferManagerEntry buf(pool, MAX_SIZE); uint8_t local_buffer[TEST_BUFFER_SIZE * 2]; const uint8_t* const test_data_ptr = reinterpret_cast(TEST_DATA.c_str()); @@ -233,11 +231,9 @@ TEST(TransferBufferManager, Basic) static const int POOL_BLOCKS = 6; uavcan::PoolAllocator pool; - uavcan::PoolManager<1> poolmgr; - poolmgr.addPool(&pool); typedef TransferBufferManager TransferBufferManagerType; - std::auto_ptr mgr(new TransferBufferManagerType(poolmgr)); + std::auto_ptr mgr(new TransferBufferManagerType(pool)); // Empty ASSERT_FALSE(mgr->access(TransferBufferManagerKey(0, uavcan::TransferTypeMessageUnicast))); diff --git a/libuavcan/test/transport/transfer_listener.cpp b/libuavcan/test/transport/transfer_listener.cpp index ee3474394c..441a0feb93 100644 --- a/libuavcan/test/transport/transfer_listener.cpp +++ b/libuavcan/test/transport/transfer_listener.cpp @@ -36,11 +36,9 @@ TEST(TransferListener, BasicMFT) static const int NUM_POOL_BLOCKS = 12; // This number is just enough to pass the test uavcan::PoolAllocator pool; - uavcan::PoolManager<1> poolmgr; - poolmgr.addPool(&pool); uavcan::TransferPerfCounter perf; - TestListener<256, 1, 1> subscriber(perf, type, poolmgr); + TestListener<256, 1, 1> subscriber(perf, type, pool); /* * Test data @@ -97,7 +95,7 @@ TEST(TransferListener, CrcFailure) { const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); - uavcan::PoolManager<1> poolmgr; // No dynamic memory + NullAllocator poolmgr; // No dynamic memory uavcan::TransferPerfCounter perf; TestListener<256, 2, 2> subscriber(perf, type, poolmgr); // Static buffer only, 2 entries @@ -140,7 +138,7 @@ TEST(TransferListener, BasicSFT) { const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); - uavcan::PoolManager<1> poolmgr; // No dynamic memory. At all. + NullAllocator poolmgr; // No dynamic memory. At all. uavcan::TransferPerfCounter perf; TestListener<0, 0, 5> subscriber(perf, type, poolmgr); // Max buf size is 0, i.e. SFT-only @@ -176,7 +174,7 @@ TEST(TransferListener, Cleanup) { const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); - uavcan::PoolManager<1> poolmgr; // No dynamic memory + NullAllocator poolmgr; // No dynamic memory uavcan::TransferPerfCounter perf; TestListener<256, 1, 2> subscriber(perf, type, poolmgr); // Static buffer only, 1 entry @@ -231,7 +229,7 @@ TEST(TransferListener, MaximumTransferLength) { const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); - uavcan::PoolManager<1> poolmgr; + NullAllocator poolmgr; uavcan::TransferPerfCounter perf; TestListener subscriber(perf, type, poolmgr); @@ -260,7 +258,7 @@ TEST(TransferListener, AnonymousTransfers) { const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); - uavcan::PoolManager<1> poolmgr; + NullAllocator poolmgr; uavcan::TransferPerfCounter perf; TestListener<0, 0, 0> subscriber(perf, type, poolmgr); diff --git a/libuavcan/test/transport/transfer_receiver.cpp b/libuavcan/test/transport/transfer_receiver.cpp index 6be7d99ce9..68aebf2204 100644 --- a/libuavcan/test/transport/transfer_receiver.cpp +++ b/libuavcan/test/transport/transfer_receiver.cpp @@ -6,6 +6,7 @@ #include #include #include "../clock.hpp" +#include "transfer_test_helpers.hpp" /* * Beware! @@ -46,17 +47,16 @@ struct RxFrameGenerator const uavcan::TransferBufferManagerKey RxFrameGenerator::DEFAULT_KEY(42, uavcan::TransferTypeMessageBroadcast); -template +template struct Context { - uavcan::PoolManager<1> poolmgr; // We don't need dynamic memory for this test - uavcan::TransferReceiver receiver; // Must be default constructible and copyable - uavcan::TransferBufferManager bufmgr; + NullAllocator pool; // We don't need dynamic memory for this test + uavcan::TransferReceiver receiver; // Must be default constructible and copyable + uavcan::TransferBufferManager bufmgr; - Context() - : bufmgr(poolmgr) + Context() : bufmgr(pool) { - assert(poolmgr.allocate(1) == NULL); + assert(pool.allocate(1) == NULL); } ~Context() @@ -144,8 +144,8 @@ TEST(TransferReceiver, Basic) ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), "345678abcdefgh")); ASSERT_EQ(0x789A, rcv.getLastTransferCrc()); - ASSERT_GT(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); - ASSERT_LT(TransferReceiver::getMinTransferInterval(), rcv.getInterval()); + ASSERT_GE(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); + ASSERT_LE(TransferReceiver::getMinTransferInterval(), rcv.getInterval()); ASSERT_EQ(1000, rcv.getLastTransferTimestampMonotonic().toUSec()); ASSERT_FALSE(rcv.isTimedOut(tsMono(1000))); ASSERT_FALSE(rcv.isTimedOut(tsMono(5000))); @@ -174,33 +174,47 @@ TEST(TransferReceiver, Basic) CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "", 0, true, 4, 3600), bk)); ASSERT_EQ(3600, rcv.getLastTransferTimestampMonotonic().toUSec()); + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + /* * Timeouts */ - CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", 0, true, 1, 100000), bk)); // Wrong iface - ignored + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", 0, true, 1, 5000), bk)); // Wrong iface - ignored CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", 0, true, 6, 1500000), bk)); // Accepted due to iface timeout ASSERT_EQ(1500000, rcv.getLastTransferTimestampMonotonic().toUSec()); + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", 0, true, 7, 1500100), bk)); // Ignored - old iface 0 CHECK_SINGLE_FRAME(rcv.addFrame(gen(1, "qwe", 0, true, 7, 1500100), bk)); ASSERT_EQ(1500100, rcv.getLastTransferTimestampMonotonic().toUSec()); + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "qwe", 0, true, 7, 1500100), bk)); // Old TID CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 7, 100000000), bk)); // Accepted - global timeout ASSERT_EQ(100000000, rcv.getLastTransferTimestampMonotonic().toUSec()); + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + CHECK_SINGLE_FRAME(rcv.addFrame(gen(0, "qwe", 0, true, 0, 100000100), bk)); ASSERT_EQ(100000100, rcv.getLastTransferTimestampMonotonic().toUSec()); + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + ASSERT_TRUE(rcv.isTimedOut(tsMono(900000000))); CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "\x78\x56" "345678", 0, false, 0, 900000000), bk)); // Global timeout CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "12345678", 0, false, 0, 900000100), bk)); // Wrong iface CHECK_NOT_COMPLETE(rcv.addFrame(gen(0, "qwe", 1, true, 0, 900000200), bk)); // Wrong iface CHECK_COMPLETE( rcv.addFrame(gen(1, "qwe", 1, true, 0, 900000200), bk)); + ASSERT_EQ(900000000, rcv.getLastTransferTimestampMonotonic().toUSec()); + std::cout << "Interval: " << rcv.getInterval().toString() << std::endl; + ASSERT_FALSE(rcv.isTimedOut(tsMono(1000))); - ASSERT_FALSE(rcv.isTimedOut(tsMono(900000200))); - ASSERT_TRUE(rcv.isTimedOut(tsMono(1000 * 1000000))); + ASSERT_FALSE(rcv.isTimedOut(tsMono(900000300))); + ASSERT_TRUE(rcv.isTimedOut(tsMono(990000000))); + ASSERT_LT(TransferReceiver::getDefaultTransferInterval(), rcv.getInterval()); ASSERT_LE(TransferReceiver::getMinTransferInterval(), rcv.getInterval()); ASSERT_GE(TransferReceiver::getMaxTransferInterval(), rcv.getInterval()); diff --git a/libuavcan/test/transport/transfer_sender.cpp b/libuavcan/test/transport/transfer_sender.cpp index 5073824658..8c4aed871f 100644 --- a/libuavcan/test/transport/transfer_sender.cpp +++ b/libuavcan/test/transport/transfer_sender.cpp @@ -29,7 +29,7 @@ static int sendOne(uavcan::TransferSender& sender, const std::string& data, TEST(TransferSender, Basic) { - uavcan::PoolManager<1> poolmgr; + NullAllocator poolmgr; SystemClockMock clockmock(100); CanDriverMock driver(2, clockmock); @@ -185,7 +185,7 @@ struct TransferSenderTestLoopbackFrameListener : public uavcan::LoopbackFrameLis TEST(TransferSender, Loopback) { - uavcan::PoolManager<1> poolmgr; + NullAllocator poolmgr; SystemClockMock clockmock(100); CanDriverMock driver(2, clockmock); @@ -226,7 +226,7 @@ TEST(TransferSender, Loopback) TEST(TransferSender, PassiveMode) { - uavcan::PoolManager<1> poolmgr; + NullAllocator poolmgr; SystemClockMock clockmock(100); CanDriverMock driver(2, clockmock); diff --git a/libuavcan/test/transport/transfer_test_helpers.cpp b/libuavcan/test/transport/transfer_test_helpers.cpp index 49f6da6c3f..5b37b546c2 100644 --- a/libuavcan/test/transport/transfer_test_helpers.cpp +++ b/libuavcan/test/transport/transfer_test_helpers.cpp @@ -10,10 +10,8 @@ TEST(TransferTestHelpers, Transfer) { uavcan::PoolAllocator pool; - uavcan::PoolManager<1> poolmgr; - poolmgr.addPool(&pool); - uavcan::TransferBufferManager<128, 1> mgr(poolmgr); + uavcan::TransferBufferManager<128, 1> mgr(pool); uavcan::TransferBufferAccessor tba(mgr, uavcan::TransferBufferManagerKey(0, uavcan::TransferTypeMessageUnicast)); uavcan::RxFrame frame(uavcan::Frame(123, uavcan::TransferTypeMessageBroadcast, 1, 0, 0, 0, true), diff --git a/libuavcan/test/transport/transfer_test_helpers.hpp b/libuavcan/test/transport/transfer_test_helpers.hpp index 5a67bf31c2..af06f998e8 100644 --- a/libuavcan/test/transport/transfer_test_helpers.hpp +++ b/libuavcan/test/transport/transfer_test_helpers.hpp @@ -323,3 +323,14 @@ public: template void send(const Transfer (&transfers)[SIZE]) { send(transfers, SIZE); } }; + +/** + * Zero allocator - always fails + */ +class NullAllocator : public uavcan::IPoolAllocator +{ +public: + virtual void* allocate(std::size_t) { return NULL; } + virtual void deallocate(const void*) { } + virtual uint16_t getNumBlocks() const { return 0; } +}; diff --git a/libuavcan/test/util/map.cpp b/libuavcan/test/util/map.cpp index e394415b00..60386058ec 100644 --- a/libuavcan/test/util/map.cpp +++ b/libuavcan/test/util/map.cpp @@ -45,11 +45,9 @@ TEST(Map, Basic) static const int POOL_BLOCKS = 3; uavcan::PoolAllocator pool; - uavcan::PoolManager<2> poolmgr; - poolmgr.addPool(&pool); typedef Map MapType; - std::auto_ptr map(new MapType(poolmgr)); + std::auto_ptr map(new MapType(pool)); // Empty ASSERT_FALSE(map->access("hi")); @@ -208,11 +206,9 @@ TEST(Map, NoStatic) static const int POOL_BLOCKS = 3; uavcan::PoolAllocator pool; - uavcan::PoolManager<2> poolmgr; - poolmgr.addPool(&pool); typedef Map MapType; - std::auto_ptr map(new MapType(poolmgr)); + std::auto_ptr map(new MapType(pool)); // Empty ASSERT_FALSE(map->access("hi")); @@ -241,11 +237,9 @@ TEST(Map, PrimitiveKey) static const int POOL_BLOCKS = 3; uavcan::PoolAllocator pool; - uavcan::PoolManager<2> poolmgr; - poolmgr.addPool(&pool); typedef Map MapType; - std::auto_ptr map(new MapType(poolmgr)); + std::auto_ptr map(new MapType(pool)); // Empty ASSERT_FALSE(map->access(1)); diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp index 8ff43e69b9..505deb8ddc 100644 --- a/libuavcan/test/util/multiset.cpp +++ b/libuavcan/test/util/multiset.cpp @@ -70,11 +70,9 @@ TEST(Multiset, Basic) static const int POOL_BLOCKS = 3; uavcan::PoolAllocator pool; - uavcan::PoolManager<2> poolmgr; - poolmgr.addPool(&pool); typedef Multiset MultisetType; - std::auto_ptr mset(new MultisetType(poolmgr)); + std::auto_ptr mset(new MultisetType(pool)); typedef SummationOperator StringConcatenationOperator; @@ -220,11 +218,9 @@ TEST(Multiset, PrimitiveKey) static const int POOL_BLOCKS = 3; uavcan::PoolAllocator pool; - uavcan::PoolManager<2> poolmgr; - poolmgr.addPool(&pool); typedef Multiset MultisetType; - std::auto_ptr mset(new MultisetType(poolmgr)); + std::auto_ptr mset(new MultisetType(pool)); // Empty mset->removeFirst(8); @@ -272,11 +268,9 @@ TEST(Multiset, NoncopyableWithCounter) static const int POOL_BLOCKS = 3; uavcan::PoolAllocator pool; - uavcan::PoolManager<2> poolmgr; - poolmgr.addPool(&pool); typedef Multiset MultisetType; - std::auto_ptr mset(new MultisetType(poolmgr)); + std::auto_ptr mset(new MultisetType(pool)); ASSERT_EQ(0, NoncopyableWithCounter::num_objects); ASSERT_EQ(0, mset->emplace()->value); diff --git a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp index c9e481f620..245d1fbd7e 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp @@ -601,11 +601,6 @@ public: { return (iface_index >= num_ifaces_) ? nullptr : static_cast(ifaces_[iface_index]); } - const SocketCanIface* getIface(std::uint8_t iface_index) const override - { - return (iface_index >= num_ifaces_) ? nullptr : static_cast(ifaces_[iface_index]); - } - std::uint8_t getNumIfaces() const override { return num_ifaces_; } diff --git a/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/can.hpp b/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/can.hpp index 26bf8f2784..46d5c81632 100644 --- a/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/can.hpp +++ b/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/can.hpp @@ -61,8 +61,6 @@ public: virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index); - virtual const uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) const; - virtual uavcan::uint8_t getNumIfaces() const; }; diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile index e2c37a1a3c..818975feca 100644 --- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile +++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile @@ -114,7 +114,7 @@ $(CPPOBJ): $(OBJDIR)/%.o: %.cpp $(CPPC) -c $(DEF) $(INC) $(CPPFLAGS) $< -o $@ clean: - rm -rf $(BUILDDIR) + rm -rf $(BUILDDIR) dsdlc_generated size: $(ELF) @if [ -f $(ELF) ]; then echo; $(SIZE) $(ELF); echo; fi; diff --git a/libuavcan_drivers/stm32/test_stm32f107/Makefile b/libuavcan_drivers/stm32/test_stm32f107/Makefile index 6695c37092..543a766a74 100644 --- a/libuavcan_drivers/stm32/test_stm32f107/Makefile +++ b/libuavcan_drivers/stm32/test_stm32f107/Makefile @@ -17,7 +17,8 @@ CPPSRC = src/main.cpp src/dummy.cpp UDEFS = -DUAVCAN_STM32_CHIBIOS=1 \ -DUAVCAN_STM32_TIMER_NUMBER=6 \ -DUAVCAN_TINY=1 \ - -DUAVCAN_STM32_NUM_IFACES=2 + -DUAVCAN_STM32_NUM_IFACES=2 \ + -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 include ../../../libuavcan/include.mk CPPSRC += $(LIBUAVCAN_SRC)