fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 07:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

CAN HW filters are added using map container

Browse Source
This commit is contained in:
ilia-sheremet
2015-06-26 14:35:53 +02:00
parent bad7123b7d
commit 573b268457
43 changed files with 953 additions and 782 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/include/uavcan/build_config.hpp
+29 -15
View File
@@ -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
libuavcan/include/uavcan/data_type.hpp
+10 -10
View File
@@ -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);
libuavcan/include/uavcan/driver/can.hpp
+19 -16
View File
@@ -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<ICanDriver*>(this)->getIface(iface_index);
}
/**
* Total number of available CAN interfaces.
libuavcan/include/uavcan/dynamic_memory.hpp
+47 -179
View File
@@ -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 <unsigned MaxPools>
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<const IPoolAllocator* const*>(raw_a);
const IPoolAllocator* const b = *static_cast<const IPoolAllocator* const*>(raw_b);
const std::size_t a_size = a ? a->getBlockSize() : NumericTraits<std::size_t>::max();
const std::size_t b_size = b ? b->getBlockSize() : NumericTraits<std::size_t>::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 <std::size_t PoolSize, std::size_t BlockSize>
template <std::size_t PoolSize, uint8_t BlockSize>
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<uint16_t>(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<uint16_t>(min<std::size_t>(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 <unsigned MaxPools>
bool PoolManager<MaxPools>::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 <unsigned MaxPools>
void* PoolManager<MaxPools>::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 <unsigned MaxPools>
void PoolManager<MaxPools>::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 <unsigned MaxPools>
bool PoolManager<MaxPools>::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 <unsigned MaxPools>
std::size_t PoolManager<MaxPools>::getBlockSize() const
{
return 0;
}
template <unsigned MaxPools>
std::size_t PoolManager<MaxPools>::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 <std::size_t PoolSize, std::size_t BlockSize>
PoolAllocator<PoolSize, BlockSize>::PoolAllocator()
: free_list_(reinterpret_cast<Node*>(pool_.bytes))
template <std::size_t PoolSize, uint8_t BlockSize>
const uint16_t PoolAllocator<PoolSize, BlockSize>::NumBlocks;
template <std::size_t PoolSize, uint8_t BlockSize>
PoolAllocator<PoolSize, BlockSize>::PoolAllocator() :
free_list_(reinterpret_cast<Node*>(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<PoolSize, BlockSize>::PoolAllocator()
free_list_[NumBlocks - 1].next = NULL;
}
template <std::size_t PoolSize, std::size_t BlockSize>
template <std::size_t PoolSize, uint8_t BlockSize>
void* PoolAllocator<PoolSize, BlockSize>::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 <std::size_t PoolSize, std::size_t BlockSize>
template <std::size_t PoolSize, uint8_t BlockSize>
void PoolAllocator<PoolSize, BlockSize>::deallocate(const void* ptr)
{
if (ptr == NULL)
{
return;
}
Node* p = static_cast<Node*>(const_cast<void*>(ptr));
p->next = free_list_;
free_list_ = p;
}
template <std::size_t PoolSize, std::size_t BlockSize>
bool PoolAllocator<PoolSize, BlockSize>::isInPool(const void* ptr) const
{
return ptr >= pool_.bytes &&
ptr < (pool_.bytes + PoolSize);
}
template <std::size_t PoolSize, std::size_t BlockSize>
unsigned PoolAllocator<PoolSize, BlockSize>::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_--;
}
}
libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp
+14 -24
View File
@@ -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(); }
};
}
libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp
+62 -165
View File
@@ -9,6 +9,7 @@
#include <uavcan/debug.hpp>
#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
#include <uavcan/util/bitset.hpp>
namespace uavcan
{
@@ -22,148 +23,59 @@ namespace centralized
*/
class Storage
{
public:
typedef uint8_t Size;
typedef BitSet<NodeID::Max + 1> OccupationMask;
typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeStatic,
BitLenToByteLen<NodeID::Max + 1>::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<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(occupation_mask_.count()); }
};
}
libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp
+1 -3
View File
@@ -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<protocol::GetNodeInfo, GetNodeInfoResponseCallback> get_node_info_client_;
Subscriber<protocol::NodeStatus, NodeStatusCallback, NumNodeStatusStaticReceivers, 0> node_status_sub_;
Subscriber<protocol::NodeStatus, NodeStatusCallback, MaxNetworkSizeHint, 0> node_status_sub_;
/*
* Methods
libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp
+15 -6
View File
@@ -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);
libuavcan/include/uavcan/protocol/node_info_retriever.hpp
+16
View File
@@ -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.
libuavcan/include/uavcan/protocol/node_status_monitor.hpp
+12 -6
View File
@@ -51,12 +51,7 @@ private:
typedef MethodBinder<NodeStatusMonitor*, void (NodeStatusMonitor::*)(const TimerEvent&)> TimerCallback;
/*
* We'll be able to handle this many nodes in the network without any dynamic memory.
*/
enum { NumStaticReceivers = 64 };
Subscriber<protocol::NodeStatus, NodeStatusCallback, NumStaticReceivers, 0> sub_;
Subscriber<protocol::NodeStatus, NodeStatusCallback, MaxNetworkSizeHint, 0> sub_;
TimerEventForwarder<TimerCallback> 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.
libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp
+105
View File
@@ -0,0 +1,105 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>,
* Ilia Sheremet <illia.sheremet@gmail.com>
*/
#ifndef UAVCAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED
#define UAVCAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED
#include <cassert>
#include <uavcan/data_type.hpp>
#include <uavcan/error.hpp>
#include <uavcan/transport/dispatcher.hpp>
#include <uavcan/node/abstract_node.hpp>
#include <uavcan/build_config.hpp>
#include <uavcan/util/multiset.hpp>
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<CanFilterConfig, 1> 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
libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp
-4
View File
@@ -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 <int NumStaticEntries>
class UAVCAN_EXPORT OutgoingTransferRegistry : public IOutgoingTransferRegistry, Noncopyable
{
UAVCAN_PACKED_BEGIN
struct Value
{
MonotonicTime deadline;
TransferID tid;
};
UAVCAN_PACKED_END
class DeadlineExpiredPredicate
{
libuavcan/include/uavcan/transport/transfer_buffer.hpp
-2
View File
@@ -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
libuavcan/include/uavcan/transport/transfer_receiver.hpp
+36 -24
View File
@@ -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<sizeof(TransferReceiver) == 32>::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
}
libuavcan/include/uavcan/util/map.hpp
+8 -9
View File
@@ -36,22 +36,22 @@ class UAVCAN_EXPORT MapBase : Noncopyable
{
template <typename, typename, unsigned> 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<KVGroup> list_;
IPoolAllocator& allocator_;