mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-21 10:17:36 +08:00
Automated style fixes in order to bring the code a little bit closer to compliance with MISRA C++ rules. No changes in logic.
This commit is contained in:
@@ -34,7 +34,7 @@ public:
|
||||
DataTypeID() : value_(0xFFFF) { }
|
||||
|
||||
DataTypeID(uint16_t id)
|
||||
: value_(id)
|
||||
: value_(id)
|
||||
{
|
||||
assert(isValid());
|
||||
}
|
||||
@@ -81,14 +81,18 @@ public:
|
||||
static const uint64_t Poly = 0x42F0E1EBA9EA3693;
|
||||
crc_ ^= uint64_t(byte) << 56;
|
||||
for (int i = 0; i < 8; i++)
|
||||
{
|
||||
crc_ = (crc_ & (uint64_t(1) << 63)) ? (crc_ << 1) ^ Poly : crc_ << 1;
|
||||
}
|
||||
}
|
||||
|
||||
void add(const uint8_t* bytes, unsigned int len)
|
||||
{
|
||||
assert(bytes);
|
||||
while (len--)
|
||||
{
|
||||
add(*bytes++);
|
||||
}
|
||||
}
|
||||
|
||||
uint64_t get() const { return crc_ ^ 0xFFFFFFFFFFFFFFFF; }
|
||||
@@ -127,15 +131,15 @@ public:
|
||||
enum { MaxFullNameLen = 80 };
|
||||
|
||||
DataTypeDescriptor()
|
||||
: kind_(DataTypeKind(0))
|
||||
, full_name_("")
|
||||
: kind_(DataTypeKind(0))
|
||||
, full_name_("")
|
||||
{ }
|
||||
|
||||
DataTypeDescriptor(DataTypeKind kind, DataTypeID id, const DataTypeSignature& signature, const char* name)
|
||||
: kind_(kind)
|
||||
, id_(id)
|
||||
, signature_(signature)
|
||||
, full_name_(name)
|
||||
: kind_(kind)
|
||||
, id_(id)
|
||||
, signature_(signature)
|
||||
, full_name_(name)
|
||||
{
|
||||
assert(kind < NumDataTypeKinds);
|
||||
assert(name);
|
||||
|
||||
@@ -7,12 +7,12 @@
|
||||
|
||||
#if UAVCAN_DEBUG
|
||||
|
||||
#include <cstdio>
|
||||
#include <cstdarg>
|
||||
# include <cstdio>
|
||||
# include <cstdarg>
|
||||
|
||||
#if __GNUC__
|
||||
__attribute__ ((format (printf, 2, 3)))
|
||||
#endif
|
||||
# if __GNUC__
|
||||
__attribute__ ((format(printf, 2, 3)))
|
||||
# endif
|
||||
static void UAVCAN_TRACE(const char* src, const char* fmt, ...)
|
||||
{
|
||||
va_list args;
|
||||
@@ -25,6 +25,6 @@ static void UAVCAN_TRACE(const char* src, const char* fmt, ...)
|
||||
|
||||
#else
|
||||
|
||||
# define UAVCAN_TRACE(...) ((void)0)
|
||||
# define UAVCAN_TRACE(...) ((void)0)
|
||||
|
||||
#endif
|
||||
|
||||
@@ -31,15 +31,15 @@ struct CanFrame
|
||||
uint8_t dlc; ///< Data Length Code
|
||||
|
||||
CanFrame()
|
||||
: id(0)
|
||||
, dlc(0)
|
||||
: id(0)
|
||||
, dlc(0)
|
||||
{
|
||||
std::fill(data, data + sizeof(data), 0);
|
||||
}
|
||||
|
||||
CanFrame(uint32_t id, const uint8_t* data, unsigned int dlc)
|
||||
: id(id)
|
||||
, dlc(dlc)
|
||||
: id(id)
|
||||
, dlc(dlc)
|
||||
{
|
||||
assert(data && dlc <= 8);
|
||||
std::copy(data, data + dlc, this->data);
|
||||
@@ -89,8 +89,8 @@ struct CanSelectMasks
|
||||
uint8_t write;
|
||||
|
||||
CanSelectMasks()
|
||||
: read(0)
|
||||
, write(0)
|
||||
: read(0)
|
||||
, write(0)
|
||||
{ }
|
||||
};
|
||||
|
||||
|
||||
@@ -76,10 +76,14 @@ public:
|
||||
for (int i = 0; i < MaxPools; i++)
|
||||
{
|
||||
if (pools_[i] == NULL)
|
||||
{
|
||||
break;
|
||||
}
|
||||
void* const pmem = pools_[i]->allocate(size);
|
||||
if (pmem != NULL)
|
||||
{
|
||||
return pmem;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
@@ -123,18 +127,22 @@ public:
|
||||
static const int NumBlocks = int(PoolSize / BlockSize);
|
||||
|
||||
PoolAllocator()
|
||||
: free_list_(reinterpret_cast<Node*>(pool_)) // TODO: alignment
|
||||
: free_list_(reinterpret_cast<Node*>(pool_)) // TODO: alignment
|
||||
{
|
||||
memset(pool_, 0, PoolSize);
|
||||
for (int i = 0; i < NumBlocks - 1; i++)
|
||||
{
|
||||
free_list_[i].next = free_list_ + i + 1;
|
||||
}
|
||||
free_list_[NumBlocks - 1].next = NULL;
|
||||
}
|
||||
|
||||
void* allocate(std::size_t size)
|
||||
{
|
||||
if (free_list_ == NULL || size > BlockSize)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
void* pmem = free_list_;
|
||||
free_list_ = free_list_->next;
|
||||
return pmem;
|
||||
@@ -143,7 +151,9 @@ public:
|
||||
void deallocate(const void* ptr)
|
||||
{
|
||||
if (ptr == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
Node* p = static_cast<Node*>(const_cast<void*>(ptr));
|
||||
#if DEBUG || UAVCAN_DEBUG
|
||||
std::memset(p, 0, sizeof(Node));
|
||||
|
||||
@@ -20,7 +20,7 @@ class LinkedListNode
|
||||
|
||||
protected:
|
||||
LinkedListNode()
|
||||
: next_(NULL)
|
||||
: next_(NULL)
|
||||
{ }
|
||||
|
||||
public:
|
||||
@@ -42,7 +42,7 @@ class LinkedListRoot
|
||||
|
||||
public:
|
||||
LinkedListRoot()
|
||||
: root_(NULL)
|
||||
: root_(NULL)
|
||||
{ }
|
||||
|
||||
T* get() const { return root_; }
|
||||
@@ -88,7 +88,9 @@ public:
|
||||
while (p->getNextListNode())
|
||||
{
|
||||
if (predicate(p->getNextListNode()))
|
||||
{
|
||||
break;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
node->setNextListNode(p->getNextListNode());
|
||||
@@ -99,7 +101,9 @@ public:
|
||||
bool remove(const T* node)
|
||||
{
|
||||
if (root_ == NULL || node == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (root_ == node)
|
||||
{
|
||||
|
||||
@@ -24,13 +24,16 @@ namespace uavcan
|
||||
template <typename Key, typename Value, unsigned int NumStaticEntries>
|
||||
class Map : Noncopyable
|
||||
{
|
||||
UAVCAN_PACKED_BEGIN
|
||||
UAVCAN_PACKED_BEGIN
|
||||
struct KVPair
|
||||
{
|
||||
Key key;
|
||||
Value value;
|
||||
KVPair() { }
|
||||
KVPair(const Key& key, const Value& value) : key(key), value(value) { }
|
||||
KVPair(const Key& key, const Value& value)
|
||||
: key(key)
|
||||
, value(value)
|
||||
{ }
|
||||
bool match(const Key& rhs) const { return rhs == key; }
|
||||
};
|
||||
|
||||
@@ -49,7 +52,9 @@ UAVCAN_PACKED_BEGIN
|
||||
{
|
||||
void* const praw = allocator.allocate(sizeof(KVGroup));
|
||||
if (praw == NULL)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
return new (praw) KVGroup();
|
||||
}
|
||||
|
||||
@@ -66,12 +71,16 @@ UAVCAN_PACKED_BEGIN
|
||||
KVPair* find(const Key& key)
|
||||
{
|
||||
for (int i = 0; i < NumKV; i++)
|
||||
{
|
||||
if (kvs[i].match(key))
|
||||
{
|
||||
return kvs + i;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
};
|
||||
UAVCAN_PACKED_END
|
||||
UAVCAN_PACKED_END
|
||||
|
||||
LinkedListRoot<KVGroup> list_;
|
||||
IAllocator& allocator_;
|
||||
@@ -80,15 +89,21 @@ UAVCAN_PACKED_END
|
||||
KVPair* find(const Key& key)
|
||||
{
|
||||
for (unsigned int i = 0; i < NumStaticEntries; i++)
|
||||
{
|
||||
if (static_[i].match(key))
|
||||
{
|
||||
return static_ + i;
|
||||
}
|
||||
}
|
||||
|
||||
KVGroup* p = list_.get();
|
||||
while (p)
|
||||
{
|
||||
KVPair* const kv = p->find(key);
|
||||
if (kv)
|
||||
{
|
||||
return kv;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
return NULL;
|
||||
@@ -109,7 +124,9 @@ UAVCAN_PACKED_END
|
||||
}
|
||||
}
|
||||
if (stat == NULL)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
// Looking for the first NON-EMPTY dynamic entry, erasing immediately
|
||||
KVGroup* p = list_.get();
|
||||
@@ -128,11 +145,15 @@ UAVCAN_PACKED_END
|
||||
}
|
||||
}
|
||||
if (stop)
|
||||
{
|
||||
break;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
if (dyn.match(Key()))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
// Migrating
|
||||
*stat = dyn;
|
||||
@@ -174,7 +195,7 @@ UAVCAN_PACKED_END
|
||||
|
||||
public:
|
||||
Map(IAllocator& allocator)
|
||||
: allocator_(allocator)
|
||||
: allocator_(allocator)
|
||||
{
|
||||
assert(Key() == Key());
|
||||
}
|
||||
@@ -203,7 +224,9 @@ public:
|
||||
|
||||
KVGroup* const kvg = KVGroup::instantiate(allocator_);
|
||||
if (kvg == NULL)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
list_.insert(kvg);
|
||||
kvg->kvs[0] = KVPair(key, value);
|
||||
return &kvg->kvs[0].value;
|
||||
@@ -313,8 +336,12 @@ public:
|
||||
{
|
||||
unsigned int num = 0;
|
||||
for (unsigned int i = 0; i < NumStaticEntries; i++)
|
||||
{
|
||||
if (!static_[i].match(Key()))
|
||||
{
|
||||
num++;
|
||||
}
|
||||
}
|
||||
return num;
|
||||
}
|
||||
|
||||
@@ -329,7 +356,9 @@ public:
|
||||
{
|
||||
const KVPair* const kv = p->kvs + i;
|
||||
if (!kv->match(Key()))
|
||||
{
|
||||
num++;
|
||||
}
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
|
||||
@@ -42,7 +42,9 @@ protected:
|
||||
SizeType validateRange(SizeType pos) const
|
||||
{
|
||||
if (pos < Size)
|
||||
{
|
||||
return pos;
|
||||
}
|
||||
#if UAVCAN_EXCEPTIONS
|
||||
throw std::out_of_range("uavcan::Array");
|
||||
#else
|
||||
@@ -71,7 +73,9 @@ protected:
|
||||
SizeType validateRange(SizeType pos) const
|
||||
{
|
||||
if (pos < size_)
|
||||
{
|
||||
return pos;
|
||||
}
|
||||
#if UAVCAN_EXCEPTIONS
|
||||
throw std::out_of_range("uavcan::Array");
|
||||
#else
|
||||
@@ -83,15 +87,21 @@ protected:
|
||||
void grow()
|
||||
{
|
||||
if (size_ >= MaxSize)
|
||||
{
|
||||
validateRange(MaxSize); // Will throw, assert() or do nothing
|
||||
}
|
||||
else
|
||||
{
|
||||
size_++;
|
||||
}
|
||||
}
|
||||
|
||||
void shrink()
|
||||
{
|
||||
if (size_ > 0)
|
||||
{
|
||||
size_--;
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
@@ -122,8 +132,8 @@ class ArrayImpl : public Select<ArrayMode == ArrayModeDynamic,
|
||||
public:
|
||||
enum
|
||||
{
|
||||
IsStringLike = IsIntegerSpec<T>::Result && (T::MaxBitLen == 8 || T::MaxBitLen == 7)
|
||||
&& (ArrayMode == ArrayModeDynamic)
|
||||
IsStringLike = IsIntegerSpec<T>::Result && (T::MaxBitLen == 8 || T::MaxBitLen == 7) &&
|
||||
(ArrayMode == ArrayModeDynamic)
|
||||
};
|
||||
|
||||
private:
|
||||
@@ -134,7 +144,9 @@ private:
|
||||
typename EnableIf<sizeof(U(0) == U())>::Type initialize(int)
|
||||
{
|
||||
if (ArrayMode != ArrayModeDynamic)
|
||||
{
|
||||
std::fill(data_, data_ + MaxSize, U());
|
||||
}
|
||||
}
|
||||
template <typename> void initialize(...) { }
|
||||
|
||||
@@ -233,7 +245,9 @@ class Array : public ArrayImpl<T, ArrayMode, MaxSize_>
|
||||
const bool last_item = i == (size() - 1);
|
||||
const int res = RawValueType::encode(Base::at(i), codec, last_item ? tao_mode : TailArrayOptDisabled);
|
||||
if (res <= 0)
|
||||
{
|
||||
return res;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
@@ -246,10 +260,14 @@ class Array : public ArrayImpl<T, ArrayMode, MaxSize_>
|
||||
{
|
||||
const int res_sz = Base::RawSizeType::encode(size(), codec, TailArrayOptDisabled);
|
||||
if (res_sz <= 0)
|
||||
{
|
||||
return res_sz;
|
||||
}
|
||||
}
|
||||
if (size() == 0)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
return encodeImpl(codec, self_tao_enabled ? TailArrayOptDisabled : tao_mode, FalseType());
|
||||
}
|
||||
|
||||
@@ -262,7 +280,9 @@ class Array : public ArrayImpl<T, ArrayMode, MaxSize_>
|
||||
ValueType value; // TODO: avoid extra copy
|
||||
const int res = RawValueType::decode(value, codec, last_item ? tao_mode : TailArrayOptDisabled);
|
||||
if (res <= 0)
|
||||
{
|
||||
return res;
|
||||
}
|
||||
Base::at(i) = value;
|
||||
}
|
||||
return 1;
|
||||
@@ -279,11 +299,17 @@ class Array : public ArrayImpl<T, ArrayMode, MaxSize_>
|
||||
ValueType value;
|
||||
const int res = RawValueType::decode(value, codec, TailArrayOptDisabled);
|
||||
if (res < 0)
|
||||
{
|
||||
return res;
|
||||
}
|
||||
if (res == 0) // Success: End of stream reached (even if zero items were read)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
if (size() == MaxSize_) // Error: Max array length reached, but the end of stream is not
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
push_back(value);
|
||||
}
|
||||
}
|
||||
@@ -292,12 +318,18 @@ class Array : public ArrayImpl<T, ArrayMode, MaxSize_>
|
||||
typename StorageType<typename Base::RawSizeType>::Type sz = 0;
|
||||
const int res_sz = Base::RawSizeType::decode(sz, codec, TailArrayOptDisabled);
|
||||
if (res_sz <= 0)
|
||||
{
|
||||
return res_sz;
|
||||
}
|
||||
if ((sz > 0) && ((sz - 1u) > (MaxSize_ - 1u))) // -Werror=type-limits
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
resize(sz);
|
||||
if (sz == 0)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
return decodeImpl(codec, tao_mode, FalseType());
|
||||
}
|
||||
assert(0); // Unreachable
|
||||
@@ -347,13 +379,17 @@ public:
|
||||
{
|
||||
unsigned int cnt = new_size - size();
|
||||
while (cnt--)
|
||||
{
|
||||
push_back(filler);
|
||||
}
|
||||
}
|
||||
else if (new_size < size())
|
||||
{
|
||||
unsigned int cnt = size() - new_size;
|
||||
while (cnt--)
|
||||
{
|
||||
pop_back();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -370,17 +406,25 @@ public:
|
||||
operator==(const R& rhs) const
|
||||
{
|
||||
if (size() != rhs.size())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
for (SizeType i = 0; i < size(); i++) // Bitset does not have iterators
|
||||
{
|
||||
if (!(Base::at(i) == rhs[i]))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool operator==(const char* ch) const
|
||||
{
|
||||
if (ch == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return std::strcmp(Base::c_str(), ch) == 0;
|
||||
}
|
||||
|
||||
@@ -395,9 +439,13 @@ public:
|
||||
StaticAssert<IsDynamic>::check();
|
||||
Base::clear();
|
||||
if (ch == NULL)
|
||||
{
|
||||
handleFatalError("Null pointer in Array<>::operator=(const char*)");
|
||||
}
|
||||
while (*ch)
|
||||
{
|
||||
push_back(*ch++);
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
@@ -406,9 +454,13 @@ public:
|
||||
StaticAssert<Base::IsStringLike>::check();
|
||||
StaticAssert<IsDynamic>::check();
|
||||
if (ch == NULL)
|
||||
{
|
||||
handleFatalError("Null pointer in Array<>::operator+=(const char*)");
|
||||
}
|
||||
while (*ch)
|
||||
{
|
||||
push_back(*ch++);
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
@@ -420,7 +472,9 @@ public:
|
||||
SizeType, typename Rhs::SizeType>::Result CommonSizeType;
|
||||
StaticAssert<IsDynamic>::check();
|
||||
for (CommonSizeType i = 0; i < rhs.size(); i++)
|
||||
{
|
||||
push_back(rhs[i]);
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
@@ -435,7 +489,7 @@ public:
|
||||
StaticAssert<IsDynamic>::check();
|
||||
|
||||
StaticAssert<sizeof(A() == A(0))>::check(); // This check allows to weed out most non-trivial types
|
||||
StaticAssert<sizeof(A) <= sizeof(long double)>::check();// Another stupid check to catch non-trivial types
|
||||
StaticAssert<sizeof(A) <= sizeof(long double)>::check(); // Another stupid check to catch non-trivial types
|
||||
|
||||
if (!format)
|
||||
{
|
||||
@@ -487,11 +541,17 @@ class YamlStreamer<Array<T, ArrayMode, MaxSize> >
|
||||
static bool isNiceCharacter(int c)
|
||||
{
|
||||
if (c >= 32 && c <= 126)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
static const char Good[] = {'\n', '\r', '\t'};
|
||||
for (unsigned int i = 0; i < sizeof(Good) / sizeof(Good[0]); i++)
|
||||
{
|
||||
if (Good[i] == c)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -503,7 +563,9 @@ class YamlStreamer<Array<T, ArrayMode, MaxSize> >
|
||||
{
|
||||
YamlStreamer<T>::stream(s, array.at(i), 0);
|
||||
if ((i + 1) < array.size())
|
||||
{
|
||||
s << ", ";
|
||||
}
|
||||
}
|
||||
s << ']';
|
||||
}
|
||||
@@ -522,14 +584,18 @@ class YamlStreamer<Array<T, ArrayMode, MaxSize> >
|
||||
{
|
||||
nibbles[i] += '0';
|
||||
if (nibbles[i] > '9')
|
||||
{
|
||||
nibbles[i] += 'A' - '9' - 1;
|
||||
}
|
||||
}
|
||||
s << "\\x" << nibbles[0] << nibbles[1];
|
||||
}
|
||||
else
|
||||
{
|
||||
if (c == '"' || c == '\\') // YAML requires to escape these two
|
||||
{
|
||||
s << '\\';
|
||||
}
|
||||
s << char(c);
|
||||
}
|
||||
}
|
||||
@@ -582,7 +648,9 @@ class YamlStreamer<Array<T, ArrayMode, MaxSize> >
|
||||
{
|
||||
s << '\n';
|
||||
for (int pos = 0; pos < level; pos++)
|
||||
{
|
||||
s << " ";
|
||||
}
|
||||
s << "- ";
|
||||
YamlStreamer<T>::stream(s, array.at(i), level + 1);
|
||||
}
|
||||
@@ -593,8 +661,9 @@ public:
|
||||
static void stream(Stream& s, const ArrayType& array, int level)
|
||||
{
|
||||
typedef typename Select<ArrayType::IsStringLike, SelectorStringLike,
|
||||
typename Select<IsPrimitiveType<typename ArrayType::RawValueType>::Result, SelectorPrimitives,
|
||||
SelectorObjects>::Result >::Result Type;
|
||||
typename Select<IsPrimitiveType<typename ArrayType::RawValueType>::Result,
|
||||
SelectorPrimitives,
|
||||
SelectorObjects>::Result >::Result Type;
|
||||
genericStreamImpl(s, array, level, Type());
|
||||
}
|
||||
};
|
||||
|
||||
@@ -43,9 +43,9 @@ public:
|
||||
};
|
||||
|
||||
BitStream(ITransferBuffer& buf)
|
||||
: buf_(buf)
|
||||
, bit_offset_(0)
|
||||
, byte_cache_(0)
|
||||
: buf_(buf)
|
||||
, bit_offset_(0)
|
||||
, byte_cache_(0)
|
||||
{
|
||||
StaticAssert<sizeof(uint8_t) == 1>::check();
|
||||
}
|
||||
|
||||
@@ -39,7 +39,9 @@ class CharArrayFormatter
|
||||
if (std::is_same<T, char>())
|
||||
{
|
||||
if (array_.size() != array_.capacity())
|
||||
{
|
||||
array_.push_back(value);
|
||||
}
|
||||
}
|
||||
else if (std::is_signed<T>())
|
||||
{
|
||||
@@ -67,7 +69,7 @@ public:
|
||||
typedef ArrayType_ ArrayType;
|
||||
|
||||
CharArrayFormatter(ArrayType& array)
|
||||
: array_(array)
|
||||
: array_(array)
|
||||
{ }
|
||||
|
||||
ArrayType& getArray() { return array_; }
|
||||
@@ -78,7 +80,7 @@ public:
|
||||
writeValue(text);
|
||||
}
|
||||
|
||||
template<typename T, typename... Args>
|
||||
template <typename T, typename... Args>
|
||||
void write(const char* s, T value, Args... args)
|
||||
{
|
||||
while (s && *s)
|
||||
@@ -105,7 +107,7 @@ public:
|
||||
typedef ArrayType_ ArrayType;
|
||||
|
||||
CharArrayFormatter(ArrayType& array)
|
||||
: array_(array)
|
||||
: array_(array)
|
||||
{ }
|
||||
|
||||
ArrayType& getArray() { return array_; }
|
||||
|
||||
@@ -124,9 +124,13 @@ public:
|
||||
{
|
||||
// cppcheck-suppress duplicateExpression
|
||||
if (CastMode == CastModeSaturate)
|
||||
{
|
||||
saturate(value);
|
||||
}
|
||||
else
|
||||
{
|
||||
truncate(value);
|
||||
}
|
||||
return codec.encode<BitLen>(IEEE754Converter::toIeee<BitLen>(value));
|
||||
}
|
||||
|
||||
@@ -135,7 +139,9 @@ public:
|
||||
typename IntegerSpec<BitLen, SignednessUnsigned, CastModeTruncate>::StorageType ieee = 0;
|
||||
const int res = codec.decode<BitLen>(ieee);
|
||||
if (res <= 0)
|
||||
{
|
||||
return res;
|
||||
}
|
||||
out_value = IEEE754Converter::toNative<BitLen>(ieee);
|
||||
return res;
|
||||
}
|
||||
@@ -149,9 +155,13 @@ private:
|
||||
if (!IsExactRepresentation && isfinite(value))
|
||||
{
|
||||
if (value > max())
|
||||
{
|
||||
value = max();
|
||||
}
|
||||
else if (value < -max())
|
||||
{
|
||||
value = -max();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -161,9 +171,13 @@ private:
|
||||
if (!IsExactRepresentation && isfinite(value))
|
||||
{
|
||||
if (value > max())
|
||||
{
|
||||
value = std::numeric_limits<StorageType>::infinity();
|
||||
}
|
||||
else if (value < -max())
|
||||
{
|
||||
value = -std::numeric_limits<StorageType>::infinity();
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
@@ -64,9 +64,13 @@ private:
|
||||
static void saturate(StorageType& value)
|
||||
{
|
||||
if (value > max())
|
||||
{
|
||||
value = max();
|
||||
}
|
||||
else if (value <= min()) // 'Less or Equal' allows to suppress compiler warning on unsigned types
|
||||
{
|
||||
value = min();
|
||||
}
|
||||
}
|
||||
|
||||
static void truncate(StorageType& value) { value &= mask(); }
|
||||
@@ -88,9 +92,13 @@ public:
|
||||
validate();
|
||||
// cppcheck-suppress duplicateExpression
|
||||
if (CastMode == CastModeSaturate)
|
||||
{
|
||||
saturate(value);
|
||||
}
|
||||
else
|
||||
{
|
||||
truncate(value);
|
||||
}
|
||||
return codec.encode<BitLen>(value);
|
||||
}
|
||||
|
||||
@@ -111,10 +119,16 @@ class IntegerSpec<0, Signedness, CastMode>; // Invalid instantiation
|
||||
|
||||
|
||||
template <typename T>
|
||||
struct IsIntegerSpec { enum { Result = 0 }; };
|
||||
struct IsIntegerSpec
|
||||
{
|
||||
enum { Result = 0 };
|
||||
};
|
||||
|
||||
template <unsigned int BitLen, Signedness Signedness, CastMode CastMode>
|
||||
struct IsIntegerSpec<IntegerSpec<BitLen, Signedness, CastMode> > { enum { Result = 1 }; };
|
||||
struct IsIntegerSpec<IntegerSpec<BitLen, Signedness, CastMode> >
|
||||
{
|
||||
enum { Result = 1 };
|
||||
};
|
||||
|
||||
|
||||
template <unsigned int BitLen, Signedness Signedness, CastMode CastMode>
|
||||
@@ -128,7 +142,7 @@ struct YamlStreamer<IntegerSpec<BitLen, Signedness, CastMode> >
|
||||
{
|
||||
// Get rid of character types - we want its integer representation, not ASCII code
|
||||
typedef typename Select<(sizeof(StorageType) >= sizeof(int)), StorageType,
|
||||
typename Select<RawType::IsSigned, int, unsigned int>::Result >::Result TempType;
|
||||
typename Select<RawType::IsSigned, int, unsigned int>::Result >::Result TempType;
|
||||
s << TempType(value);
|
||||
}
|
||||
};
|
||||
|
||||
@@ -45,9 +45,10 @@ class ScalarCodec
|
||||
* It is likely to be OK anyway, so feel free to remove this assert() as needed.
|
||||
*/
|
||||
assert(big_endian == false);
|
||||
|
||||
if (big_endian)
|
||||
{
|
||||
swapByteOrder(bytes);
|
||||
}
|
||||
}
|
||||
|
||||
template <int BitLen, int Size>
|
||||
@@ -62,9 +63,10 @@ class ScalarCodec
|
||||
fixTwosComplement(T& value)
|
||||
{
|
||||
StaticAssert<std::numeric_limits<T>::is_integer>::check(); // Not applicable to floating point types
|
||||
|
||||
if (value & (T(1) << (BitLen - 1))) // The most significant bit is set --> negative
|
||||
{
|
||||
value |= 0xFFFFFFFFFFFFFFFF & ~((T(1) << BitLen) - 1);
|
||||
}
|
||||
}
|
||||
|
||||
template <int BitLen, typename T>
|
||||
@@ -98,7 +100,7 @@ class ScalarCodec
|
||||
|
||||
public:
|
||||
ScalarCodec(BitStream& stream)
|
||||
: stream_(stream)
|
||||
: stream_(stream)
|
||||
{ }
|
||||
|
||||
template <int BitLen, typename T>
|
||||
@@ -111,14 +113,13 @@ public:
|
||||
uint8_t bytes[sizeof(T)];
|
||||
} byte_union;
|
||||
byte_union.value = value;
|
||||
|
||||
clearExtraBits<BitLen>(byte_union.value);
|
||||
convertByteOrder<BitLen>(byte_union.bytes);
|
||||
|
||||
// Underlying stream class assumes that more significant bits have lower index, so we need to shift some.
|
||||
if (BitLen % 8)
|
||||
{
|
||||
byte_union.bytes[BitLen / 8] <<= (8 - (BitLen % 8)) & 7;
|
||||
|
||||
}
|
||||
return stream_.write(byte_union.bytes, BitLen);
|
||||
}
|
||||
|
||||
@@ -135,14 +136,15 @@ public:
|
||||
|
||||
const int read_res = stream_.read(byte_union.bytes, BitLen);
|
||||
if (read_res <= 0)
|
||||
{
|
||||
return read_res;
|
||||
|
||||
}
|
||||
if (BitLen % 8)
|
||||
{
|
||||
byte_union.bytes[BitLen / 8] >>= (8 - (BitLen % 8)) & 7; // As in encode(), vice versa
|
||||
|
||||
}
|
||||
convertByteOrder<BitLen>(byte_union.bytes);
|
||||
fixTwosComplement<BitLen>(byte_union.value);
|
||||
|
||||
value = byte_union.value;
|
||||
return read_res;
|
||||
}
|
||||
|
||||
@@ -15,17 +15,29 @@ enum TailArrayOptimizationMode { TailArrayOptDisabled, TailArrayOptEnabled };
|
||||
|
||||
|
||||
template <unsigned long Num>
|
||||
struct IntegerBitLen { enum { Result = 1 + IntegerBitLen<(Num >> 1)>::Result }; };
|
||||
struct IntegerBitLen
|
||||
{
|
||||
enum { Result = 1 + IntegerBitLen<(Num >> 1)>::Result };
|
||||
};
|
||||
template <>
|
||||
struct IntegerBitLen<0> { enum { Result = 0 }; };
|
||||
struct IntegerBitLen<0>
|
||||
{
|
||||
enum { Result = 0 };
|
||||
};
|
||||
|
||||
|
||||
template <unsigned long BitLen>
|
||||
struct BitLenToByteLen { enum { Result = (BitLen + 7) / 8 }; };
|
||||
struct BitLenToByteLen
|
||||
{
|
||||
enum { Result = (BitLen + 7) / 8 };
|
||||
};
|
||||
|
||||
|
||||
template <typename T, typename Enable = void>
|
||||
struct StorageType { typedef T Type; };
|
||||
struct StorageType
|
||||
{
|
||||
typedef T Type;
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
struct StorageType<T, typename EnableIfType<typename T::StorageType>::Type>
|
||||
|
||||
@@ -24,8 +24,11 @@ namespace uavcan
|
||||
template <typename DataSpec, typename DataStruct>
|
||||
class GenericPublisher
|
||||
{
|
||||
enum { Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ?
|
||||
CanTxQueue::Volatile : CanTxQueue::Persistent };
|
||||
enum
|
||||
{
|
||||
Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ?
|
||||
CanTxQueue::Volatile : CanTxQueue::Persistent
|
||||
};
|
||||
|
||||
const MonotonicDuration max_transfer_interval_; // TODO: memory usage can be reduced
|
||||
MonotonicDuration tx_timeout_;
|
||||
@@ -35,7 +38,9 @@ class GenericPublisher
|
||||
bool checkInit()
|
||||
{
|
||||
if (sender_)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
GlobalDataTypeRegistry::instance().freeze();
|
||||
|
||||
@@ -63,11 +68,15 @@ class GenericPublisher
|
||||
TransferID* tid, MonotonicTime blocking_deadline)
|
||||
{
|
||||
if (!checkInit())
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
IMarshalBuffer* const buf = getBuffer();
|
||||
if (!buf)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
{
|
||||
BitStream bitstream(*buf);
|
||||
@@ -94,9 +103,9 @@ class GenericPublisher
|
||||
public:
|
||||
GenericPublisher(INode& node, MonotonicDuration tx_timeout,
|
||||
MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval())
|
||||
: max_transfer_interval_(max_transfer_interval)
|
||||
, tx_timeout_(tx_timeout)
|
||||
, node_(node)
|
||||
: max_transfer_interval_(max_transfer_interval)
|
||||
, tx_timeout_(tx_timeout)
|
||||
, node_(node)
|
||||
{
|
||||
setTxTimeout(tx_timeout);
|
||||
#if UAVCAN_DEBUG
|
||||
|
||||
@@ -22,7 +22,7 @@ class ReceivedDataStructure : public DataType_
|
||||
{
|
||||
const IncomingTransfer* transfer_;
|
||||
|
||||
template <typename Ret, Ret (IncomingTransfer::*Fun)() const>
|
||||
template <typename Ret, Ret(IncomingTransfer::*Fun) () const>
|
||||
Ret safeget() const
|
||||
{
|
||||
if (!transfer_)
|
||||
@@ -60,8 +60,8 @@ template <typename Stream, typename DataType>
|
||||
static Stream& operator<<(Stream& s, const ReceivedDataStructure<DataType>& rds)
|
||||
{
|
||||
s << "# Received struct ts_m=" << rds.getMonotonicTimestamp()
|
||||
<< " ts_utc=" << rds.getUtcTimestamp()
|
||||
<< " snid=" << int(rds.getSrcNodeID().get()) << "\n";
|
||||
<< " ts_utc=" << rds.getUtcTimestamp()
|
||||
<< " snid=" << int(rds.getSrcNodeID().get()) << "\n";
|
||||
s << static_cast<const DataType&>(rds);
|
||||
return s;
|
||||
}
|
||||
@@ -98,8 +98,8 @@ class GenericSubscriber : Noncopyable
|
||||
|
||||
public:
|
||||
TransferForwarder(SelfType& obj, const DataTypeDescriptor& data_type, IAllocator& allocator)
|
||||
: TransferListenerType(data_type, allocator)
|
||||
, obj_(obj)
|
||||
: TransferListenerType(data_type, allocator)
|
||||
, obj_(obj)
|
||||
{ }
|
||||
};
|
||||
|
||||
@@ -116,7 +116,9 @@ class GenericSubscriber : Noncopyable
|
||||
bool checkInit()
|
||||
{
|
||||
if (forwarder_)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
GlobalDataTypeRegistry::instance().freeze();
|
||||
|
||||
@@ -161,7 +163,7 @@ class GenericSubscriber : Noncopyable
|
||||
}
|
||||
}
|
||||
|
||||
int genericStart(bool(Dispatcher::*registration_method)(TransferListenerBase* listener))
|
||||
int genericStart(bool (Dispatcher::*registration_method)(TransferListenerBase* listener))
|
||||
{
|
||||
stop();
|
||||
|
||||
@@ -182,8 +184,8 @@ class GenericSubscriber : Noncopyable
|
||||
|
||||
protected:
|
||||
explicit GenericSubscriber(INode& node)
|
||||
: node_(node)
|
||||
, failure_count_(0)
|
||||
: node_(node)
|
||||
, failure_count_(0)
|
||||
{ }
|
||||
|
||||
virtual ~GenericSubscriber() { stop(); }
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
#include <uavcan/fatal_error.hpp>
|
||||
#include <uavcan/linked_list.hpp>
|
||||
#if UAVCAN_DEBUG
|
||||
#include <uavcan/debug.hpp>
|
||||
# include <uavcan/debug.hpp>
|
||||
#endif
|
||||
|
||||
namespace uavcan
|
||||
@@ -30,7 +30,7 @@ class GlobalDataTypeRegistry : Noncopyable
|
||||
Entry() { }
|
||||
|
||||
Entry(DataTypeKind kind, DataTypeID id, const DataTypeSignature& signature, const char* name)
|
||||
: descriptor(kind, id, signature, name)
|
||||
: descriptor(kind, id, signature, name)
|
||||
{ }
|
||||
};
|
||||
|
||||
@@ -75,19 +75,25 @@ public:
|
||||
RegistResult regist(DataTypeID id)
|
||||
{
|
||||
if (isFrozen())
|
||||
{
|
||||
return RegistResultFrozen;
|
||||
}
|
||||
|
||||
static Entry entry;
|
||||
{
|
||||
const RegistResult remove_res = remove(&entry);
|
||||
if (remove_res != RegistResultOk)
|
||||
{
|
||||
return remove_res;
|
||||
}
|
||||
}
|
||||
entry = Entry(DataTypeKind(Type::DataTypeKind), id, Type::getDataTypeSignature(), Type::getDataTypeFullName());
|
||||
{
|
||||
const RegistResult remove_res = remove(&entry);
|
||||
if (remove_res != RegistResultOk)
|
||||
{
|
||||
return remove_res;
|
||||
}
|
||||
}
|
||||
return registImpl(&entry);
|
||||
}
|
||||
@@ -114,9 +120,13 @@ public:
|
||||
int(frozen_), getNumMessageTypes(), getNumServiceTypes());
|
||||
frozen_ = false;
|
||||
while (msgs_.get())
|
||||
{
|
||||
msgs_.remove(msgs_.get());
|
||||
}
|
||||
while (srvs_.get())
|
||||
{
|
||||
srvs_.remove(srvs_.get());
|
||||
}
|
||||
}
|
||||
#endif
|
||||
};
|
||||
@@ -131,7 +141,9 @@ struct DefaultDataTypeRegistrator
|
||||
GlobalDataTypeRegistry::instance().regist<Type>(Type::DefaultDataTypeID);
|
||||
|
||||
if (res != GlobalDataTypeRegistry::RegistResultOk)
|
||||
{
|
||||
handleFatalError("Type registration failed");
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -59,7 +59,9 @@ public:
|
||||
IMarshalBuffer* getBuffer(unsigned int size)
|
||||
{
|
||||
if (size > MaxSize)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
buffer_.reset();
|
||||
return &buffer_;
|
||||
}
|
||||
|
||||
@@ -19,7 +19,7 @@ public:
|
||||
|
||||
explicit Publisher(INode& node, MonotonicDuration tx_timeout = getDefaultTxTimeout(),
|
||||
MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval())
|
||||
: BaseType(node, tx_timeout, max_transfer_interval)
|
||||
: BaseType(node, tx_timeout, max_transfer_interval)
|
||||
{
|
||||
#if UAVCAN_DEBUG
|
||||
assert(getTxTimeout() == tx_timeout); // Making sure default values are OK
|
||||
|
||||
@@ -20,7 +20,7 @@ protected:
|
||||
Scheduler& scheduler_;
|
||||
|
||||
explicit DeadlineHandler(Scheduler& scheduler)
|
||||
: scheduler_(scheduler)
|
||||
: scheduler_(scheduler)
|
||||
{ }
|
||||
|
||||
virtual ~DeadlineHandler() { stop(); }
|
||||
@@ -76,10 +76,10 @@ class Scheduler : Noncopyable
|
||||
|
||||
public:
|
||||
Scheduler(ICanDriver& can_driver, IAllocator& allocator, ISystemClock& sysclock, IOutgoingTransferRegistry& otr)
|
||||
: dispatcher_(can_driver, allocator, sysclock, otr)
|
||||
, prev_cleanup_ts_(sysclock.getMonotonic())
|
||||
, deadline_resolution_(MonotonicDuration::fromMSec(DefaultDeadlineResolutionMs))
|
||||
, cleanup_period_(MonotonicDuration::fromMSec(DefaultCleanupPeriodMs))
|
||||
: dispatcher_(can_driver, allocator, sysclock, otr)
|
||||
, prev_cleanup_ts_(sysclock.getMonotonic())
|
||||
, deadline_resolution_(MonotonicDuration::fromMSec(DefaultDeadlineResolutionMs))
|
||||
, cleanup_period_(MonotonicDuration::fromMSec(DefaultCleanupPeriodMs))
|
||||
{ }
|
||||
|
||||
int spin(MonotonicTime deadline);
|
||||
|
||||
@@ -29,9 +29,9 @@ struct ServiceCallResult
|
||||
ResponseFieldType& response; ///< Either response contents or unspecified response structure
|
||||
|
||||
ServiceCallResult(Status status, NodeID server_node_id, ResponseFieldType& response)
|
||||
: status(status)
|
||||
, server_node_id(server_node_id)
|
||||
, response(response)
|
||||
: status(status)
|
||||
, server_node_id(server_node_id)
|
||||
, response(response)
|
||||
{
|
||||
assert(server_node_id.isUnicast());
|
||||
assert(status == Success || status == ErrorTimeout);
|
||||
@@ -44,21 +44,25 @@ template <typename Stream, typename DataType>
|
||||
static Stream& operator<<(Stream& s, const ServiceCallResult<DataType>& scr)
|
||||
{
|
||||
s << "# Service call result [" << DataType::getDataTypeFullName() << "] "
|
||||
<< (scr.isSuccessful() ? "OK" : "FAILURE")
|
||||
<< " server_node_id=" << int(scr.server_node_id.get()) << "\n";
|
||||
<< (scr.isSuccessful() ? "OK" : "FAILURE")
|
||||
<< " server_node_id=" << int(scr.server_node_id.get()) << "\n";
|
||||
if (scr.isSuccessful())
|
||||
{
|
||||
s << scr.response;
|
||||
}
|
||||
else
|
||||
{
|
||||
s << "# (no data)";
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
||||
|
||||
template <typename DataType_, typename Callback = void(*)(const ServiceCallResult<DataType_>&)>
|
||||
class ServiceClient :
|
||||
public GenericSubscriber<DataType_, typename DataType_::Response,
|
||||
typename ServiceResponseTransferListenerInstantiationHelper<DataType_>::Type >,
|
||||
public DeadlineHandler
|
||||
template <typename DataType_, typename Callback = void (*)(const ServiceCallResult<DataType_>&)>
|
||||
class ServiceClient
|
||||
: public GenericSubscriber<DataType_, typename DataType_::Response,
|
||||
typename ServiceResponseTransferListenerInstantiationHelper<DataType_>::Type >
|
||||
, public DeadlineHandler
|
||||
{
|
||||
public:
|
||||
typedef DataType_ DataType;
|
||||
@@ -82,9 +86,13 @@ private:
|
||||
void invokeCallback(ServiceCallResultType& result)
|
||||
{
|
||||
if (isCallbackValid())
|
||||
{
|
||||
callback_(result);
|
||||
}
|
||||
else
|
||||
{
|
||||
handleFatalError("Invalid caller callback");
|
||||
}
|
||||
}
|
||||
|
||||
void handleReceivedDataStruct(ReceivedDataStructure<ResponseType>& response)
|
||||
@@ -128,12 +136,12 @@ private:
|
||||
|
||||
public:
|
||||
explicit ServiceClient(INode& node, const Callback& callback = Callback())
|
||||
: SubscriberType(node)
|
||||
, DeadlineHandler(node.getScheduler())
|
||||
, publisher_(node, getDefaultRequestTimeout())
|
||||
, callback_(callback)
|
||||
, request_timeout_(getDefaultRequestTimeout())
|
||||
, pending_(false)
|
||||
: SubscriberType(node)
|
||||
, DeadlineHandler(node.getScheduler())
|
||||
, publisher_(node, getDefaultRequestTimeout())
|
||||
, callback_(callback)
|
||||
, request_timeout_(getDefaultRequestTimeout())
|
||||
, pending_(false)
|
||||
{
|
||||
setRequestTimeout(getDefaultRequestTimeout());
|
||||
#if UAVCAN_DEBUG
|
||||
@@ -181,7 +189,7 @@ public:
|
||||
const MonotonicTime otr_deadline =
|
||||
SubscriberType::getNode().getMonotonicTime() + TransferSender::getDefaultMaxTransferInterval();
|
||||
TransferID* const otr_tid = SubscriberType::getNode().getDispatcher().getOutgoingTransferRegistry()
|
||||
.accessOrCreate(otr_key, otr_deadline);
|
||||
.accessOrCreate(otr_key, otr_deadline);
|
||||
if (!otr_tid)
|
||||
{
|
||||
UAVCAN_TRACE("ServiceClient", "OTR access failure, dtd=%s", descr->toString().c_str());
|
||||
@@ -238,7 +246,9 @@ public:
|
||||
DeadlineHandler::stop();
|
||||
TransferListenerType* const tl = SubscriberType::getTransferListener();
|
||||
if (tl)
|
||||
{
|
||||
tl->stopAcceptingAnything();
|
||||
}
|
||||
}
|
||||
|
||||
bool isPending() const { return pending_; }
|
||||
|
||||
@@ -11,8 +11,8 @@ namespace uavcan
|
||||
{
|
||||
|
||||
template <typename DataType_,
|
||||
typename Callback = void(*)(const ReceivedDataStructure<typename DataType_::Request>&,
|
||||
typename DataType_::Response&),
|
||||
typename Callback = void (*)(const ReceivedDataStructure<typename DataType_::Request>&,
|
||||
typename DataType_::Response&),
|
||||
unsigned int NumStaticReceivers = 2,
|
||||
unsigned int NumStaticBufs = 1>
|
||||
class ServiceServer : public GenericSubscriber<DataType_, typename DataType_::Request,
|
||||
@@ -45,10 +45,12 @@ private:
|
||||
callback_(request, response_);
|
||||
}
|
||||
else
|
||||
{
|
||||
handleFatalError("Invalid service server callback");
|
||||
}
|
||||
|
||||
const int res = publisher_.publish(response_, TransferTypeServiceResponse, request.getSrcNodeID(),
|
||||
request.getTransferID());
|
||||
request.getTransferID());
|
||||
if (res < 0)
|
||||
{
|
||||
UAVCAN_TRACE("ServiceServer", "Response publication failure: %i", res);
|
||||
@@ -58,10 +60,10 @@ private:
|
||||
|
||||
public:
|
||||
explicit ServiceServer(INode& node)
|
||||
: SubscriberType(node)
|
||||
, publisher_(node, getDefaultTxTimeout())
|
||||
, callback_()
|
||||
, response_failure_count_(0)
|
||||
: SubscriberType(node)
|
||||
, publisher_(node, getDefaultTxTimeout())
|
||||
, callback_()
|
||||
, response_failure_count_(0)
|
||||
{
|
||||
assert(getTxTimeout() == getDefaultTxTimeout()); // Making sure it is valid
|
||||
|
||||
|
||||
@@ -11,7 +11,7 @@ namespace uavcan
|
||||
{
|
||||
|
||||
template <typename DataType_,
|
||||
typename Callback = void(*)(const ReceivedDataStructure<DataType_>&),
|
||||
typename Callback = void (*)(const ReceivedDataStructure<DataType_>&),
|
||||
unsigned int NumStaticReceivers = 2,
|
||||
unsigned int NumStaticBufs = 1>
|
||||
class Subscriber : public GenericSubscriber<DataType_, DataType_,
|
||||
@@ -28,17 +28,21 @@ class Subscriber : public GenericSubscriber<DataType_, DataType_,
|
||||
void handleReceivedDataStruct(ReceivedDataStructure<DataType_>& msg)
|
||||
{
|
||||
if (try_implicit_cast<bool>(callback_, true))
|
||||
{
|
||||
callback_(msg);
|
||||
}
|
||||
else
|
||||
{
|
||||
handleFatalError("Invalid subscriber callback");
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
typedef DataType_ DataType;
|
||||
|
||||
explicit Subscriber(INode& node)
|
||||
: BaseType(node)
|
||||
, callback_()
|
||||
: BaseType(node)
|
||||
, callback_()
|
||||
{
|
||||
StaticAssert<DataTypeKind(DataType::DataTypeKind) == DataTypeKindMessage>::check();
|
||||
}
|
||||
|
||||
@@ -22,8 +22,8 @@ struct TimerEvent
|
||||
MonotonicTime real_time;
|
||||
|
||||
TimerEvent(MonotonicTime scheduled_time, MonotonicTime real_time)
|
||||
: scheduled_time(scheduled_time)
|
||||
, real_time(real_time)
|
||||
: scheduled_time(scheduled_time)
|
||||
, real_time(real_time)
|
||||
{ }
|
||||
};
|
||||
|
||||
@@ -41,8 +41,8 @@ public:
|
||||
using DeadlineHandler::getScheduler;
|
||||
|
||||
explicit Timer(INode& node)
|
||||
: DeadlineHandler(node.getScheduler())
|
||||
, period_(MonotonicDuration::getInfinite())
|
||||
: DeadlineHandler(node.getScheduler())
|
||||
, period_(MonotonicDuration::getInfinite())
|
||||
{ }
|
||||
|
||||
void startOneShotWithDeadline(MonotonicTime deadline);
|
||||
@@ -63,20 +63,24 @@ class TimerEventForwarder : public Timer
|
||||
void handleTimerEvent(const TimerEvent& event)
|
||||
{
|
||||
if (try_implicit_cast<bool>(callback_, true))
|
||||
{
|
||||
callback_(event);
|
||||
}
|
||||
else
|
||||
{
|
||||
handleFatalError("Invalid timer callback");
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
explicit TimerEventForwarder(INode& node)
|
||||
: Timer(node)
|
||||
, callback_()
|
||||
: Timer(node)
|
||||
, callback_()
|
||||
{ }
|
||||
|
||||
TimerEventForwarder(INode& node, Callback callback)
|
||||
: Timer(node)
|
||||
, callback_(callback)
|
||||
: Timer(node)
|
||||
, callback_(callback)
|
||||
{ }
|
||||
|
||||
const Callback& getCallback() const { return callback_; }
|
||||
|
||||
@@ -16,13 +16,13 @@ namespace uavcan
|
||||
class DataTypeInfoProvider : Noncopyable
|
||||
{
|
||||
typedef MethodBinder<DataTypeInfoProvider*,
|
||||
void (DataTypeInfoProvider::*)(const protocol::ComputeAggregateTypeSignature::Request&,
|
||||
protocol::ComputeAggregateTypeSignature::Response&)>
|
||||
void (DataTypeInfoProvider::*)(const protocol::ComputeAggregateTypeSignature::Request&,
|
||||
protocol::ComputeAggregateTypeSignature::Response&)>
|
||||
ComputeAggregateTypeSignatureCallback;
|
||||
|
||||
typedef MethodBinder<DataTypeInfoProvider*,
|
||||
void (DataTypeInfoProvider::*)(const protocol::GetDataTypeInfo::Request&,
|
||||
protocol::GetDataTypeInfo::Response&)> GetDataTypeInfoCallback;
|
||||
void (DataTypeInfoProvider::*)(const protocol::GetDataTypeInfo::Request&,
|
||||
protocol::GetDataTypeInfo::Response&)> GetDataTypeInfoCallback;
|
||||
|
||||
ServiceServer<protocol::ComputeAggregateTypeSignature, ComputeAggregateTypeSignatureCallback> cats_srv_;
|
||||
ServiceServer<protocol::GetDataTypeInfo, GetDataTypeInfoCallback> gdti_srv_;
|
||||
@@ -39,8 +39,8 @@ class DataTypeInfoProvider : Noncopyable
|
||||
|
||||
public:
|
||||
DataTypeInfoProvider(INode& node)
|
||||
: cats_srv_(node)
|
||||
, gdti_srv_(node)
|
||||
: cats_srv_(node)
|
||||
, gdti_srv_(node)
|
||||
{ }
|
||||
|
||||
int start();
|
||||
|
||||
@@ -26,8 +26,8 @@ class GlobalTimeSyncMaster : protected LoopbackFrameListenerBase
|
||||
|
||||
public:
|
||||
IfaceMaster(INode& node, uint8_t iface_index)
|
||||
: pub_(node)
|
||||
, iface_index_(iface_index)
|
||||
: pub_(node)
|
||||
, iface_index_(iface_index)
|
||||
{
|
||||
assert(iface_index < MaxCanIfaces);
|
||||
}
|
||||
@@ -48,9 +48,9 @@ class GlobalTimeSyncMaster : protected LoopbackFrameListenerBase
|
||||
|
||||
public:
|
||||
GlobalTimeSyncMaster(INode& node)
|
||||
: LoopbackFrameListenerBase(node.getDispatcher())
|
||||
, node_(node)
|
||||
, initialized_(false)
|
||||
: LoopbackFrameListenerBase(node.getDispatcher())
|
||||
, node_(node)
|
||||
, initialized_(false)
|
||||
{ }
|
||||
|
||||
int init();
|
||||
|
||||
@@ -14,7 +14,8 @@ namespace uavcan
|
||||
class GlobalTimeSyncSlave : Noncopyable
|
||||
{
|
||||
typedef MethodBinder<GlobalTimeSyncSlave*,
|
||||
void (GlobalTimeSyncSlave::*)(const ReceivedDataStructure<protocol::GlobalTimeSync>&)> GlobalTimeSyncCallback;
|
||||
void (GlobalTimeSyncSlave::*)(const ReceivedDataStructure<protocol::GlobalTimeSync>&)>
|
||||
GlobalTimeSyncCallback;
|
||||
|
||||
// Static buffers are explicitly disabled because time should never be unicasted.
|
||||
Subscriber<protocol::GlobalTimeSync, GlobalTimeSyncCallback, 2, 0> sub_;
|
||||
@@ -39,9 +40,9 @@ class GlobalTimeSyncSlave : Noncopyable
|
||||
|
||||
public:
|
||||
GlobalTimeSyncSlave(INode& node)
|
||||
: sub_(node)
|
||||
, state_(Update)
|
||||
, prev_iface_index_(0xFF)
|
||||
: sub_(node)
|
||||
, state_(Update)
|
||||
, prev_iface_index_(0xFF)
|
||||
{ }
|
||||
|
||||
int start();
|
||||
|
||||
@@ -41,8 +41,8 @@ private:
|
||||
|
||||
public:
|
||||
Logger(INode& node)
|
||||
: logmsg_pub_(node)
|
||||
, external_sink_(NULL)
|
||||
: logmsg_pub_(node)
|
||||
, external_sink_(NULL)
|
||||
{
|
||||
level_ = protocol::debug::LogLevel::ERROR;
|
||||
setTxTimeout(MonotonicDuration::fromMSec(DefaultTxTimeoutMs));
|
||||
|
||||
@@ -19,11 +19,11 @@ namespace uavcan
|
||||
|
||||
class NodeStatusProvider : private Timer
|
||||
{
|
||||
typedef MethodBinder<NodeStatusProvider*, void(NodeStatusProvider::*)(const protocol::GlobalDiscoveryRequest&)>
|
||||
typedef MethodBinder<NodeStatusProvider*, void (NodeStatusProvider::*)(const protocol::GlobalDiscoveryRequest&)>
|
||||
GlobalDiscoveryRequestCallback;
|
||||
typedef MethodBinder<NodeStatusProvider*,
|
||||
void(NodeStatusProvider::*)(const protocol::GetNodeInfo::Request&,
|
||||
protocol::GetNodeInfo::Response&)> GetNodeInfoCallback;
|
||||
void (NodeStatusProvider::*)(const protocol::GetNodeInfo::Request&,
|
||||
protocol::GetNodeInfo::Response&)> GetNodeInfoCallback;
|
||||
|
||||
const MonotonicTime creation_timestamp_;
|
||||
|
||||
@@ -46,11 +46,11 @@ class NodeStatusProvider : private Timer
|
||||
|
||||
public:
|
||||
NodeStatusProvider(INode& node)
|
||||
: Timer(node)
|
||||
, creation_timestamp_(node.getMonotonicTime())
|
||||
, node_status_pub_(node)
|
||||
, gdr_sub_(node)
|
||||
, gni_srv_(node)
|
||||
: Timer(node)
|
||||
, creation_timestamp_(node.getMonotonicTime())
|
||||
, node_status_pub_(node)
|
||||
, gdr_sub_(node)
|
||||
, gni_srv_(node)
|
||||
{
|
||||
assert(!creation_timestamp_.isZero());
|
||||
|
||||
|
||||
@@ -22,8 +22,8 @@ class PanicBroadcaster : private Timer
|
||||
|
||||
public:
|
||||
PanicBroadcaster(INode& node)
|
||||
: Timer(node)
|
||||
, pub_(node)
|
||||
: Timer(node)
|
||||
, pub_(node)
|
||||
{
|
||||
pub_.setTxTimeout(MonotonicDuration::fromMSec(protocol::Panic::BROADCASTING_INTERVAL_MS - 10));
|
||||
}
|
||||
|
||||
@@ -4,6 +4,8 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <uavcan/debug.hpp>
|
||||
#include <uavcan/node/subscriber.hpp>
|
||||
#include <uavcan/util/method_binder.hpp>
|
||||
#include <uavcan/protocol/Panic.hpp>
|
||||
@@ -17,10 +19,10 @@ namespace uavcan
|
||||
* void (const protocol::Panic&)
|
||||
* The listener can be stopped from the callback.
|
||||
*/
|
||||
template <typename Callback = void(*)(const ReceivedDataStructure<protocol::Panic>&)>
|
||||
template <typename Callback = void (*)(const ReceivedDataStructure<protocol::Panic>&)>
|
||||
class PanicListener : Noncopyable
|
||||
{
|
||||
typedef MethodBinder<PanicListener*, void(PanicListener::*)(const ReceivedDataStructure<protocol::Panic>&)>
|
||||
typedef MethodBinder<PanicListener*, void (PanicListener::*)(const ReceivedDataStructure<protocol::Panic>&)>
|
||||
PanicMsgCallback;
|
||||
|
||||
Subscriber<protocol::Panic, PanicMsgCallback> sub_;
|
||||
@@ -73,9 +75,9 @@ class PanicListener : Noncopyable
|
||||
|
||||
public:
|
||||
PanicListener(INode& node)
|
||||
: sub_(node)
|
||||
, callback_()
|
||||
, num_subsequent_msgs_(0)
|
||||
: sub_(node)
|
||||
, callback_()
|
||||
, num_subsequent_msgs_(0)
|
||||
{ }
|
||||
|
||||
int start(const Callback& callback)
|
||||
|
||||
@@ -22,8 +22,8 @@ public:
|
||||
class RestartRequestServer : Noncopyable
|
||||
{
|
||||
typedef MethodBinder<const RestartRequestServer*,
|
||||
void(RestartRequestServer::*)(const ReceivedDataStructure<protocol::RestartNode::Request>&,
|
||||
protocol::RestartNode::Response&) const> RestartNodeCallback;
|
||||
void (RestartRequestServer::*)(const ReceivedDataStructure<protocol::RestartNode::Request>&,
|
||||
protocol::RestartNode::Response&) const> RestartNodeCallback;
|
||||
|
||||
ServiceServer<protocol::RestartNode, RestartNodeCallback> srv_;
|
||||
IRestartRequestHandler* handler_;
|
||||
@@ -33,8 +33,8 @@ class RestartRequestServer : Noncopyable
|
||||
|
||||
public:
|
||||
RestartRequestServer(INode& node)
|
||||
: srv_(node)
|
||||
, handler_(NULL)
|
||||
: srv_(node)
|
||||
, handler_(NULL)
|
||||
{ }
|
||||
|
||||
IRestartRequestHandler* getHandler() const { return handler_; }
|
||||
|
||||
@@ -23,7 +23,7 @@ class DurationBase
|
||||
|
||||
public:
|
||||
DurationBase()
|
||||
: usec_(0)
|
||||
: usec_(0)
|
||||
{
|
||||
StaticAssert<(sizeof(D) == 8)>::check();
|
||||
}
|
||||
@@ -55,17 +55,17 @@ public:
|
||||
bool operator<=(const D& r) const { return usec_ <= r.usec_; }
|
||||
bool operator>=(const D& r) const { return usec_ >= r.usec_; }
|
||||
|
||||
D operator+(const D &r) const { return fromUSec(usec_ + r.usec_); } // TODO: overflow check
|
||||
D operator-(const D &r) const { return fromUSec(usec_ - r.usec_); } // ditto
|
||||
D operator+(const D& r) const { return fromUSec(usec_ + r.usec_); } // TODO: overflow check
|
||||
D operator-(const D& r) const { return fromUSec(usec_ - r.usec_); } // ditto
|
||||
|
||||
D operator-() const { return fromUSec(-usec_); }
|
||||
|
||||
D& operator+=(const D &r)
|
||||
D& operator+=(const D& r)
|
||||
{
|
||||
*this = *this + r;
|
||||
return *static_cast<D*>(this);
|
||||
}
|
||||
D& operator-=(const D &r)
|
||||
D& operator-=(const D& r)
|
||||
{
|
||||
*this = *this - r;
|
||||
return *static_cast<D*>(this);
|
||||
@@ -92,7 +92,7 @@ class TimeBase
|
||||
|
||||
public:
|
||||
TimeBase()
|
||||
: usec_(0)
|
||||
: usec_(0)
|
||||
{
|
||||
StaticAssert<(sizeof(T) == 8)>::check();
|
||||
StaticAssert<(sizeof(D) == 8)>::check();
|
||||
@@ -126,12 +126,16 @@ public:
|
||||
if (r.isNegative())
|
||||
{
|
||||
if (uint64_t(r.getAbs().toUSec()) > usec_)
|
||||
{
|
||||
return fromUSec(0);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (uint64_t(usec_ + r.toUSec()) < usec_)
|
||||
{
|
||||
return fromUSec(std::numeric_limits<uint64_t>::max());
|
||||
}
|
||||
}
|
||||
return fromUSec(usec_ + r.toUSec());
|
||||
}
|
||||
@@ -202,7 +206,9 @@ inline Stream& operator<<(Stream& s, DurationBase<D> d)
|
||||
char buf[8];
|
||||
std::snprintf(buf, sizeof(buf), "%06lu", static_cast<unsigned long>(std::abs(d.toUSec() % 1000000L)));
|
||||
if (d.isNegative())
|
||||
{
|
||||
s << '-';
|
||||
}
|
||||
s << std::abs(d.toUSec() / 1000000L) << '.' << buf;
|
||||
return s;
|
||||
}
|
||||
|
||||
@@ -27,7 +27,7 @@ struct CanRxFrame : public CanFrame
|
||||
uint8_t iface_index;
|
||||
|
||||
CanRxFrame()
|
||||
: iface_index(0)
|
||||
: iface_index(0)
|
||||
{ }
|
||||
|
||||
std::string toString(StringRepresentation mode = StrTight) const;
|
||||
@@ -47,10 +47,10 @@ public:
|
||||
CanIOFlags flags;
|
||||
|
||||
Entry(const CanFrame& frame, MonotonicTime deadline, Qos qos, CanIOFlags flags)
|
||||
: deadline(deadline)
|
||||
, frame(frame)
|
||||
, qos(uint8_t(qos))
|
||||
, flags(flags)
|
||||
: deadline(deadline)
|
||||
, frame(frame)
|
||||
, qos(uint8_t(qos))
|
||||
, flags(flags)
|
||||
{
|
||||
assert(qos == Volatile || qos == Persistent);
|
||||
IsDynamicallyAllocatable<Entry>::check();
|
||||
@@ -90,15 +90,15 @@ private:
|
||||
|
||||
public:
|
||||
CanTxQueue()
|
||||
: allocator_(NULL)
|
||||
, sysclock_(NULL)
|
||||
, rejected_frames_cnt_(0)
|
||||
: allocator_(NULL)
|
||||
, sysclock_(NULL)
|
||||
, rejected_frames_cnt_(0)
|
||||
{ }
|
||||
|
||||
CanTxQueue(IAllocator* allocator, ISystemClock* sysclock)
|
||||
: allocator_(allocator)
|
||||
, sysclock_(sysclock)
|
||||
, rejected_frames_cnt_(0)
|
||||
: allocator_(allocator)
|
||||
, sysclock_(sysclock)
|
||||
, rejected_frames_cnt_(0)
|
||||
{ }
|
||||
|
||||
~CanTxQueue();
|
||||
@@ -133,8 +133,8 @@ class CanIOManager : Noncopyable
|
||||
|
||||
public:
|
||||
CanIOManager(ICanDriver& driver, IAllocator& allocator, ISystemClock& sysclock)
|
||||
: driver_(driver)
|
||||
, sysclock_(sysclock)
|
||||
: driver_(driver)
|
||||
, sysclock_(sysclock)
|
||||
{
|
||||
assert(driver.getNumIfaces() <= MaxCanIfaces);
|
||||
// We can't initialize member array with non-default constructors in C++03
|
||||
|
||||
@@ -32,7 +32,7 @@ public:
|
||||
enum { NumBytes = 2 };
|
||||
|
||||
TransferCRC()
|
||||
: value_(0xFFFF)
|
||||
: value_(0xFFFF)
|
||||
{ }
|
||||
|
||||
void add(uint8_t byte)
|
||||
@@ -44,7 +44,9 @@ public:
|
||||
{
|
||||
assert(bytes);
|
||||
while (len--)
|
||||
{
|
||||
add(*bytes++);
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t get() const { return value_; }
|
||||
|
||||
@@ -22,7 +22,7 @@ class LoopbackFrameListenerBase : public LinkedListNode<LoopbackFrameListenerBas
|
||||
|
||||
protected:
|
||||
LoopbackFrameListenerBase(Dispatcher& dispatcher)
|
||||
: dispatcher_(dispatcher)
|
||||
: dispatcher_(dispatcher)
|
||||
{ }
|
||||
|
||||
virtual ~LoopbackFrameListenerBase() { stopListening(); }
|
||||
@@ -99,9 +99,9 @@ class Dispatcher : Noncopyable
|
||||
|
||||
public:
|
||||
Dispatcher(ICanDriver& driver, IAllocator& allocator, ISystemClock& sysclock, IOutgoingTransferRegistry& otr)
|
||||
: canio_(driver, allocator, sysclock)
|
||||
, sysclock_(sysclock)
|
||||
, outgoing_transfer_reg_(otr)
|
||||
: canio_(driver, allocator, sysclock)
|
||||
, sysclock_(sysclock)
|
||||
, outgoing_transfer_reg_(otr)
|
||||
{ }
|
||||
|
||||
int spin(MonotonicTime deadline);
|
||||
|
||||
@@ -32,23 +32,23 @@ public:
|
||||
enum { MaxIndex = 62 }; // 63 (or 0b111111) is reserved
|
||||
|
||||
Frame()
|
||||
: transfer_type_(TransferType(NumTransferTypes)) // That is invalid value
|
||||
, payload_len_(0)
|
||||
, frame_index_(0)
|
||||
, transfer_id_(0)
|
||||
, last_frame_(false)
|
||||
: transfer_type_(TransferType(NumTransferTypes)) // That is invalid value
|
||||
, payload_len_(0)
|
||||
, frame_index_(0)
|
||||
, transfer_id_(0)
|
||||
, last_frame_(false)
|
||||
{ }
|
||||
|
||||
Frame(DataTypeID data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
|
||||
uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false)
|
||||
: transfer_type_(transfer_type)
|
||||
, data_type_id_(data_type_id)
|
||||
, payload_len_(0)
|
||||
, src_node_id_(src_node_id)
|
||||
, dst_node_id_(dst_node_id)
|
||||
, frame_index_(frame_index)
|
||||
, transfer_id_(transfer_id)
|
||||
, last_frame_(last_frame)
|
||||
: transfer_type_(transfer_type)
|
||||
, data_type_id_(data_type_id)
|
||||
, payload_len_(0)
|
||||
, src_node_id_(src_node_id)
|
||||
, dst_node_id_(dst_node_id)
|
||||
, frame_index_(frame_index)
|
||||
, transfer_id_(transfer_id)
|
||||
, last_frame_(last_frame)
|
||||
{
|
||||
assert((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast());
|
||||
assert(data_type_id.isValid());
|
||||
@@ -95,13 +95,13 @@ class RxFrame : public Frame
|
||||
|
||||
public:
|
||||
RxFrame()
|
||||
: iface_index_(0)
|
||||
: iface_index_(0)
|
||||
{ }
|
||||
|
||||
RxFrame(const Frame& frame, MonotonicTime ts_mono, UtcTime ts_utc, uint8_t iface_index)
|
||||
: ts_mono_(ts_mono)
|
||||
, ts_utc_(ts_utc)
|
||||
, iface_index_(iface_index)
|
||||
: ts_mono_(ts_mono)
|
||||
, ts_utc_(ts_utc)
|
||||
, iface_index_(iface_index)
|
||||
{
|
||||
*static_cast<Frame*>(this) = frame;
|
||||
}
|
||||
|
||||
@@ -25,13 +25,13 @@ class OutgoingTransferRegistryKey
|
||||
|
||||
public:
|
||||
OutgoingTransferRegistryKey()
|
||||
: transfer_type_(0xFF)
|
||||
: transfer_type_(0xFF)
|
||||
{ }
|
||||
|
||||
OutgoingTransferRegistryKey(DataTypeID data_type_id, TransferType transfer_type, NodeID destination_node_id)
|
||||
: data_type_id_(data_type_id)
|
||||
, transfer_type_(transfer_type)
|
||||
, destination_node_id_(destination_node_id)
|
||||
: data_type_id_(data_type_id)
|
||||
, transfer_type_(transfer_type)
|
||||
, destination_node_id_(destination_node_id)
|
||||
{
|
||||
assert((transfer_type == TransferTypeMessageBroadcast) == destination_node_id.isBroadcast());
|
||||
|
||||
@@ -56,8 +56,8 @@ public:
|
||||
{
|
||||
std::ostringstream os;
|
||||
os << "dtid=" << int(data_type_id_.get())
|
||||
<< " tt=" << int(transfer_type_)
|
||||
<< " dnid=" << int(destination_node_id_.get());
|
||||
<< " tt=" << int(transfer_type_)
|
||||
<< " dnid=" << int(destination_node_id_.get());
|
||||
return os.str();
|
||||
}
|
||||
};
|
||||
@@ -77,13 +77,13 @@ public:
|
||||
template <int NumStaticEntries>
|
||||
class OutgoingTransferRegistry : public IOutgoingTransferRegistry, Noncopyable
|
||||
{
|
||||
UAVCAN_PACKED_BEGIN
|
||||
UAVCAN_PACKED_BEGIN
|
||||
struct Value
|
||||
{
|
||||
MonotonicTime deadline;
|
||||
TransferID tid;
|
||||
};
|
||||
UAVCAN_PACKED_END
|
||||
UAVCAN_PACKED_END
|
||||
|
||||
class DeadlineExpiredPredicate
|
||||
{
|
||||
@@ -91,7 +91,7 @@ UAVCAN_PACKED_END
|
||||
|
||||
public:
|
||||
DeadlineExpiredPredicate(MonotonicTime ts)
|
||||
: ts_(ts)
|
||||
: ts_(ts)
|
||||
{ }
|
||||
|
||||
bool operator()(const OutgoingTransferRegistryKey& key, const Value& value) const
|
||||
@@ -115,8 +115,8 @@ UAVCAN_PACKED_END
|
||||
|
||||
public:
|
||||
ExistenceCheckingPredicate(DataTypeID dtid, TransferType tt)
|
||||
: dtid_(dtid)
|
||||
, tt_(tt)
|
||||
: dtid_(dtid)
|
||||
, tt_(tt)
|
||||
{ }
|
||||
|
||||
bool operator()(const OutgoingTransferRegistryKey& key, const Value&) const
|
||||
@@ -129,7 +129,7 @@ UAVCAN_PACKED_END
|
||||
|
||||
public:
|
||||
OutgoingTransferRegistry(IAllocator& allocator)
|
||||
: map_(allocator)
|
||||
: map_(allocator)
|
||||
{ }
|
||||
|
||||
TransferID* accessOrCreate(const OutgoingTransferRegistryKey& key, MonotonicTime new_deadline)
|
||||
@@ -140,7 +140,9 @@ public:
|
||||
{
|
||||
p = map_.insert(key, Value());
|
||||
if (p == NULL)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
UAVCAN_TRACE("OutgoingTransferRegistry", "Created %s", key.toString().c_str());
|
||||
}
|
||||
p->deadline = new_deadline;
|
||||
|
||||
@@ -33,11 +33,11 @@ public:
|
||||
enum { Max = (1 << BitLen) - 1 };
|
||||
|
||||
TransferID()
|
||||
: value_(0)
|
||||
: value_(0)
|
||||
{ }
|
||||
|
||||
TransferID(uint8_t value) // implicit
|
||||
: value_(value)
|
||||
: value_(value)
|
||||
{
|
||||
value_ &= Max;
|
||||
assert(value == value_);
|
||||
@@ -82,16 +82,16 @@ public:
|
||||
NodeID() : value_(ValueInvalid) { }
|
||||
|
||||
NodeID(uint8_t value)
|
||||
: value_(value)
|
||||
: value_(value)
|
||||
{
|
||||
assert(isValid());
|
||||
}
|
||||
|
||||
uint8_t get() const { return value_; }
|
||||
|
||||
bool isValid() const { return value_ <= Max; }
|
||||
bool isValid() const { return value_ <= Max; }
|
||||
bool isBroadcast() const { return value_ == ValueBroadcast; }
|
||||
bool isUnicast() const { return (value_ <= Max) && (value_ != ValueBroadcast); }
|
||||
bool isUnicast() const { return (value_ <= Max) && (value_ != ValueBroadcast); }
|
||||
|
||||
bool operator!=(NodeID rhs) const { return !operator==(rhs); }
|
||||
bool operator==(NodeID rhs) const { return value_ == rhs.value_; }
|
||||
|
||||
@@ -38,14 +38,14 @@ class TransferBufferManagerKey
|
||||
|
||||
public:
|
||||
TransferBufferManagerKey()
|
||||
: transfer_type_(TransferType(0))
|
||||
: transfer_type_(TransferType(0))
|
||||
{
|
||||
assert(isEmpty());
|
||||
}
|
||||
|
||||
TransferBufferManagerKey(NodeID node_id, TransferType ttype)
|
||||
: node_id_(node_id)
|
||||
, transfer_type_(ttype)
|
||||
: node_id_(node_id)
|
||||
, transfer_type_(ttype)
|
||||
{
|
||||
assert(!isEmpty());
|
||||
}
|
||||
@@ -77,7 +77,7 @@ public:
|
||||
TransferBufferManagerEntry() { }
|
||||
|
||||
TransferBufferManagerEntry(const TransferBufferManagerKey& key)
|
||||
: key_(key)
|
||||
: key_(key)
|
||||
{ }
|
||||
|
||||
const TransferBufferManagerKey& getKey() const { return key_; }
|
||||
@@ -95,8 +95,9 @@ public:
|
||||
* reset() call releases all memory blocks.
|
||||
* Supports unordered write operations - from higher to lower offsets
|
||||
*/
|
||||
class DynamicTransferBufferManagerEntry : public TransferBufferManagerEntry,
|
||||
public LinkedListNode<DynamicTransferBufferManagerEntry>
|
||||
class DynamicTransferBufferManagerEntry
|
||||
: public TransferBufferManagerEntry
|
||||
, public LinkedListNode<DynamicTransferBufferManagerEntry>
|
||||
{
|
||||
struct Block : LinkedListNode<Block>
|
||||
{
|
||||
@@ -121,9 +122,9 @@ class DynamicTransferBufferManagerEntry : public TransferBufferManagerEntry,
|
||||
|
||||
public:
|
||||
DynamicTransferBufferManagerEntry(IAllocator& allocator, unsigned int max_size)
|
||||
: allocator_(allocator)
|
||||
, max_write_pos_(0)
|
||||
, max_size_(max_size)
|
||||
: allocator_(allocator)
|
||||
, max_write_pos_(0)
|
||||
, max_size_(max_size)
|
||||
{
|
||||
StaticAssert<(Block::Size > 8)>::check();
|
||||
IsDynamicallyAllocatable<Block>::check();
|
||||
@@ -154,7 +155,7 @@ class StaticTransferBuffer : public ITransferBuffer
|
||||
|
||||
public:
|
||||
StaticTransferBuffer()
|
||||
: max_write_pos_(0)
|
||||
: max_write_pos_(0)
|
||||
{
|
||||
StaticAssert<(Size > 0)>::check();
|
||||
std::fill(data_, data_ + Size, 0);
|
||||
@@ -168,9 +169,13 @@ public:
|
||||
return -1;
|
||||
}
|
||||
if (offset >= max_write_pos_)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if ((offset + len) > max_write_pos_)
|
||||
{
|
||||
len = max_write_pos_ - offset;
|
||||
}
|
||||
assert((offset + len) <= max_write_pos_);
|
||||
std::copy(data_ + offset, data_ + offset + len, data);
|
||||
return len;
|
||||
@@ -184,9 +189,13 @@ public:
|
||||
return -1;
|
||||
}
|
||||
if (offset >= Size)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if ((offset + len) > Size)
|
||||
{
|
||||
len = Size - offset;
|
||||
}
|
||||
assert((offset + len) <= Size);
|
||||
std::copy(data, data + len, data_ + offset);
|
||||
max_write_pos_ = std::max(offset + len, max_write_pos_);
|
||||
@@ -250,7 +259,9 @@ public:
|
||||
}
|
||||
buf_.setMaxWritePos(res);
|
||||
if (res < int(Size))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
// Now we need to make sure that all data can fit the storage
|
||||
uint8_t dummy = 0;
|
||||
@@ -285,8 +296,8 @@ class TransferBufferAccessor
|
||||
|
||||
public:
|
||||
TransferBufferAccessor(ITransferBufferManager& bufmgr, TransferBufferManagerKey key)
|
||||
: bufmgr_(bufmgr)
|
||||
, key_(key)
|
||||
: bufmgr_(bufmgr)
|
||||
, key_(key)
|
||||
{
|
||||
assert(!key.isEmpty());
|
||||
}
|
||||
@@ -312,7 +323,9 @@ class TransferBufferManager : public ITransferBufferManager, Noncopyable
|
||||
for (unsigned int i = 0; i < NumStaticBufs; i++)
|
||||
{
|
||||
if (static_buffers_[i].getKey() == key)
|
||||
{
|
||||
return static_buffers_ + i;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
@@ -324,7 +337,9 @@ class TransferBufferManager : public ITransferBufferManager, Noncopyable
|
||||
{
|
||||
assert(!dyn->isEmpty());
|
||||
if (dyn->getKey() == key)
|
||||
{
|
||||
return dyn;
|
||||
}
|
||||
dyn = dyn->getNextListNode();
|
||||
}
|
||||
return NULL;
|
||||
@@ -336,7 +351,9 @@ class TransferBufferManager : public ITransferBufferManager, Noncopyable
|
||||
{
|
||||
StaticBufferType* const sb = findFirstStatic(TransferBufferManagerKey());
|
||||
if (sb == NULL)
|
||||
{
|
||||
break;
|
||||
}
|
||||
DynamicTransferBufferManagerEntry* dyn = dynamic_buffers_.get();
|
||||
assert(dyn);
|
||||
assert(!dyn->isEmpty());
|
||||
@@ -354,7 +371,7 @@ class TransferBufferManager : public ITransferBufferManager, Noncopyable
|
||||
* This should never happen during normal operation because dynamic buffers are limited in growth.
|
||||
*/
|
||||
UAVCAN_TRACE("TransferBufferManager", "Storage optimization: MIGRATION FAILURE %s MAXSIZE %u",
|
||||
dyn->getKey().toString().c_str(), MaxBufSize);
|
||||
dyn->getKey().toString().c_str(), MaxBufSize);
|
||||
assert(0);
|
||||
sb->reset();
|
||||
break;
|
||||
@@ -364,7 +381,7 @@ class TransferBufferManager : public ITransferBufferManager, Noncopyable
|
||||
|
||||
public:
|
||||
TransferBufferManager(IAllocator& allocator)
|
||||
: allocator_(allocator)
|
||||
: allocator_(allocator)
|
||||
{
|
||||
StaticAssert<(MaxBufSize > 0)>::check();
|
||||
StaticAssert<(NumStaticBufs > 0)>::check();
|
||||
@@ -391,7 +408,9 @@ public:
|
||||
}
|
||||
TransferBufferManagerEntry* tbme = findFirstStatic(key);
|
||||
if (tbme)
|
||||
{
|
||||
return tbme;
|
||||
}
|
||||
return findFirstDynamic(key);
|
||||
}
|
||||
|
||||
@@ -407,10 +426,13 @@ public:
|
||||
TransferBufferManagerEntry* tbme = findFirstStatic(TransferBufferManagerKey());
|
||||
if (tbme == NULL)
|
||||
{
|
||||
DynamicTransferBufferManagerEntry* dyn = DynamicTransferBufferManagerEntry::instantiate(allocator_, MaxBufSize);
|
||||
DynamicTransferBufferManagerEntry* dyn =
|
||||
DynamicTransferBufferManagerEntry::instantiate(allocator_, MaxBufSize);
|
||||
tbme = dyn;
|
||||
if (dyn == NULL)
|
||||
{
|
||||
return NULL; // Epic fail.
|
||||
}
|
||||
dynamic_buffers_.insert(dyn);
|
||||
UAVCAN_TRACE("TransferBufferManager", "Dynamic buffer created [st=%u, dyn=%u], %s",
|
||||
getNumStaticBuffers(), getNumDynamicBuffers(), key.toString().c_str());
|
||||
@@ -461,7 +483,9 @@ public:
|
||||
for (unsigned int i = 0; i < NumStaticBufs; i++)
|
||||
{
|
||||
if (!static_buffers_[i].isEmpty())
|
||||
{
|
||||
res++;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
@@ -34,12 +34,12 @@ class IncomingTransfer : public ITransferBuffer
|
||||
protected:
|
||||
IncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc, TransferType transfer_type,
|
||||
TransferID transfer_id, NodeID source_node_id, uint8_t iface_index)
|
||||
: ts_mono_(ts_mono)
|
||||
, ts_utc_(ts_utc)
|
||||
, transfer_type_(transfer_type)
|
||||
, transfer_id_(transfer_id)
|
||||
, src_node_id_(source_node_id)
|
||||
, iface_index_(iface_index)
|
||||
: ts_mono_(ts_mono)
|
||||
, ts_utc_(ts_utc)
|
||||
, transfer_type_(transfer_type)
|
||||
, transfer_id_(transfer_id)
|
||||
, src_node_id_(source_node_id)
|
||||
, iface_index_(iface_index)
|
||||
{ }
|
||||
|
||||
public:
|
||||
@@ -93,8 +93,8 @@ class TransferListenerBase : public LinkedListNode<TransferListenerBase>, Noncop
|
||||
|
||||
protected:
|
||||
TransferListenerBase(const DataTypeDescriptor& data_type)
|
||||
: data_type_(data_type)
|
||||
, crc_base_(data_type.getSignature().toTransferCRC())
|
||||
: data_type_(data_type)
|
||||
, crc_base_(data_type.getSignature().toTransferCRC())
|
||||
{ }
|
||||
|
||||
virtual ~TransferListenerBase() { }
|
||||
@@ -127,8 +127,8 @@ class TransferListener : public TransferListenerBase
|
||||
|
||||
public:
|
||||
TimedOutReceiverPredicate(MonotonicTime ts, BufferManager& bufmgr)
|
||||
: ts_(ts)
|
||||
, bufmgr_(bufmgr)
|
||||
: ts_(ts)
|
||||
, bufmgr_(bufmgr)
|
||||
{ }
|
||||
|
||||
bool operator()(const TransferBufferManagerKey& key, const TransferReceiver& value) const
|
||||
@@ -164,7 +164,9 @@ protected:
|
||||
if (recv == NULL)
|
||||
{
|
||||
if (!frame.isFirst())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
TransferReceiver new_recv;
|
||||
recv = receivers_.insert(key, new_recv);
|
||||
@@ -180,9 +182,9 @@ protected:
|
||||
|
||||
public:
|
||||
TransferListener(const DataTypeDescriptor& data_type, IAllocator& allocator)
|
||||
: TransferListenerBase(data_type)
|
||||
, bufmgr_(allocator)
|
||||
, receivers_(allocator)
|
||||
: TransferListenerBase(data_type)
|
||||
, bufmgr_(allocator)
|
||||
, receivers_(allocator)
|
||||
{
|
||||
StaticAssert<(NumStaticReceivers >= NumStaticBufs)>::check(); // Otherwise it would be meaningless
|
||||
}
|
||||
@@ -212,8 +214,8 @@ public:
|
||||
}
|
||||
|
||||
ExpectedResponseParams(NodeID src_node_id, TransferID transfer_id)
|
||||
: src_node_id(src_node_id)
|
||||
, transfer_id(transfer_id)
|
||||
: src_node_id(src_node_id)
|
||||
, transfer_id(transfer_id)
|
||||
{
|
||||
assert(src_node_id.isUnicast());
|
||||
}
|
||||
@@ -245,7 +247,7 @@ private:
|
||||
|
||||
public:
|
||||
ServiceResponseTransferListener(const DataTypeDescriptor& data_type, IAllocator& allocator)
|
||||
: BaseType(data_type, allocator)
|
||||
: BaseType(data_type, allocator)
|
||||
{ }
|
||||
|
||||
void setExpectedResponseParams(const ExpectedResponseParams& erp)
|
||||
|
||||
@@ -55,11 +55,11 @@ private:
|
||||
|
||||
public:
|
||||
TransferReceiver()
|
||||
: transfer_interval_usec_(DefaultTransferIntervalUSec)
|
||||
, this_transfer_crc_(0)
|
||||
, buffer_write_pos_(0)
|
||||
, iface_index_(IfaceIndexNotSet)
|
||||
, next_frame_index_(0)
|
||||
: transfer_interval_usec_(DefaultTransferIntervalUSec)
|
||||
, this_transfer_crc_(0)
|
||||
, buffer_write_pos_(0)
|
||||
, iface_index_(IfaceIndexNotSet)
|
||||
, next_frame_index_(0)
|
||||
{ }
|
||||
|
||||
bool isTimedOut(MonotonicTime current_ts) const;
|
||||
|
||||
@@ -35,13 +35,13 @@ public:
|
||||
|
||||
TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos,
|
||||
MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval())
|
||||
: max_transfer_interval_(max_transfer_interval)
|
||||
, data_type_(data_type)
|
||||
, qos_(qos)
|
||||
, crc_base_(data_type.getSignature().toTransferCRC())
|
||||
, flags_(CanIOFlags(0))
|
||||
, iface_mask_(AllIfacesMask)
|
||||
, dispatcher_(dispatcher)
|
||||
: max_transfer_interval_(max_transfer_interval)
|
||||
, data_type_(data_type)
|
||||
, qos_(qos)
|
||||
, crc_base_(data_type.getSignature().toTransferCRC())
|
||||
, flags_(CanIOFlags(0))
|
||||
, iface_mask_(AllIfacesMask)
|
||||
, dispatcher_(dispatcher)
|
||||
{ }
|
||||
|
||||
CanIOFlags getCanIOFlags() const { return flags_; }
|
||||
|
||||
@@ -23,7 +23,7 @@ struct StaticAssert<true>
|
||||
* Usage:
|
||||
* ShowIntegerAsError<integer_expression>::foobar();
|
||||
*/
|
||||
template<long N> struct ShowIntegerAsError;
|
||||
template <long N> struct ShowIntegerAsError;
|
||||
|
||||
/**
|
||||
* Prevents copying when inherited
|
||||
@@ -39,15 +39,21 @@ protected:
|
||||
/**
|
||||
* Compile time conditions
|
||||
*/
|
||||
template<bool B, typename T = void>
|
||||
template <bool B, typename T = void>
|
||||
struct EnableIf { };
|
||||
|
||||
template<typename T>
|
||||
struct EnableIf<true, T> { typedef T Type; };
|
||||
template <typename T>
|
||||
struct EnableIf<true, T>
|
||||
{
|
||||
typedef T Type;
|
||||
};
|
||||
|
||||
|
||||
template<typename T, typename R = void>
|
||||
struct EnableIfType { typedef R Type; };
|
||||
template <typename T, typename R = void>
|
||||
struct EnableIfType
|
||||
{
|
||||
typedef R Type;
|
||||
};
|
||||
|
||||
|
||||
template <bool Condition, typename TrueType, typename FalseType>
|
||||
@@ -68,7 +74,7 @@ struct Select<false, TrueType, FalseType>
|
||||
/**
|
||||
* Value types
|
||||
*/
|
||||
template<bool> struct BooleanType { };
|
||||
template <bool> struct BooleanType { };
|
||||
typedef BooleanType<true> TrueType;
|
||||
typedef BooleanType<false> FalseType;
|
||||
|
||||
@@ -102,14 +108,14 @@ template <typename To, typename From>
|
||||
To try_implicit_cast(const From& from, const To& default_)
|
||||
{
|
||||
return TryImplicitCastImpl<From, To>::impl(from, default_,
|
||||
BooleanType<IsImplicitlyConvertibleFromTo<From, To>::Result>());
|
||||
BooleanType<IsImplicitlyConvertibleFromTo<From, To>::Result>());
|
||||
}
|
||||
|
||||
template <typename To, typename From>
|
||||
To try_implicit_cast(const From& from)
|
||||
{
|
||||
return TryImplicitCastImpl<From, To>::impl(from, To(),
|
||||
BooleanType<IsImplicitlyConvertibleFromTo<From, To>::Result>());
|
||||
BooleanType<IsImplicitlyConvertibleFromTo<From, To>::Result>());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -24,13 +24,17 @@ class LazyConstructor
|
||||
void ensureConstructed() const
|
||||
{
|
||||
if (!ptr_)
|
||||
{
|
||||
handleFatalError("LazyConstructor<T> is not constructed");
|
||||
}
|
||||
}
|
||||
|
||||
void ensureNotConstructed() const
|
||||
{
|
||||
if (ptr_)
|
||||
{
|
||||
handleFatalError("LazyConstructor<T> is already constructed");
|
||||
}
|
||||
}
|
||||
|
||||
template <typename U> struct ParamType { typedef const U& Type; };
|
||||
@@ -41,17 +45,19 @@ class LazyConstructor
|
||||
|
||||
public:
|
||||
LazyConstructor()
|
||||
: ptr_(NULL)
|
||||
: ptr_(NULL)
|
||||
{
|
||||
std::fill(data_, data_ + sizeof(T), 0);
|
||||
}
|
||||
|
||||
LazyConstructor(const LazyConstructor<T>& rhs)
|
||||
: ptr_(NULL)
|
||||
: ptr_(NULL)
|
||||
{
|
||||
std::fill(data_, data_ + sizeof(T), 0);
|
||||
if (rhs)
|
||||
{
|
||||
construct<const T&>(*rhs); // Invoke copy constructor
|
||||
}
|
||||
}
|
||||
|
||||
~LazyConstructor() { destroy(); }
|
||||
@@ -60,7 +66,9 @@ public:
|
||||
{
|
||||
destroy();
|
||||
if (rhs)
|
||||
{
|
||||
construct<const T&>(*rhs); // Invoke copy constructor
|
||||
}
|
||||
return *this;
|
||||
}
|
||||
|
||||
@@ -77,7 +85,9 @@ public:
|
||||
void destroy()
|
||||
{
|
||||
if (ptr_)
|
||||
{
|
||||
ptr_->~T();
|
||||
}
|
||||
ptr_ = NULL;
|
||||
std::fill(data_, data_ + sizeof(T), 0);
|
||||
}
|
||||
|
||||
@@ -20,18 +20,20 @@ class MethodBinder
|
||||
void validateBeforeCall() const
|
||||
{
|
||||
if (!operator bool())
|
||||
{
|
||||
handleFatalError("Null method binder");
|
||||
}
|
||||
}
|
||||
|
||||
public:
|
||||
MethodBinder()
|
||||
: obj_()
|
||||
, fun_()
|
||||
: obj_()
|
||||
, fun_()
|
||||
{ }
|
||||
|
||||
MethodBinder(ObjectPtr o, MemFunPtr f)
|
||||
: obj_(o)
|
||||
, fun_(f)
|
||||
: obj_(o)
|
||||
, fun_(f)
|
||||
{ }
|
||||
|
||||
operator bool() const
|
||||
|
||||
@@ -18,7 +18,9 @@ void DataTypeSignature::mixin64(uint64_t x)
|
||||
{
|
||||
DataTypeSignatureCRC crc = DataTypeSignatureCRC::extend(value_);
|
||||
for (int i = 0; i < 64; i += 8) // LSB first
|
||||
{
|
||||
crc.add((x >> i) & 0xFF);
|
||||
}
|
||||
value_ = crc.get();
|
||||
}
|
||||
|
||||
@@ -33,7 +35,9 @@ TransferCRC DataTypeSignature::toTransferCRC() const
|
||||
{
|
||||
TransferCRC tcrc;
|
||||
for (int i = 0; i < 64; i += 8) // LSB first
|
||||
{
|
||||
tcrc.add((value_ >> i) & 0xFF);
|
||||
}
|
||||
return tcrc;
|
||||
}
|
||||
|
||||
@@ -71,7 +75,7 @@ std::string DataTypeDescriptor::toString() const
|
||||
|
||||
std::ostringstream os;
|
||||
os << full_name_ << ":" << id_.get() << kindch << ":" << std::hex
|
||||
<< std::setfill('0') << std::setw(16) << signature_.get();
|
||||
<< std::setfill('0') << std::setw(16) << signature_.get();
|
||||
return os.str();
|
||||
}
|
||||
|
||||
|
||||
@@ -52,17 +52,23 @@ std::string CanFrame::toString(StringRepresentation mode) const
|
||||
else
|
||||
{
|
||||
for (int dlen = 0; dlen < dlc; dlen++) // hex bytes
|
||||
{
|
||||
wpos += snprintf(wpos, epos - wpos, " %02x", (unsigned int)data[dlen]);
|
||||
}
|
||||
|
||||
while (mode == StrAligned && wpos < buf + ASCII_COLUMN_OFFSET) // alignment
|
||||
{
|
||||
*wpos++ = ' ';
|
||||
}
|
||||
|
||||
wpos += snprintf(wpos, epos - wpos, " \'"); // ascii
|
||||
for (int dlen = 0; dlen < dlc; dlen++)
|
||||
{
|
||||
uint8_t ch = data[dlen];
|
||||
if (ch < 0x20 || ch > 0x7E)
|
||||
{
|
||||
ch = '.';
|
||||
}
|
||||
wpos += snprintf(wpos, epos - wpos, "%c", ch);
|
||||
}
|
||||
wpos += snprintf(wpos, epos - wpos, "\'");
|
||||
|
||||
@@ -15,7 +15,7 @@
|
||||
} else { \
|
||||
*dst &= reverse_mask[dst_offset_modulo] | \
|
||||
reverse_mask_xor[dst_offset_modulo + src_len + 1]; \
|
||||
c &= reverse_mask[dst_offset_modulo + src_len]; \
|
||||
c &= reverse_mask[dst_offset_modulo + src_len]; \
|
||||
src_len = 0; \
|
||||
} } while (0)
|
||||
|
||||
|
||||
@@ -38,9 +38,13 @@ int BitStream::write(const uint8_t* bytes, const int bitlen)
|
||||
*/
|
||||
const int write_res = buf_.write(bit_offset_ / 8, tmp, bytelen);
|
||||
if (write_res < 0)
|
||||
{
|
||||
return write_res;
|
||||
}
|
||||
if (write_res < bytelen)
|
||||
{
|
||||
return ResultOutOfBuffer;
|
||||
}
|
||||
|
||||
bit_offset_ = new_bit_offset;
|
||||
return ResultOk;
|
||||
@@ -55,9 +59,13 @@ int BitStream::read(uint8_t* bytes, const int bitlen)
|
||||
|
||||
const int read_res = buf_.read(bit_offset_ / 8, tmp, bytelen);
|
||||
if (read_res < 0)
|
||||
{
|
||||
return read_res;
|
||||
}
|
||||
if (read_res < bytelen)
|
||||
{
|
||||
return ResultOutOfBuffer;
|
||||
}
|
||||
|
||||
std::fill(bytes, bytes + bitlenToBytelen(bitlen), 0);
|
||||
copyBitArray(tmp, bit_offset_ % 8, bitlen, bytes, 0);
|
||||
@@ -72,14 +80,20 @@ std::string BitStream::toString() const
|
||||
{
|
||||
uint8_t byte = 0;
|
||||
if (1 != buf_.read(offset, &byte, 1))
|
||||
{
|
||||
break;
|
||||
}
|
||||
for (int i = 7; i >= 0; i--) // Most significant goes first
|
||||
{
|
||||
os << !!(byte & (1 << i));
|
||||
}
|
||||
os << " ";
|
||||
}
|
||||
std::string output = os.str();
|
||||
if (output.length())
|
||||
{
|
||||
output.erase(output.length() - 1, 1);
|
||||
}
|
||||
return output;
|
||||
}
|
||||
|
||||
|
||||
@@ -22,17 +22,27 @@ uint16_t IEEE754Converter::nativeNonIeeeToHalf(float value)
|
||||
{
|
||||
uint16_t hbits = signbit(value) << 15;
|
||||
if (value == 0.0f)
|
||||
{
|
||||
return hbits;
|
||||
}
|
||||
if (isnanf(value))
|
||||
{
|
||||
return hbits | 0x7FFF;
|
||||
}
|
||||
if (isinff(value))
|
||||
{
|
||||
return hbits | 0x7C00;
|
||||
}
|
||||
int exp;
|
||||
std::frexp(value, &exp);
|
||||
if (exp > 16)
|
||||
{
|
||||
return hbits | 0x7C00;
|
||||
}
|
||||
if (exp < -13)
|
||||
{
|
||||
value = std::ldexp(value, 24);
|
||||
}
|
||||
else
|
||||
{
|
||||
value = std::ldexp(value, 11 - exp);
|
||||
@@ -50,14 +60,22 @@ float IEEE754Converter::halfToNativeNonIeee(uint16_t value)
|
||||
float out;
|
||||
int abs = value & 0x7FFF;
|
||||
if (abs > 0x7C00)
|
||||
{
|
||||
out = std::numeric_limits<float>::has_quiet_NaN ? std::numeric_limits<float>::quiet_NaN() : 0.0f;
|
||||
}
|
||||
else if (abs == 0x7C00)
|
||||
{
|
||||
out = std::numeric_limits<float>::has_infinity ?
|
||||
std::numeric_limits<float>::infinity() : std::numeric_limits<float>::max();
|
||||
std::numeric_limits<float>::infinity() : std::numeric_limits<float>::max();
|
||||
}
|
||||
else if (abs > 0x3FF)
|
||||
{
|
||||
out = std::ldexp(static_cast<float>((value & 0x3FF) | 0x400), (abs >> 10) - 25);
|
||||
}
|
||||
else
|
||||
{
|
||||
out = std::ldexp(static_cast<float>(abs), -24);
|
||||
}
|
||||
return (value & 0x8000) ? -out : out;
|
||||
}
|
||||
|
||||
|
||||
@@ -35,11 +35,15 @@ GlobalDataTypeRegistry::RegistResult GlobalDataTypeRegistry::remove(Entry* dtd)
|
||||
return RegistResultInvalidParams;
|
||||
}
|
||||
if (isFrozen())
|
||||
{
|
||||
return RegistResultFrozen;
|
||||
}
|
||||
|
||||
List* list = selectList(dtd->descriptor.getKind());
|
||||
if (!list)
|
||||
{
|
||||
return RegistResultInvalidParams;
|
||||
}
|
||||
|
||||
list->remove(dtd); // If this call came from regist<>(), that would be enough
|
||||
Entry* p = list->get(); // But anyway
|
||||
@@ -47,7 +51,9 @@ GlobalDataTypeRegistry::RegistResult GlobalDataTypeRegistry::remove(Entry* dtd)
|
||||
{
|
||||
Entry* const next = p->getNextListNode();
|
||||
if (p->descriptor.match(dtd->descriptor.getKind(), dtd->descriptor.getFullName()))
|
||||
{
|
||||
list->remove(p);
|
||||
}
|
||||
p = next;
|
||||
}
|
||||
return RegistResultOk;
|
||||
@@ -61,20 +67,28 @@ GlobalDataTypeRegistry::RegistResult GlobalDataTypeRegistry::registImpl(Entry* d
|
||||
return RegistResultInvalidParams;
|
||||
}
|
||||
if (isFrozen())
|
||||
{
|
||||
return RegistResultFrozen;
|
||||
}
|
||||
|
||||
List* list = selectList(dtd->descriptor.getKind());
|
||||
if (!list)
|
||||
{
|
||||
return RegistResultInvalidParams;
|
||||
}
|
||||
|
||||
{ // Collision check
|
||||
Entry* p = list->get();
|
||||
while (p)
|
||||
{
|
||||
if (p->descriptor.getID() == dtd->descriptor.getID()) // ID collision
|
||||
{
|
||||
return RegistResultCollision;
|
||||
}
|
||||
if (!std::strcmp(p->descriptor.getFullName(), dtd->descriptor.getFullName())) // Name collision
|
||||
{
|
||||
return RegistResultCollision;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
}
|
||||
@@ -132,7 +146,9 @@ const DataTypeDescriptor* GlobalDataTypeRegistry::find(DataTypeKind kind, const
|
||||
while (p)
|
||||
{
|
||||
if (p->descriptor.match(kind, name))
|
||||
{
|
||||
return &p->descriptor;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
return NULL;
|
||||
@@ -150,7 +166,9 @@ const DataTypeDescriptor* GlobalDataTypeRegistry::find(DataTypeKind kind, DataTy
|
||||
while (p)
|
||||
{
|
||||
if (p->descriptor.match(kind, dtid))
|
||||
{
|
||||
return &p->descriptor;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
return NULL;
|
||||
@@ -180,23 +198,31 @@ DataTypeSignature GlobalDataTypeRegistry::computeAggregateSignature(DataTypeKind
|
||||
if (inout_id_mask[dtid])
|
||||
{
|
||||
if (signature_initialized)
|
||||
{
|
||||
signature.extend(desc.getSignature());
|
||||
}
|
||||
else
|
||||
{
|
||||
signature = DataTypeSignature(desc.getSignature());
|
||||
}
|
||||
signature_initialized = true;
|
||||
}
|
||||
|
||||
assert(prev_dtid < dtid); // Making sure that list is ordered properly
|
||||
prev_dtid++;
|
||||
while (prev_dtid < dtid)
|
||||
{
|
||||
inout_id_mask[prev_dtid++] = false; // Erasing bits for missing types
|
||||
}
|
||||
assert(prev_dtid == dtid);
|
||||
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
prev_dtid++;
|
||||
while (prev_dtid <= DataTypeID::Max)
|
||||
{
|
||||
inout_id_mask[prev_dtid++] = false;
|
||||
}
|
||||
|
||||
return signature;
|
||||
}
|
||||
|
||||
@@ -70,11 +70,15 @@ bool DeadlineScheduler::doesExist(const DeadlineHandler* mdh) const
|
||||
{
|
||||
#if UAVCAN_DEBUG
|
||||
if (prev_deadline > p->getDeadline()) // Self check
|
||||
{
|
||||
std::abort();
|
||||
}
|
||||
prev_deadline = p->getDeadline();
|
||||
#endif
|
||||
if (p == mdh)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
return false;
|
||||
@@ -86,15 +90,21 @@ MonotonicTime DeadlineScheduler::pollAndGetMonotonicTime(ISystemClock& sysclock)
|
||||
{
|
||||
DeadlineHandler* const mdh = handlers_.get();
|
||||
if (!mdh)
|
||||
{
|
||||
return sysclock.getMonotonic();
|
||||
}
|
||||
#if UAVCAN_DEBUG
|
||||
if (mdh->getNextListNode()) // Order check
|
||||
{
|
||||
assert(mdh->getDeadline() <= mdh->getNextListNode()->getDeadline());
|
||||
}
|
||||
#endif
|
||||
|
||||
const MonotonicTime ts = sysclock.getMonotonic();
|
||||
if (ts < mdh->getDeadline())
|
||||
{
|
||||
return ts;
|
||||
}
|
||||
|
||||
handlers_.remove(mdh);
|
||||
mdh->handleDeadline(ts); // This handler can be re-registered immediately
|
||||
@@ -107,7 +117,9 @@ MonotonicTime DeadlineScheduler::getEarliestDeadline() const
|
||||
{
|
||||
const DeadlineHandler* const mdh = handlers_.get();
|
||||
if (mdh)
|
||||
{
|
||||
return mdh->getDeadline();
|
||||
}
|
||||
return MonotonicTime::getMax();
|
||||
}
|
||||
|
||||
@@ -121,7 +133,9 @@ MonotonicTime Scheduler::computeDispatcherSpinDeadline(MonotonicTime spin_deadli
|
||||
if (earliest > ts)
|
||||
{
|
||||
if (ts - earliest > deadline_resolution_)
|
||||
{
|
||||
return ts + deadline_resolution_;
|
||||
}
|
||||
}
|
||||
return earliest;
|
||||
}
|
||||
@@ -146,12 +160,16 @@ int Scheduler::spin(MonotonicTime deadline)
|
||||
const MonotonicTime dl = computeDispatcherSpinDeadline(deadline);
|
||||
retval = dispatcher_.spin(dl);
|
||||
if (retval < 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock());
|
||||
pollCleanup(ts, retval);
|
||||
if (ts >= deadline)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
return retval;
|
||||
}
|
||||
|
||||
@@ -15,7 +15,9 @@ void Timer::handleDeadline(MonotonicTime current)
|
||||
const MonotonicTime scheduled_time = getDeadline();
|
||||
|
||||
if (period_ < MonotonicDuration::getInfinite())
|
||||
{
|
||||
startWithDeadline(scheduled_time + period_);
|
||||
}
|
||||
|
||||
// Application can re-register the timer with different params, it's OK
|
||||
handleTimerEvent(TimerEvent(scheduled_time, current));
|
||||
|
||||
@@ -89,16 +89,20 @@ int DataTypeInfoProvider::start()
|
||||
res = cats_srv_.start(
|
||||
ComputeAggregateTypeSignatureCallback(this, &DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest));
|
||||
if (res < 0)
|
||||
{
|
||||
goto fail;
|
||||
}
|
||||
|
||||
res = gdti_srv_.start(GetDataTypeInfoCallback(this, &DataTypeInfoProvider::handleGetDataTypeInfoRequest));
|
||||
if (res < 0)
|
||||
{
|
||||
goto fail;
|
||||
}
|
||||
|
||||
assert(res >= 0);
|
||||
return res;
|
||||
|
||||
fail:
|
||||
fail:
|
||||
assert(res < 0);
|
||||
cats_srv_.stop();
|
||||
gdti_srv_.stop();
|
||||
|
||||
@@ -13,8 +13,8 @@ bool NodeStatusProvider::isNodeInfoInitialized() const
|
||||
{
|
||||
// Hardware version is not required
|
||||
return (node_info_.software_version != protocol::SoftwareVersion()) &&
|
||||
(node_info_.uavcan_version != protocol::SoftwareVersion()) &&
|
||||
(!node_info_.name.empty());
|
||||
(node_info_.uavcan_version != protocol::SoftwareVersion()) &&
|
||||
(!node_info_.name.empty());
|
||||
}
|
||||
|
||||
int NodeStatusProvider::publish()
|
||||
@@ -32,7 +32,9 @@ void NodeStatusProvider::publishWithErrorHandling()
|
||||
{
|
||||
const int res = publish();
|
||||
if (res < 0)
|
||||
{
|
||||
getNode().registerInternalFailure("NodeStatus publication failed");
|
||||
}
|
||||
}
|
||||
|
||||
void NodeStatusProvider::handleTimerEvent(const TimerEvent&)
|
||||
@@ -67,21 +69,27 @@ int NodeStatusProvider::startAndPublish()
|
||||
|
||||
res = publish(); // Initial broadcast
|
||||
if (res < 0)
|
||||
{
|
||||
goto fail;
|
||||
}
|
||||
|
||||
res = gdr_sub_.start(GlobalDiscoveryRequestCallback(this, &NodeStatusProvider::handleGlobalDiscoveryRequest));
|
||||
if (res < 0)
|
||||
{
|
||||
goto fail;
|
||||
}
|
||||
|
||||
res = gni_srv_.start(GetNodeInfoCallback(this, &NodeStatusProvider::handleGetNodeInfoRequest));
|
||||
if (res < 0)
|
||||
{
|
||||
goto fail;
|
||||
}
|
||||
|
||||
Timer::startPeriodic(MonotonicDuration::fromMSec(protocol::NodeStatus::PUBLICATION_PERIOD_MS));
|
||||
|
||||
return res;
|
||||
|
||||
fail:
|
||||
fail:
|
||||
assert(res < 0);
|
||||
gdr_sub_.stop();
|
||||
gni_srv_.stop();
|
||||
|
||||
@@ -29,7 +29,9 @@ void PanicBroadcaster::panic(const char* short_reason)
|
||||
while (p && *p)
|
||||
{
|
||||
if (msg_.reason_text.size() == msg_.reason_text.capacity())
|
||||
{
|
||||
break;
|
||||
}
|
||||
msg_.reason_text.push_back(*p);
|
||||
p++;
|
||||
}
|
||||
|
||||
@@ -19,7 +19,7 @@ std::string CanRxFrame::toString(StringRepresentation mode) const
|
||||
{
|
||||
std::ostringstream os;
|
||||
os << CanFrame::toString(mode)
|
||||
<< " ts_m=" << ts_mono << " ts_utc=" << ts_utc << " iface=" << int(iface_index);
|
||||
<< " ts_m=" << ts_mono << " ts_utc=" << ts_utc << " iface=" << int(iface_index);
|
||||
return os.str();
|
||||
}
|
||||
|
||||
@@ -39,14 +39,18 @@ void CanTxQueue::Entry::destroy(Entry*& obj, IAllocator& allocator)
|
||||
bool CanTxQueue::Entry::qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const
|
||||
{
|
||||
if (qos != rhs_qos)
|
||||
{
|
||||
return qos > rhs_qos;
|
||||
}
|
||||
return frame.priorityHigherThan(rhs_frame);
|
||||
}
|
||||
|
||||
bool CanTxQueue::Entry::qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const
|
||||
{
|
||||
if (qos != rhs_qos)
|
||||
{
|
||||
return qos < rhs_qos;
|
||||
}
|
||||
return frame.priorityLowerThan(rhs_frame);
|
||||
}
|
||||
|
||||
@@ -56,11 +60,15 @@ std::string CanTxQueue::Entry::toString() const
|
||||
switch (qos)
|
||||
{
|
||||
case Volatile:
|
||||
{
|
||||
str_qos = "<volat> ";
|
||||
break;
|
||||
}
|
||||
case Persistent:
|
||||
{
|
||||
str_qos = "<perst> ";
|
||||
break;
|
||||
}
|
||||
default:
|
||||
assert(0);
|
||||
str_qos = "<?WTF?> ";
|
||||
@@ -85,7 +93,9 @@ CanTxQueue::~CanTxQueue()
|
||||
void CanTxQueue::registerRejectedFrame()
|
||||
{
|
||||
if (rejected_frames_cnt_ < std::numeric_limits<uint32_t>::max())
|
||||
{
|
||||
rejected_frames_cnt_++;
|
||||
}
|
||||
}
|
||||
|
||||
void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags)
|
||||
@@ -130,7 +140,9 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos,
|
||||
while (p)
|
||||
{
|
||||
if (lowestqos->qosHigherThan(*p))
|
||||
{
|
||||
lowestqos = p;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
// Note that frame with *equal* QoS will be replaced too.
|
||||
@@ -145,8 +157,10 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos,
|
||||
}
|
||||
|
||||
if (praw == NULL)
|
||||
{
|
||||
return; // Seems that there is no memory at all.
|
||||
|
||||
}
|
||||
Entry* entry = new (praw) Entry(frame, tx_deadline, qos, flags);
|
||||
assert(entry);
|
||||
queue_.insertBefore(entry, PriorityInsertionComparator(frame));
|
||||
@@ -167,7 +181,9 @@ CanTxQueue::Entry* CanTxQueue::peek()
|
||||
p = next;
|
||||
}
|
||||
else
|
||||
{
|
||||
return p;
|
||||
}
|
||||
}
|
||||
return NULL;
|
||||
}
|
||||
@@ -187,7 +203,9 @@ bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const
|
||||
{
|
||||
const Entry* entry = queue_.get();
|
||||
if (entry == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return !rhs_frame.priorityHigherThan(entry->frame);
|
||||
}
|
||||
|
||||
@@ -217,10 +235,14 @@ int CanIOManager::sendFromTxQueue(int iface_index)
|
||||
assert(iface_index >= 0 && iface_index < MaxCanIfaces);
|
||||
CanTxQueue::Entry* entry = tx_queues_[iface_index].peek();
|
||||
if (entry == NULL)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
const int res = sendToIface(iface_index, entry->frame, entry->deadline, entry->flags);
|
||||
if (res > 0)
|
||||
{
|
||||
tx_queues_[iface_index].remove(entry);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
@@ -230,7 +252,9 @@ int CanIOManager::makePendingTxMask() const
|
||||
for (int i = 0; i < getNumIfaces(); i++)
|
||||
{
|
||||
if (!tx_queues_[i].isEmpty())
|
||||
{
|
||||
write_mask |= 1 << i;
|
||||
}
|
||||
}
|
||||
return write_mask;
|
||||
}
|
||||
@@ -254,27 +278,33 @@ uint64_t CanIOManager::getNumErrors(int iface_index) const
|
||||
}
|
||||
|
||||
int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
|
||||
int iface_mask, CanTxQueue::Qos qos, CanIOFlags flags)
|
||||
int iface_mask, CanTxQueue::Qos qos, CanIOFlags flags)
|
||||
{
|
||||
const int num_ifaces = getNumIfaces();
|
||||
const int all_ifaces_mask = (1 << num_ifaces) - 1;
|
||||
iface_mask &= all_ifaces_mask;
|
||||
|
||||
if (blocking_deadline > tx_deadline)
|
||||
{
|
||||
blocking_deadline = tx_deadline;
|
||||
}
|
||||
|
||||
int retval = 0;
|
||||
|
||||
while (true)
|
||||
{
|
||||
if (iface_mask == 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
CanSelectMasks masks;
|
||||
masks.write = iface_mask | makePendingTxMask();
|
||||
{
|
||||
const int select_res = driver_.select(masks, blocking_deadline);
|
||||
if (select_res < 0)
|
||||
{
|
||||
return select_res;
|
||||
}
|
||||
assert(masks.read == 0);
|
||||
}
|
||||
|
||||
@@ -294,7 +324,9 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton
|
||||
{
|
||||
res = sendToIface(i, frame, tx_deadline, flags);
|
||||
if (res > 0)
|
||||
{
|
||||
iface_mask &= ~(1 << i); // Mark transmitted
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
@@ -302,7 +334,9 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton
|
||||
res = sendFromTxQueue(i);
|
||||
}
|
||||
if (res > 0)
|
||||
{
|
||||
retval++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -318,7 +352,9 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton
|
||||
for (int i = 0; i < num_ifaces; i++)
|
||||
{
|
||||
if (iface_mask & (1 << i))
|
||||
{
|
||||
tx_queues_[i].push(frame, tx_deadline, qos, flags);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
@@ -338,14 +374,18 @@ int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline
|
||||
{
|
||||
const int select_res = driver_.select(masks, blocking_deadline);
|
||||
if (select_res < 0)
|
||||
{
|
||||
return select_res;
|
||||
}
|
||||
}
|
||||
|
||||
// Write - if buffers are not empty, one frame will be sent for each iface per one receive() call
|
||||
for (int i = 0; i < num_ifaces; i++)
|
||||
{
|
||||
if (masks.write & (1 << i))
|
||||
{
|
||||
sendFromTxQueue(i);
|
||||
}
|
||||
}
|
||||
|
||||
// Read
|
||||
@@ -372,7 +412,9 @@ int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline
|
||||
|
||||
// Timeout checked in the last order - this way we can operate with expired deadline:
|
||||
if (sysclock_.getMonotonic() >= blocking_deadline)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -48,7 +48,9 @@ bool LoopbackFrameListenerRegistry::doesExist(const LoopbackFrameListenerBase* l
|
||||
while (p)
|
||||
{
|
||||
if (p == listener)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
return false;
|
||||
@@ -76,7 +78,9 @@ bool Dispatcher::ListenerRegistry::add(TransferListenerBase* listener, Mode mode
|
||||
while (p)
|
||||
{
|
||||
if (p->getDataTypeDescriptor().getID() == listener->getDataTypeDescriptor().getID())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
}
|
||||
@@ -96,7 +100,9 @@ bool Dispatcher::ListenerRegistry::exists(DataTypeID dtid) const
|
||||
while (p)
|
||||
{
|
||||
if (p->getDataTypeDescriptor().getID() == dtid)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
return false;
|
||||
@@ -118,9 +124,13 @@ void Dispatcher::ListenerRegistry::handleFrame(const RxFrame& frame)
|
||||
while (p)
|
||||
{
|
||||
if (p->getDataTypeDescriptor().getID() == frame.getDataTypeID())
|
||||
{
|
||||
p->handleFrame(frame);
|
||||
}
|
||||
else if (p->getDataTypeDescriptor().getID() < frame.getDataTypeID()) // Listeners are ordered by data type id!
|
||||
{
|
||||
break;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
}
|
||||
@@ -147,16 +157,22 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame)
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
case TransferTypeMessageUnicast:
|
||||
{
|
||||
lmsg_.handleFrame(frame);
|
||||
break;
|
||||
}
|
||||
|
||||
case TransferTypeServiceRequest:
|
||||
{
|
||||
lsrv_req_.handleFrame(frame);
|
||||
break;
|
||||
}
|
||||
|
||||
case TransferTypeServiceResponse:
|
||||
{
|
||||
lsrv_resp_.handleFrame(frame);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
assert(0);
|
||||
@@ -185,7 +201,9 @@ int Dispatcher::spin(MonotonicTime deadline)
|
||||
CanRxFrame frame;
|
||||
const int res = canio_.receive(frame, deadline, flags);
|
||||
if (res < 0)
|
||||
{
|
||||
return res;
|
||||
}
|
||||
if (res > 0)
|
||||
{
|
||||
if (flags & CanIOFlagLoopback)
|
||||
@@ -283,8 +301,8 @@ bool Dispatcher::hasSubscriber(DataTypeID dtid) const
|
||||
|
||||
bool Dispatcher::hasPublisher(DataTypeID dtid) const
|
||||
{
|
||||
return outgoing_transfer_reg_.exists(dtid, TransferTypeMessageBroadcast)
|
||||
|| outgoing_transfer_reg_.exists(dtid, TransferTypeMessageUnicast);
|
||||
return outgoing_transfer_reg_.exists(dtid, TransferTypeMessageBroadcast) ||
|
||||
outgoing_transfer_reg_.exists(dtid, TransferTypeMessageUnicast);
|
||||
}
|
||||
|
||||
bool Dispatcher::hasServer(DataTypeID dtid) const
|
||||
|
||||
@@ -18,15 +18,15 @@ int Frame::getMaxPayloadLen() const
|
||||
switch (getTransferType())
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
{
|
||||
return sizeof(payload_);
|
||||
break;
|
||||
|
||||
}
|
||||
case TransferTypeServiceResponse:
|
||||
case TransferTypeServiceRequest:
|
||||
case TransferTypeMessageUnicast:
|
||||
{
|
||||
return sizeof(payload_) - 1;
|
||||
break;
|
||||
|
||||
}
|
||||
default:
|
||||
assert(0);
|
||||
return -1;
|
||||
@@ -53,7 +53,9 @@ inline static uint32_t bitunpack(uint32_t val)
|
||||
bool Frame::parse(const CanFrame& can_frame)
|
||||
{
|
||||
if (can_frame.isErrorFrame() || can_frame.isRemoteTransmissionRequest() || !can_frame.isExtended())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (can_frame.dlc > sizeof(CanFrame::data))
|
||||
{
|
||||
@@ -78,22 +80,30 @@ bool Frame::parse(const CanFrame& can_frame)
|
||||
switch (transfer_type_)
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
{
|
||||
dst_node_id_ = NodeID::Broadcast;
|
||||
payload_len_ = can_frame.dlc;
|
||||
std::copy(can_frame.data, can_frame.data + can_frame.dlc, payload_);
|
||||
break;
|
||||
}
|
||||
|
||||
case TransferTypeServiceResponse:
|
||||
case TransferTypeServiceRequest:
|
||||
case TransferTypeMessageUnicast:
|
||||
{
|
||||
if (can_frame.dlc < 1)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (can_frame.data[0] & 0x80) // RESERVED, must be zero
|
||||
{
|
||||
return false;
|
||||
}
|
||||
dst_node_id_ = can_frame.data[0] & 0x7F;
|
||||
payload_len_ = can_frame.dlc - 1;
|
||||
std::copy(can_frame.data + 1, can_frame.data + can_frame.dlc, payload_);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
return false;
|
||||
@@ -128,18 +138,22 @@ bool Frame::compile(CanFrame& out_can_frame) const
|
||||
switch (transfer_type_)
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
{
|
||||
out_can_frame.dlc = payload_len_;
|
||||
std::copy(payload_, payload_ + payload_len_, out_can_frame.data);
|
||||
break;
|
||||
}
|
||||
|
||||
case TransferTypeServiceResponse:
|
||||
case TransferTypeServiceRequest:
|
||||
case TransferTypeMessageUnicast:
|
||||
{
|
||||
assert((payload_len_ + 1) <= sizeof(CanFrame::data));
|
||||
out_can_frame.data[0] = dst_node_id_.get();
|
||||
out_can_frame.dlc = payload_len_ + 1;
|
||||
std::copy(payload_, payload_ + payload_len_, out_can_frame.data + 1);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
assert(0);
|
||||
@@ -200,7 +214,9 @@ std::string Frame::toString() const
|
||||
{
|
||||
ofs += std::snprintf(buf + ofs, BUFLEN - ofs, "%02x", payload_[i]);
|
||||
if ((i + 1) < payload_len_)
|
||||
{
|
||||
ofs += std::snprintf(buf + ofs, BUFLEN - ofs, " ");
|
||||
}
|
||||
}
|
||||
ofs += std::snprintf(buf + ofs, BUFLEN - ofs, "]");
|
||||
return std::string(buf);
|
||||
|
||||
@@ -22,7 +22,9 @@ int TransferID::computeForwardDistance(TransferID rhs) const
|
||||
{
|
||||
int d = int(rhs.get()) - int(get());
|
||||
if (d < 0)
|
||||
{
|
||||
d += 1 << BitLen;
|
||||
}
|
||||
|
||||
assert(((get() + d) & Max) == rhs.get());
|
||||
return d;
|
||||
|
||||
@@ -27,7 +27,9 @@ DynamicTransferBufferManagerEntry::Block* DynamicTransferBufferManagerEntry::Blo
|
||||
{
|
||||
void* const praw = allocator.allocate(sizeof(Block));
|
||||
if (praw == NULL)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
return new (praw) Block;
|
||||
}
|
||||
|
||||
@@ -77,7 +79,9 @@ DynamicTransferBufferManagerEntry* DynamicTransferBufferManagerEntry::instantiat
|
||||
{
|
||||
void* const praw = allocator.allocate(sizeof(DynamicTransferBufferManagerEntry));
|
||||
if (praw == NULL)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
return new (praw) DynamicTransferBufferManagerEntry(allocator, max_size);
|
||||
}
|
||||
|
||||
@@ -112,9 +116,13 @@ int DynamicTransferBufferManagerEntry::read(unsigned int offset, uint8_t* data,
|
||||
return -1;
|
||||
}
|
||||
if (offset >= max_write_pos_)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if ((offset + len) > max_write_pos_)
|
||||
{
|
||||
len = max_write_pos_ - offset;
|
||||
}
|
||||
assert((offset + len) <= max_write_pos_);
|
||||
|
||||
// This shall be optimized.
|
||||
@@ -125,7 +133,9 @@ int DynamicTransferBufferManagerEntry::read(unsigned int offset, uint8_t* data,
|
||||
{
|
||||
p->read(outptr, offset, total_offset, left_to_read);
|
||||
if (left_to_read == 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
|
||||
@@ -142,9 +152,13 @@ int DynamicTransferBufferManagerEntry::write(unsigned int offset, const uint8_t*
|
||||
}
|
||||
|
||||
if (offset >= max_size_)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if ((offset + len) > max_size_)
|
||||
{
|
||||
len = max_size_ - offset;
|
||||
}
|
||||
assert((offset + len) <= max_size_);
|
||||
|
||||
unsigned int total_offset = 0;
|
||||
@@ -159,7 +173,9 @@ int DynamicTransferBufferManagerEntry::write(unsigned int offset, const uint8_t*
|
||||
last_written_block = p;
|
||||
p->write(inptr, offset, total_offset, left_to_write);
|
||||
if (left_to_write == 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
|
||||
@@ -172,8 +188,10 @@ int DynamicTransferBufferManagerEntry::write(unsigned int offset, const uint8_t*
|
||||
// Allocating the chunk
|
||||
Block* new_block = Block::instantiate(allocator_);
|
||||
if (new_block == NULL)
|
||||
{
|
||||
break; // We're in deep shit.
|
||||
|
||||
}
|
||||
// Appending the chain with the new block
|
||||
if (last_written_block != NULL)
|
||||
{
|
||||
|
||||
@@ -22,10 +22,10 @@ int IncomingTransfer::write(unsigned int, const uint8_t*, unsigned int)
|
||||
* SingleFrameIncomingTransfer
|
||||
*/
|
||||
SingleFrameIncomingTransfer::SingleFrameIncomingTransfer(const RxFrame& frm)
|
||||
: IncomingTransfer(frm.getMonotonicTimestamp(), frm.getUtcTimestamp(), frm.getTransferType(),
|
||||
frm.getTransferID(), frm.getSrcNodeID(), frm.getIfaceIndex())
|
||||
, payload_(frm.getPayloadPtr())
|
||||
, payload_len_(frm.getPayloadLen())
|
||||
: IncomingTransfer(frm.getMonotonicTimestamp(), frm.getUtcTimestamp(), frm.getTransferType(),
|
||||
frm.getTransferID(), frm.getSrcNodeID(), frm.getIfaceIndex())
|
||||
, payload_(frm.getPayloadPtr())
|
||||
, payload_len_(frm.getPayloadLen())
|
||||
{
|
||||
assert(frm.isValid());
|
||||
}
|
||||
@@ -38,9 +38,13 @@ int SingleFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsign
|
||||
return -1;
|
||||
}
|
||||
if (offset >= payload_len_)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if ((offset + len) > payload_len_)
|
||||
{
|
||||
len = payload_len_ - offset;
|
||||
}
|
||||
assert((offset + len) <= payload_len_);
|
||||
std::copy(payload_ + offset, payload_ + offset + len, data);
|
||||
return len;
|
||||
@@ -51,9 +55,9 @@ int SingleFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsign
|
||||
*/
|
||||
MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(MonotonicTime ts_mono, UtcTime ts_utc,
|
||||
const RxFrame& last_frame, TransferBufferAccessor& tba)
|
||||
: IncomingTransfer(ts_mono, ts_utc, last_frame.getTransferType(), last_frame.getTransferID(),
|
||||
last_frame.getSrcNodeID(), last_frame.getIfaceIndex())
|
||||
, buf_acc_(tba)
|
||||
: IncomingTransfer(ts_mono, ts_utc, last_frame.getTransferType(), last_frame.getTransferID(),
|
||||
last_frame.getSrcNodeID(), last_frame.getIfaceIndex())
|
||||
, buf_acc_(tba)
|
||||
{
|
||||
assert(last_frame.isValid());
|
||||
assert(last_frame.isLast());
|
||||
@@ -87,7 +91,9 @@ bool TransferListenerBase::checkPayloadCrc(const uint16_t compare_with, const IT
|
||||
return false;
|
||||
}
|
||||
if (res == 0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
offset += res;
|
||||
crc.add(buf, res);
|
||||
}
|
||||
@@ -106,8 +112,10 @@ void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxF
|
||||
switch (receiver.addFrame(frame, tba))
|
||||
{
|
||||
case TransferReceiver::ResultNotComplete:
|
||||
{
|
||||
return;
|
||||
|
||||
}
|
||||
case TransferReceiver::ResultSingleFrame:
|
||||
{
|
||||
SingleFrameIncomingTransfer it(frame);
|
||||
@@ -138,9 +146,11 @@ void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxF
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
assert(0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -21,9 +21,13 @@ TransferReceiver::TidRelation TransferReceiver::getTidRelation(const RxFrame& fr
|
||||
{
|
||||
const int distance = tid_.computeForwardDistance(frame.getTransferID());
|
||||
if (distance == 0)
|
||||
{
|
||||
return TidSame;
|
||||
}
|
||||
if (distance < ((1 << TransferID::BitLen) / 2))
|
||||
{
|
||||
return TidFuture;
|
||||
}
|
||||
return TidRepeat;
|
||||
}
|
||||
|
||||
@@ -53,7 +57,9 @@ void TransferReceiver::prepareForNextTransfer()
|
||||
bool TransferReceiver::validate(const RxFrame& frame) const
|
||||
{
|
||||
if (iface_index_ != frame.getIfaceIndex())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (frame.isFirst() && !frame.isLast() && (frame.getPayloadLen() < TransferCRC::NumBytes))
|
||||
{
|
||||
@@ -90,15 +96,19 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
|
||||
if (frame.isFirst()) // First frame contains CRC, we need to extract it now
|
||||
{
|
||||
if (frame.getPayloadLen() < TransferCRC::NumBytes)
|
||||
{
|
||||
return false; // Must have been validated earlier though. I think I'm paranoid.
|
||||
|
||||
}
|
||||
this_transfer_crc_ = (payload[0] & 0xFF) | (uint16_t(payload[1] & 0xFF) << 8); // Little endian.
|
||||
|
||||
const int effective_payload_len = payload_len - TransferCRC::NumBytes;
|
||||
const int res = buf.write(buffer_write_pos_, payload + TransferCRC::NumBytes, effective_payload_len);
|
||||
const bool success = res == effective_payload_len;
|
||||
if (success)
|
||||
{
|
||||
buffer_write_pos_ += effective_payload_len;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
else
|
||||
@@ -106,7 +116,9 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
|
||||
const int res = buf.write(buffer_write_pos_, payload, payload_len);
|
||||
const bool success = res == payload_len;
|
||||
if (success)
|
||||
{
|
||||
buffer_write_pos_ += payload_len;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
}
|
||||
@@ -132,7 +144,9 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, Tra
|
||||
// Payload write
|
||||
ITransferBuffer* buf = tba.access();
|
||||
if (buf == NULL)
|
||||
{
|
||||
buf = tba.create();
|
||||
}
|
||||
if (buf == NULL)
|
||||
{
|
||||
UAVCAN_TRACE("TransferReceiver", "Failed to access the buffer, %s", frame.toString().c_str());
|
||||
@@ -161,7 +175,9 @@ bool TransferReceiver::isTimedOut(MonotonicTime current_ts) const
|
||||
{
|
||||
static const uint64_t INTERVAL_MULT = (1 << TransferID::BitLen) / 2 + 1;
|
||||
if (current_ts <= this_transfer_ts_)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return (current_ts - this_transfer_ts_).toUSec() > (uint64_t(transfer_interval_usec_) * INTERVAL_MULT);
|
||||
}
|
||||
|
||||
@@ -209,7 +225,9 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr
|
||||
}
|
||||
|
||||
if (!validate(frame))
|
||||
{
|
||||
return ResultNotComplete;
|
||||
}
|
||||
|
||||
return receive(frame, tba);
|
||||
}
|
||||
|
||||
@@ -59,11 +59,15 @@ int TransferSender::send(const uint8_t* payload, int payload_len, MonotonicTime
|
||||
{
|
||||
const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_, iface_mask_);
|
||||
if (send_res < 0)
|
||||
{
|
||||
return send_res;
|
||||
}
|
||||
|
||||
if (frame.isLast())
|
||||
{
|
||||
return next_frame_index; // Number of frames transmitted
|
||||
|
||||
}
|
||||
frame.setIndex(next_frame_index++);
|
||||
|
||||
const int write_res = frame.setPayload(payload + offset, payload_len - offset);
|
||||
@@ -76,7 +80,9 @@ int TransferSender::send(const uint8_t* payload, int payload_len, MonotonicTime
|
||||
offset += write_res;
|
||||
assert(offset <= payload_len);
|
||||
if (offset >= payload_len)
|
||||
{
|
||||
frame.makeLast();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user