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:
Pavel Kirienko
2014-03-25 02:52:39 +04:00
parent cce657e1fe
commit 0cc627016a
64 changed files with 777 additions and 283 deletions
+11 -7
View File
@@ -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);
+6 -6
View File
@@ -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
+6 -6
View File
@@ -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)
{ }
};
+11 -1
View File
@@ -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));
+6 -2
View File
@@ -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)
{
+33 -4
View File
@@ -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();
}
+74 -5
View File
@@ -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;
}
+16 -4
View File
@@ -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_;
}
+1 -1
View File
@@ -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
+5 -5
View File
@@ -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
+7 -3
View File
@@ -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();
}
+12 -8
View File
@@ -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();
+2 -2
View File
@@ -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_; }
+12 -6
View File
@@ -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;
}
+13 -13
View File
@@ -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
+3 -1
View File
@@ -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);
+17 -17
View File
@@ -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_; }
+15 -9
View File
@@ -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
+5 -1
View File
@@ -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();
}
+6
View File
@@ -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, "\'");
+1 -1
View File
@@ -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)
+14
View File
@@ -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;
}
+19 -1
View File
@@ -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;
}
+18
View File
@@ -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;
}
+2
View File
@@ -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++;
}
+44 -2
View File
@@ -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;
}
+20 -2
View File
@@ -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
+20 -4
View File
@@ -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);
+2
View File
@@ -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)
{
+17 -7
View File
@@ -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();
}
}
}