mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-28 05:24:08 +08:00
Fixed inclusion loops
This commit is contained in:
parent
6eee97fb05
commit
befea376c2
@ -23,10 +23,8 @@ typedef ${t.cpp_type_name} ${t.short_name};
|
||||
}
|
||||
% endfor
|
||||
|
||||
#include <uavcan/data_type.hpp>
|
||||
#include <uavcan/global_data_type_registry.hpp>
|
||||
#include <uavcan/internal/marshal/types.hpp>
|
||||
#include <uavcan/util/compile_time.hpp>
|
||||
|
||||
% for inc in t.cpp_includes:
|
||||
#include <${inc}>
|
||||
|
||||
@ -6,6 +6,7 @@
|
||||
|
||||
#include <cassert>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <stdint.h>
|
||||
#include <algorithm>
|
||||
#include <uavcan/internal/transport/transfer.hpp>
|
||||
@ -93,7 +94,7 @@ class DataTypeDescriptor
|
||||
const char* full_name_;
|
||||
|
||||
public:
|
||||
enum { MaxDataTypeID = Frame::MaxDataTypeID };
|
||||
enum { MaxDataTypeID = 1023 };
|
||||
enum { MaxFullNameLen = 80 };
|
||||
|
||||
DataTypeDescriptor()
|
||||
|
||||
@ -9,9 +9,9 @@
|
||||
#include <stdint.h>
|
||||
#include <algorithm>
|
||||
#include <uavcan/data_type.hpp>
|
||||
#include <uavcan/util/compile_time.hpp>
|
||||
#include <uavcan/internal/fatal_error.hpp>
|
||||
#include <uavcan/internal/linked_list.hpp>
|
||||
#include <uavcan/internal/impl_constants.hpp>
|
||||
#if UAVCAN_DEBUG
|
||||
#include <uavcan/internal/debug.hpp>
|
||||
#endif
|
||||
|
||||
@ -8,11 +8,12 @@
|
||||
#include <stdint.h>
|
||||
#include <string>
|
||||
#include <uavcan/util/compile_time.hpp>
|
||||
#include <uavcan/internal/transport/transfer_buffer.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
class ITransferBuffer;
|
||||
|
||||
void bitarray_copy(const unsigned char *src_org, int src_offset, int src_len, unsigned char *dst_org, int dst_offset);
|
||||
|
||||
class BitStream
|
||||
|
||||
120
libuavcan/include/uavcan/internal/transport/frame.hpp
Normal file
120
libuavcan/include/uavcan/internal/transport/frame.hpp
Normal file
@ -0,0 +1,120 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <algorithm>
|
||||
#include <string>
|
||||
#include <uavcan/internal/transport/transfer.hpp>
|
||||
#include <uavcan/internal/transport/can_io.hpp>
|
||||
#include <uavcan/data_type.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
struct CanRxFrame;
|
||||
|
||||
class Frame
|
||||
{
|
||||
uint8_t payload_[sizeof(CanFrame::data)];
|
||||
TransferType transfer_type_;
|
||||
uint_fast16_t data_type_id_;
|
||||
uint_fast8_t payload_len_;
|
||||
NodeID src_node_id_;
|
||||
NodeID dst_node_id_;
|
||||
uint_fast8_t frame_index_;
|
||||
TransferID transfer_id_;
|
||||
bool last_frame_;
|
||||
|
||||
public:
|
||||
enum { MaxIndex = 62 }; // 63 (or 0b111111) is reserved
|
||||
|
||||
Frame()
|
||||
: transfer_type_(TransferType(NumTransferTypes)) // That is invalid value
|
||||
, data_type_id_(0)
|
||||
, payload_len_(0)
|
||||
, frame_index_(0)
|
||||
, transfer_id_(0)
|
||||
, last_frame_(false)
|
||||
{ }
|
||||
|
||||
Frame(uint_fast16_t 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)
|
||||
{
|
||||
assert((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast());
|
||||
assert(data_type_id <= DataTypeDescriptor::MaxDataTypeID);
|
||||
assert(src_node_id != dst_node_id);
|
||||
assert(frame_index <= MaxIndex);
|
||||
}
|
||||
|
||||
int getMaxPayloadLen() const;
|
||||
int setPayload(const uint8_t* data, int len);
|
||||
|
||||
int getPayloadLen() const { return payload_len_; }
|
||||
const uint8_t* getPayloadPtr() const { return payload_; }
|
||||
|
||||
TransferType getTransferType() const { return transfer_type_; }
|
||||
uint_fast16_t getDataTypeID() const { return data_type_id_; }
|
||||
NodeID getSrcNodeID() const { return src_node_id_; }
|
||||
NodeID getDstNodeID() const { return dst_node_id_; }
|
||||
TransferID getTransferID() const { return transfer_id_; }
|
||||
uint_fast8_t getIndex() const { return frame_index_; }
|
||||
bool isLast() const { return last_frame_; }
|
||||
|
||||
void makeLast() { last_frame_ = true; }
|
||||
void setIndex(uint_fast8_t index) { frame_index_ = index; }
|
||||
|
||||
bool isFirst() const { return frame_index_ == 0; }
|
||||
|
||||
bool parse(const CanFrame& can_frame);
|
||||
bool compile(CanFrame& can_frame) const;
|
||||
|
||||
bool isValid() const;
|
||||
|
||||
bool operator!=(const Frame& rhs) const { return !operator==(rhs); }
|
||||
bool operator==(const Frame& rhs) const;
|
||||
|
||||
std::string toString() const;
|
||||
};
|
||||
|
||||
|
||||
class RxFrame : public Frame
|
||||
{
|
||||
MonotonicTime ts_mono_;
|
||||
UtcTime ts_utc_;
|
||||
uint8_t iface_index_;
|
||||
|
||||
public:
|
||||
RxFrame()
|
||||
: 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)
|
||||
{
|
||||
*static_cast<Frame*>(this) = frame;
|
||||
}
|
||||
|
||||
bool parse(const CanRxFrame& can_frame);
|
||||
|
||||
MonotonicTime getMonotonicTimestamp() const { return ts_mono_; }
|
||||
UtcTime getUtcTimestamp() const { return ts_utc_; }
|
||||
|
||||
uint8_t getIfaceIndex() const { return iface_index_; }
|
||||
|
||||
std::string toString() const;
|
||||
};
|
||||
|
||||
}
|
||||
@ -9,6 +9,7 @@
|
||||
#include <uavcan/internal/map.hpp>
|
||||
#include <uavcan/internal/debug.hpp>
|
||||
#include <uavcan/internal/transport/transfer.hpp>
|
||||
#include <uavcan/time.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
@ -5,15 +5,11 @@
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <algorithm>
|
||||
#include <string>
|
||||
#include <uavcan/can_driver.hpp>
|
||||
#include <stdint.h>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
struct CanRxFrame;
|
||||
|
||||
enum { MaxTransferPayloadLen = 439 }; ///< According to the standard
|
||||
|
||||
enum { MaxSingleFrameTransferPayloadLen = 7 };
|
||||
@ -28,40 +24,6 @@ enum TransferType
|
||||
};
|
||||
|
||||
|
||||
class NodeID
|
||||
{
|
||||
enum
|
||||
{
|
||||
ValueBroadcast = 0,
|
||||
ValueInvalid = 0xFF
|
||||
};
|
||||
uint8_t value_;
|
||||
|
||||
public:
|
||||
enum { BitLen = 7 };
|
||||
enum { Max = (1 << BitLen) - 1 };
|
||||
|
||||
static const NodeID Broadcast;
|
||||
|
||||
NodeID() : value_(ValueInvalid) { }
|
||||
|
||||
NodeID(uint8_t value)
|
||||
: value_(value)
|
||||
{
|
||||
assert(isValid());
|
||||
}
|
||||
|
||||
uint8_t get() const { return value_; }
|
||||
|
||||
bool isValid() const { return value_ <= Max; }
|
||||
bool isBroadcast() const { return 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_; }
|
||||
};
|
||||
|
||||
|
||||
class TransferID
|
||||
{
|
||||
uint8_t value_;
|
||||
@ -102,106 +64,37 @@ public:
|
||||
};
|
||||
|
||||
|
||||
class Frame
|
||||
class NodeID
|
||||
{
|
||||
uint8_t payload_[sizeof(CanFrame::data)];
|
||||
TransferType transfer_type_;
|
||||
uint_fast16_t data_type_id_;
|
||||
uint_fast8_t payload_len_;
|
||||
NodeID src_node_id_;
|
||||
NodeID dst_node_id_;
|
||||
uint_fast8_t frame_index_;
|
||||
TransferID transfer_id_;
|
||||
bool last_frame_;
|
||||
enum
|
||||
{
|
||||
ValueBroadcast = 0,
|
||||
ValueInvalid = 0xFF
|
||||
};
|
||||
uint8_t value_;
|
||||
|
||||
public:
|
||||
enum { MaxDataTypeID = 1023 };
|
||||
enum { MaxIndex = 62 }; // 63 (or 0b111111) is reserved
|
||||
enum { BitLen = 7 };
|
||||
enum { Max = (1 << BitLen) - 1 };
|
||||
|
||||
Frame()
|
||||
: transfer_type_(TransferType(NumTransferTypes)) // That is invalid value
|
||||
, data_type_id_(0)
|
||||
, payload_len_(0)
|
||||
, frame_index_(0)
|
||||
, transfer_id_(0)
|
||||
, last_frame_(false)
|
||||
{ }
|
||||
static const NodeID Broadcast;
|
||||
|
||||
Frame(uint_fast16_t 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)
|
||||
NodeID() : value_(ValueInvalid) { }
|
||||
|
||||
NodeID(uint8_t value)
|
||||
: value_(value)
|
||||
{
|
||||
assert((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast());
|
||||
assert(data_type_id <= MaxDataTypeID);
|
||||
assert(src_node_id != dst_node_id);
|
||||
assert(frame_index <= MaxIndex);
|
||||
assert(isValid());
|
||||
}
|
||||
|
||||
int getMaxPayloadLen() const;
|
||||
int setPayload(const uint8_t* data, int len);
|
||||
uint8_t get() const { return value_; }
|
||||
|
||||
int getPayloadLen() const { return payload_len_; }
|
||||
const uint8_t* getPayloadPtr() const { return payload_; }
|
||||
bool isValid() const { return value_ <= Max; }
|
||||
bool isBroadcast() const { return value_ == ValueBroadcast; }
|
||||
bool isUnicast() const { return (value_ <= Max) && (value_ != ValueBroadcast); }
|
||||
|
||||
TransferType getTransferType() const { return transfer_type_; }
|
||||
uint_fast16_t getDataTypeID() const { return data_type_id_; }
|
||||
NodeID getSrcNodeID() const { return src_node_id_; }
|
||||
NodeID getDstNodeID() const { return dst_node_id_; }
|
||||
TransferID getTransferID() const { return transfer_id_; }
|
||||
uint_fast8_t getIndex() const { return frame_index_; }
|
||||
bool isLast() const { return last_frame_; }
|
||||
|
||||
void makeLast() { last_frame_ = true; }
|
||||
void setIndex(uint_fast8_t index) { frame_index_ = index; }
|
||||
|
||||
bool isFirst() const { return frame_index_ == 0; }
|
||||
|
||||
bool parse(const CanFrame& can_frame);
|
||||
bool compile(CanFrame& can_frame) const;
|
||||
|
||||
bool isValid() const;
|
||||
|
||||
bool operator!=(const Frame& rhs) const { return !operator==(rhs); }
|
||||
bool operator==(const Frame& rhs) const;
|
||||
|
||||
std::string toString() const;
|
||||
};
|
||||
|
||||
|
||||
class RxFrame : public Frame
|
||||
{
|
||||
MonotonicTime ts_mono_;
|
||||
UtcTime ts_utc_;
|
||||
uint8_t iface_index_;
|
||||
|
||||
public:
|
||||
RxFrame()
|
||||
: 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)
|
||||
{
|
||||
*static_cast<Frame*>(this) = frame;
|
||||
}
|
||||
|
||||
bool parse(const CanRxFrame& can_frame);
|
||||
|
||||
MonotonicTime getMonotonicTimestamp() const { return ts_mono_; }
|
||||
UtcTime getUtcTimestamp() const { return ts_utc_; }
|
||||
|
||||
uint8_t getIfaceIndex() const { return iface_index_; }
|
||||
|
||||
std::string toString() const;
|
||||
bool operator!=(NodeID rhs) const { return !operator==(rhs); }
|
||||
bool operator==(NodeID rhs) const { return value_ == rhs.value_; }
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -7,7 +7,7 @@
|
||||
#include <stdint.h>
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
#include <uavcan/internal/transport/transfer.hpp>
|
||||
#include <uavcan/internal/transport/frame.hpp>
|
||||
#include <uavcan/internal/linked_list.hpp>
|
||||
#include <uavcan/internal/dynamic_memory.hpp>
|
||||
#include <uavcan/internal/impl_constants.hpp>
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdlib>
|
||||
#include <uavcan/internal/transport/transfer.hpp>
|
||||
#include <uavcan/internal/transport/frame.hpp>
|
||||
#include <uavcan/internal/transport/transfer_buffer.hpp>
|
||||
|
||||
namespace uavcan
|
||||
|
||||
@ -10,15 +10,8 @@
|
||||
#include <sstream>
|
||||
#include <cstdio>
|
||||
#include <uavcan/util/compile_time.hpp>
|
||||
#include <uavcan/Timestamp.hpp>
|
||||
|
||||
// TODO: Fix inclusion loops!
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
struct Timestamp_;
|
||||
typedef Timestamp_ Timestamp;
|
||||
|
||||
}
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
@ -182,9 +175,24 @@ class UtcTime : public TimeBase<UtcTime, UtcDuration>
|
||||
{
|
||||
public:
|
||||
UtcTime() { }
|
||||
UtcTime(const Timestamp& ts); // Implicit
|
||||
UtcTime& operator=(const Timestamp& ts);
|
||||
operator Timestamp() const;
|
||||
|
||||
UtcTime(const Timestamp& ts) // Implicit
|
||||
{
|
||||
operator=(ts);
|
||||
}
|
||||
|
||||
UtcTime& operator=(const Timestamp& ts)
|
||||
{
|
||||
*this = fromUSec(ts.husec * Timestamp::USEC_PER_LSB);
|
||||
return *this;
|
||||
}
|
||||
|
||||
operator Timestamp() const
|
||||
{
|
||||
Timestamp ts;
|
||||
ts.husec = toUSec() / Timestamp::USEC_PER_LSB;
|
||||
return ts;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -4,8 +4,6 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
/**
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
#include <cassert>
|
||||
#include <sstream>
|
||||
#include <uavcan/internal/marshal/bit_stream.hpp>
|
||||
#include <uavcan/internal/transport/transfer_buffer.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
228
libuavcan/src/internal/transport/frame.cpp
Normal file
228
libuavcan/src/internal/transport/frame.cpp
Normal file
@ -0,0 +1,228 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
#include <sstream>
|
||||
#include <uavcan/internal/transport/frame.hpp>
|
||||
#include <uavcan/internal/transport/can_io.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
/**
|
||||
* Frame
|
||||
*/
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
int Frame::setPayload(const uint8_t* data, int len)
|
||||
{
|
||||
len = std::min(getMaxPayloadLen(), len);
|
||||
if (len >= 0)
|
||||
{
|
||||
std::copy(data, data + len, payload_);
|
||||
payload_len_ = len;
|
||||
}
|
||||
return len;
|
||||
}
|
||||
|
||||
template <int OFFSET, int WIDTH>
|
||||
inline static uint32_t bitunpack(uint32_t val)
|
||||
{
|
||||
return (val >> OFFSET) & ((1UL << WIDTH) - 1);
|
||||
}
|
||||
|
||||
bool Frame::parse(const CanFrame& can_frame)
|
||||
{
|
||||
if ((can_frame.id & CanFrame::FlagRTR) || !(can_frame.id & CanFrame::FlagEFF))
|
||||
return false;
|
||||
|
||||
if (can_frame.dlc > sizeof(CanFrame::data))
|
||||
{
|
||||
assert(0); // This is not a protocol error, so assert() is ok
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
* CAN ID parsing
|
||||
*/
|
||||
const uint32_t id = can_frame.id & CanFrame::MaskExtID;
|
||||
transfer_id_ = bitunpack<0, 3>(id);
|
||||
last_frame_ = bitunpack<3, 1>(id);
|
||||
frame_index_ = bitunpack<4, 6>(id);
|
||||
src_node_id_ = bitunpack<10, 7>(id);
|
||||
transfer_type_ = TransferType(bitunpack<17, 2>(id));
|
||||
data_type_id_ = bitunpack<19, 10>(id);
|
||||
|
||||
/*
|
||||
* CAN payload parsing
|
||||
*/
|
||||
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;
|
||||
}
|
||||
|
||||
return isValid();
|
||||
}
|
||||
|
||||
template <int OFFSET, int WIDTH>
|
||||
inline static uint32_t bitpack(uint32_t field)
|
||||
{
|
||||
return (field & ((1UL << WIDTH) - 1)) << OFFSET;
|
||||
}
|
||||
|
||||
bool Frame::compile(CanFrame& out_can_frame) const
|
||||
{
|
||||
if (!isValid())
|
||||
{
|
||||
assert(0); // This is an application error, so we need to maximize it.
|
||||
return false;
|
||||
}
|
||||
|
||||
out_can_frame.id = CanFrame::FlagEFF |
|
||||
bitpack<0, 3>(transfer_id_.get()) |
|
||||
bitpack<3, 1>(last_frame_) |
|
||||
bitpack<4, 6>(frame_index_) |
|
||||
bitpack<10, 7>(src_node_id_.get()) |
|
||||
bitpack<17, 2>(transfer_type_) |
|
||||
bitpack<19, 10>(data_type_id_);
|
||||
|
||||
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);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Frame::isValid() const
|
||||
{
|
||||
// Refer to the specification for the detailed explanation of the checks
|
||||
const bool invalid =
|
||||
(frame_index_ > MaxIndex) ||
|
||||
((frame_index_ == MaxIndex) && !last_frame_) ||
|
||||
(!src_node_id_.isUnicast()) ||
|
||||
(!dst_node_id_.isValid()) ||
|
||||
(src_node_id_ == dst_node_id_) ||
|
||||
((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast()) ||
|
||||
(transfer_type_ >= NumTransferTypes) ||
|
||||
(payload_len_ > getMaxPayloadLen()) ||
|
||||
(data_type_id_ > DataTypeDescriptor::MaxDataTypeID);
|
||||
|
||||
return !invalid;
|
||||
}
|
||||
|
||||
bool Frame::operator==(const Frame& rhs) const
|
||||
{
|
||||
return
|
||||
(transfer_type_ == rhs.transfer_type_) &&
|
||||
(data_type_id_ == rhs.data_type_id_) &&
|
||||
(src_node_id_ == rhs.src_node_id_) &&
|
||||
(dst_node_id_ == rhs.dst_node_id_) &&
|
||||
(frame_index_ == rhs.frame_index_) &&
|
||||
(transfer_id_ == rhs.transfer_id_) &&
|
||||
(last_frame_ == rhs.last_frame_) &&
|
||||
(payload_len_ == rhs.payload_len_) &&
|
||||
std::equal(payload_, payload_ + payload_len_, rhs.payload_);
|
||||
}
|
||||
|
||||
std::string Frame::toString() const
|
||||
{
|
||||
/*
|
||||
* Frame ID fields, according to UAVCAN specs:
|
||||
* - Data Type ID
|
||||
* - Transfer Type
|
||||
* - Source Node ID
|
||||
* - Frame Index
|
||||
* - Last Frame
|
||||
* - Transfer ID
|
||||
*/
|
||||
static const int BUFLEN = 100;
|
||||
char buf[BUFLEN];
|
||||
int ofs = std::snprintf(buf, BUFLEN, "dtid=%i tt=%i snid=%i dnid=%i idx=%i last=%i tid=%i payload=[",
|
||||
int(data_type_id_), int(transfer_type_), int(src_node_id_.get()), int(dst_node_id_.get()),
|
||||
int(frame_index_), int(last_frame_), int(transfer_id_.get()));
|
||||
|
||||
for (int i = 0; i < payload_len_; i++)
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
* RxFrame
|
||||
*/
|
||||
bool RxFrame::parse(const CanRxFrame& can_frame)
|
||||
{
|
||||
if (!Frame::parse(can_frame))
|
||||
return false;
|
||||
ts_mono_ = can_frame.ts_mono;
|
||||
ts_utc_ = can_frame.ts_utc;
|
||||
iface_index_ = can_frame.iface_index;
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string RxFrame::toString() const
|
||||
{
|
||||
std::ostringstream os; // C++03 doesn't support long long, so we need ostream to print the timestamp
|
||||
os << Frame::toString() << " ts_m=" << ts_mono_ << " ts_utc=" << ts_utc_ << " iface=" << int(iface_index_);
|
||||
return os.str();
|
||||
}
|
||||
|
||||
}
|
||||
@ -5,7 +5,7 @@
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
#include <sstream>
|
||||
#include <uavcan/internal/transport/transfer.hpp>
|
||||
#include <uavcan/internal/transport/frame.hpp>
|
||||
#include <uavcan/internal/transport/can_io.hpp>
|
||||
|
||||
namespace uavcan
|
||||
@ -28,219 +28,4 @@ int TransferID::forwardDistance(TransferID rhs) const
|
||||
return d;
|
||||
}
|
||||
|
||||
/**
|
||||
* Frame
|
||||
*/
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
int Frame::setPayload(const uint8_t* data, int len)
|
||||
{
|
||||
len = std::min(getMaxPayloadLen(), len);
|
||||
if (len >= 0)
|
||||
{
|
||||
std::copy(data, data + len, payload_);
|
||||
payload_len_ = len;
|
||||
}
|
||||
return len;
|
||||
}
|
||||
|
||||
template <int OFFSET, int WIDTH>
|
||||
inline static uint32_t bitunpack(uint32_t val)
|
||||
{
|
||||
return (val >> OFFSET) & ((1UL << WIDTH) - 1);
|
||||
}
|
||||
|
||||
bool Frame::parse(const CanFrame& can_frame)
|
||||
{
|
||||
if ((can_frame.id & CanFrame::FlagRTR) || !(can_frame.id & CanFrame::FlagEFF))
|
||||
return false;
|
||||
|
||||
if (can_frame.dlc > sizeof(CanFrame::data))
|
||||
{
|
||||
assert(0); // This is not a protocol error, so assert() is ok
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
* CAN ID parsing
|
||||
*/
|
||||
const uint32_t id = can_frame.id & CanFrame::MaskExtID;
|
||||
transfer_id_ = bitunpack<0, 3>(id);
|
||||
last_frame_ = bitunpack<3, 1>(id);
|
||||
frame_index_ = bitunpack<4, 6>(id);
|
||||
src_node_id_ = bitunpack<10, 7>(id);
|
||||
transfer_type_ = TransferType(bitunpack<17, 2>(id));
|
||||
data_type_id_ = bitunpack<19, 10>(id);
|
||||
|
||||
/*
|
||||
* CAN payload parsing
|
||||
*/
|
||||
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;
|
||||
}
|
||||
|
||||
return isValid();
|
||||
}
|
||||
|
||||
template <int OFFSET, int WIDTH>
|
||||
inline static uint32_t bitpack(uint32_t field)
|
||||
{
|
||||
return (field & ((1UL << WIDTH) - 1)) << OFFSET;
|
||||
}
|
||||
|
||||
bool Frame::compile(CanFrame& out_can_frame) const
|
||||
{
|
||||
if (!isValid())
|
||||
{
|
||||
assert(0); // This is an application error, so we need to maximize it.
|
||||
return false;
|
||||
}
|
||||
|
||||
out_can_frame.id = CanFrame::FlagEFF |
|
||||
bitpack<0, 3>(transfer_id_.get()) |
|
||||
bitpack<3, 1>(last_frame_) |
|
||||
bitpack<4, 6>(frame_index_) |
|
||||
bitpack<10, 7>(src_node_id_.get()) |
|
||||
bitpack<17, 2>(transfer_type_) |
|
||||
bitpack<19, 10>(data_type_id_);
|
||||
|
||||
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);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Frame::isValid() const
|
||||
{
|
||||
// Refer to the specification for the detailed explanation of the checks
|
||||
const bool invalid =
|
||||
(frame_index_ > MaxIndex) ||
|
||||
((frame_index_ == MaxIndex) && !last_frame_) ||
|
||||
(!src_node_id_.isUnicast()) ||
|
||||
(!dst_node_id_.isValid()) ||
|
||||
(src_node_id_ == dst_node_id_) ||
|
||||
((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast()) ||
|
||||
(transfer_type_ >= NumTransferTypes) ||
|
||||
(payload_len_ > getMaxPayloadLen()) ||
|
||||
(data_type_id_ > MaxDataTypeID);
|
||||
|
||||
return !invalid;
|
||||
}
|
||||
|
||||
bool Frame::operator==(const Frame& rhs) const
|
||||
{
|
||||
return
|
||||
(transfer_type_ == rhs.transfer_type_) &&
|
||||
(data_type_id_ == rhs.data_type_id_) &&
|
||||
(src_node_id_ == rhs.src_node_id_) &&
|
||||
(dst_node_id_ == rhs.dst_node_id_) &&
|
||||
(frame_index_ == rhs.frame_index_) &&
|
||||
(transfer_id_ == rhs.transfer_id_) &&
|
||||
(last_frame_ == rhs.last_frame_) &&
|
||||
(payload_len_ == rhs.payload_len_) &&
|
||||
std::equal(payload_, payload_ + payload_len_, rhs.payload_);
|
||||
}
|
||||
|
||||
std::string Frame::toString() const
|
||||
{
|
||||
/*
|
||||
* Frame ID fields, according to UAVCAN specs:
|
||||
* - Data Type ID
|
||||
* - Transfer Type
|
||||
* - Source Node ID
|
||||
* - Frame Index
|
||||
* - Last Frame
|
||||
* - Transfer ID
|
||||
*/
|
||||
static const int BUFLEN = 100;
|
||||
char buf[BUFLEN];
|
||||
int ofs = std::snprintf(buf, BUFLEN, "dtid=%i tt=%i snid=%i dnid=%i idx=%i last=%i tid=%i payload=[",
|
||||
int(data_type_id_), int(transfer_type_), int(src_node_id_.get()), int(dst_node_id_.get()),
|
||||
int(frame_index_), int(last_frame_), int(transfer_id_.get()));
|
||||
|
||||
for (int i = 0; i < payload_len_; i++)
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
* RxFrame
|
||||
*/
|
||||
bool RxFrame::parse(const CanRxFrame& can_frame)
|
||||
{
|
||||
if (!Frame::parse(can_frame))
|
||||
return false;
|
||||
ts_mono_ = can_frame.ts_mono;
|
||||
ts_utc_ = can_frame.ts_utc;
|
||||
iface_index_ = can_frame.iface_index;
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string RxFrame::toString() const
|
||||
{
|
||||
std::ostringstream os; // C++03 doesn't support long long, so we need ostream to print the timestamp
|
||||
os << Frame::toString() << " ts_m=" << ts_mono_ << " ts_utc=" << ts_utc_ << " iface=" << int(iface_index_);
|
||||
return os.str();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -1,31 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <uavcan/time.hpp>
|
||||
#include <uavcan/Timestamp.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
/*
|
||||
* UtcTime
|
||||
*/
|
||||
UtcTime::UtcTime(const Timestamp& ts) // Implicit
|
||||
{
|
||||
operator=(ts);
|
||||
}
|
||||
|
||||
UtcTime& UtcTime::operator=(const Timestamp& ts)
|
||||
{
|
||||
*this = fromUSec(ts.husec * Timestamp::USEC_PER_LSB);
|
||||
return *this;
|
||||
}
|
||||
|
||||
UtcTime::operator Timestamp() const
|
||||
{
|
||||
Timestamp ts;
|
||||
ts.husec = toUSec() / Timestamp::USEC_PER_LSB;
|
||||
return ts;
|
||||
}
|
||||
|
||||
}
|
||||
@ -3,6 +3,7 @@
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <uavcan/internal/transport/transfer_buffer.hpp>
|
||||
#include <root_ns_a/EmptyService.hpp>
|
||||
#include <root_ns_a/EmptyMessage.hpp>
|
||||
#include <root_ns_a/NestedMessage.hpp>
|
||||
|
||||
@ -4,6 +4,7 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <uavcan/internal/marshal/types.hpp>
|
||||
#include <uavcan/internal/transport/transfer_buffer.hpp>
|
||||
|
||||
using uavcan::Array;
|
||||
using uavcan::ArrayModeDynamic;
|
||||
|
||||
@ -4,6 +4,7 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <uavcan/internal/marshal/bit_stream.hpp>
|
||||
#include <uavcan/internal/transport/transfer_buffer.hpp>
|
||||
|
||||
|
||||
TEST(BitStream, ToString)
|
||||
|
||||
@ -4,6 +4,7 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <uavcan/internal/marshal/types.hpp>
|
||||
#include <uavcan/internal/transport/transfer_buffer.hpp>
|
||||
|
||||
|
||||
TEST(FloatSpec, Limits)
|
||||
|
||||
@ -4,6 +4,7 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <uavcan/internal/marshal/types.hpp>
|
||||
#include <uavcan/internal/transport/transfer_buffer.hpp>
|
||||
|
||||
|
||||
TEST(IntegerSpec, Limits)
|
||||
|
||||
@ -4,6 +4,7 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <uavcan/internal/marshal/scalar_codec.hpp>
|
||||
#include <uavcan/internal/transport/transfer_buffer.hpp>
|
||||
|
||||
|
||||
TEST(ScalarCodec, Basic)
|
||||
|
||||
@ -9,7 +9,7 @@
|
||||
#include <vector>
|
||||
#include <gtest/gtest.h>
|
||||
#include <uavcan/internal/transport/can_io.hpp>
|
||||
#include <uavcan/internal/transport/transfer.hpp>
|
||||
#include <uavcan/internal/transport/frame.hpp>
|
||||
#include <uavcan/can_driver.hpp>
|
||||
#include <uavcan/system_clock.hpp>
|
||||
#include "../../../clock.hpp"
|
||||
|
||||
238
libuavcan/test/internal/transport/frame.cpp
Normal file
238
libuavcan/test/internal/transport/frame.cpp
Normal file
@ -0,0 +1,238 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <string>
|
||||
#include <gtest/gtest.h>
|
||||
#include <uavcan/internal/transport/transfer.hpp>
|
||||
#include "../../clock.hpp"
|
||||
#include "can/can.hpp"
|
||||
|
||||
|
||||
TEST(Frame, FrameParseCompile)
|
||||
{
|
||||
using uavcan::Frame;
|
||||
using uavcan::CanFrame;
|
||||
using uavcan::TransferID;
|
||||
using uavcan::TransferType;
|
||||
|
||||
Frame frame;
|
||||
|
||||
const uint32_t can_id =
|
||||
(2 << 0) | // Transfer ID
|
||||
(1 << 3) | // Last Frame
|
||||
(29 << 4) | // Frame Index
|
||||
(42 << 10) | // Source Node ID
|
||||
(uavcan::TransferTypeMessageBroadcast << 17) |
|
||||
(456 << 19); // Data Type ID
|
||||
|
||||
const std::string payload_string = "hello";
|
||||
|
||||
/*
|
||||
* Parse
|
||||
*/
|
||||
// Invalid CAN frames
|
||||
ASSERT_FALSE(frame.parse(CanFrame(can_id | CanFrame::FlagRTR, (const uint8_t*)"", 0)));
|
||||
ASSERT_FALSE(frame.parse(makeCanFrame(can_id, payload_string, STD)));
|
||||
|
||||
// Valid
|
||||
ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT)));
|
||||
|
||||
EXPECT_EQ(TransferID(2), frame.getTransferID());
|
||||
EXPECT_TRUE(frame.isLast());
|
||||
EXPECT_EQ(29, frame.getIndex());
|
||||
EXPECT_EQ(uavcan::NodeID(42), frame.getSrcNodeID());
|
||||
EXPECT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType());
|
||||
EXPECT_EQ(456, frame.getDataTypeID());
|
||||
|
||||
EXPECT_EQ(payload_string.length(), frame.getPayloadLen());
|
||||
EXPECT_TRUE(std::equal(frame.getPayloadPtr(), frame.getPayloadPtr() + frame.getPayloadLen(),
|
||||
payload_string.begin()));
|
||||
|
||||
/*
|
||||
* Compile
|
||||
*/
|
||||
CanFrame can_frame;
|
||||
ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT)));
|
||||
|
||||
ASSERT_TRUE(frame.compile(can_frame));
|
||||
ASSERT_EQ(can_frame, makeCanFrame(can_id, payload_string, EXT));
|
||||
|
||||
EXPECT_EQ(payload_string.length(), can_frame.dlc);
|
||||
EXPECT_TRUE(std::equal(can_frame.data, can_frame.data + can_frame.dlc, payload_string.begin()));
|
||||
|
||||
/*
|
||||
* Comparison
|
||||
*/
|
||||
ASSERT_FALSE(Frame() == frame);
|
||||
ASSERT_TRUE(Frame() != frame);
|
||||
frame = Frame();
|
||||
ASSERT_TRUE(Frame() == frame);
|
||||
ASSERT_FALSE(Frame() != frame);
|
||||
}
|
||||
|
||||
|
||||
TEST(Frame, FrameParsing)
|
||||
{
|
||||
using uavcan::Frame;
|
||||
using uavcan::CanFrame;
|
||||
using uavcan::NodeID;
|
||||
using uavcan::TransferID;
|
||||
|
||||
CanFrame can;
|
||||
Frame frame;
|
||||
ASSERT_FALSE(frame.parse(can));
|
||||
|
||||
for (unsigned int i = 0; i < sizeof(CanFrame::data); i++)
|
||||
can.data[i] = i | (i << 4);
|
||||
|
||||
// CAN ID field order: Transfer ID, Last Frame, Frame Index, Source Node ID, Transfer Type, Data Type ID
|
||||
|
||||
/*
|
||||
* SFT broadcast
|
||||
*/
|
||||
can.id = CanFrame::FlagEFF |
|
||||
(2 << 0) | (1 << 3) | (0 << 4) | (42 << 10) | (uavcan::TransferTypeMessageBroadcast << 17) | (456 << 19);
|
||||
|
||||
ASSERT_TRUE(frame.parse(can));
|
||||
ASSERT_TRUE(frame.isFirst());
|
||||
ASSERT_TRUE(frame.isLast());
|
||||
ASSERT_EQ(0, frame.getIndex());
|
||||
ASSERT_EQ(NodeID(42), frame.getSrcNodeID());
|
||||
ASSERT_EQ(NodeID::Broadcast, frame.getDstNodeID());
|
||||
ASSERT_EQ(456, frame.getDataTypeID());
|
||||
ASSERT_EQ(TransferID(2), frame.getTransferID());
|
||||
ASSERT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType());
|
||||
ASSERT_EQ(sizeof(CanFrame::data), frame.getMaxPayloadLen());
|
||||
|
||||
/*
|
||||
* SFT addressed
|
||||
*/
|
||||
can.id = CanFrame::FlagEFF |
|
||||
(2 << 0) | (1 << 3) | (0 << 4) | (42 << 10) | (uavcan::TransferTypeMessageUnicast << 17) | (456 << 19);
|
||||
|
||||
ASSERT_FALSE(frame.parse(can)); // No payload - failure
|
||||
|
||||
can.dlc = 1;
|
||||
can.data[0] = 0xFF;
|
||||
ASSERT_FALSE(frame.parse(can)); // Invalid first byte - failure
|
||||
|
||||
can.data[0] = 127;
|
||||
ASSERT_TRUE(frame.parse(can));
|
||||
|
||||
ASSERT_TRUE(frame.isFirst());
|
||||
ASSERT_TRUE(frame.isLast());
|
||||
ASSERT_EQ(0, frame.getIndex());
|
||||
ASSERT_EQ(NodeID(42), frame.getSrcNodeID());
|
||||
ASSERT_EQ(NodeID(127), frame.getDstNodeID());
|
||||
ASSERT_EQ(456, frame.getDataTypeID());
|
||||
ASSERT_EQ(TransferID(2), frame.getTransferID());
|
||||
ASSERT_EQ(uavcan::TransferTypeMessageUnicast, frame.getTransferType());
|
||||
ASSERT_EQ(sizeof(CanFrame::data) - 1, frame.getMaxPayloadLen());
|
||||
|
||||
/*
|
||||
* MFT invalid - unterminated transfer
|
||||
*/
|
||||
can.id = CanFrame::FlagEFF |
|
||||
(2 << 0) | (0 << 3) | (Frame::MaxIndex << 4) | (42 << 10) |
|
||||
(uavcan::TransferTypeMessageUnicast << 17) | (456 << 19);
|
||||
|
||||
ASSERT_FALSE(frame.parse(can));
|
||||
|
||||
/*
|
||||
* MFT invalid - invalid frame index
|
||||
*/
|
||||
can.id = CanFrame::FlagEFF |
|
||||
(2 << 0) | (0 << 3) | (63 << 4) | (42 << 10) | (uavcan::TransferTypeMessageUnicast << 17) | (456 << 19);
|
||||
|
||||
ASSERT_FALSE(frame.parse(can));
|
||||
|
||||
/*
|
||||
* Malformed frames
|
||||
*/
|
||||
can.id = CanFrame::FlagEFF |
|
||||
(2 << 0) | (1 << 3) | (0 << 4) | (42 << 10) | (uavcan::TransferTypeMessageUnicast << 17) | (456 << 19);
|
||||
can.dlc = 3;
|
||||
can.data[0] = 42;
|
||||
ASSERT_FALSE(frame.parse(can)); // Src == Dst Node ID
|
||||
can.data[0] = 41;
|
||||
ASSERT_TRUE(frame.parse(can));
|
||||
|
||||
can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression
|
||||
(2 << 0) | (1 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageUnicast << 17) | (456 << 19);
|
||||
ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID
|
||||
}
|
||||
|
||||
|
||||
TEST(Frame, RxFrameParse)
|
||||
{
|
||||
using uavcan::Frame;
|
||||
using uavcan::RxFrame;
|
||||
using uavcan::CanFrame;
|
||||
using uavcan::CanRxFrame;
|
||||
|
||||
CanRxFrame can_rx_frame;
|
||||
RxFrame rx_frame;
|
||||
|
||||
// Failure
|
||||
ASSERT_FALSE(rx_frame.parse(can_rx_frame));
|
||||
|
||||
// Valid
|
||||
can_rx_frame.id = CanFrame::FlagEFF |
|
||||
(2 << 0) | // Transfer ID
|
||||
(1 << 3) | // Last Frame
|
||||
(29 << 4) | // Frame Index
|
||||
(42 << 10) | // Source Node ID
|
||||
(uavcan::TransferTypeMessageBroadcast << 17) |
|
||||
(456 << 19); // Data Type ID
|
||||
|
||||
ASSERT_TRUE(rx_frame.parse(can_rx_frame));
|
||||
ASSERT_EQ(0, rx_frame.getMonotonicTimestamp().toUSec());
|
||||
ASSERT_EQ(0, rx_frame.getIfaceIndex());
|
||||
|
||||
can_rx_frame.ts_mono = tsMono(123);
|
||||
can_rx_frame.iface_index = 2;
|
||||
|
||||
Frame frame(456, uavcan::TransferTypeMessageBroadcast, 1, uavcan::NodeID::Broadcast, 0, 0);
|
||||
ASSERT_TRUE(frame.compile(can_rx_frame));
|
||||
|
||||
ASSERT_TRUE(rx_frame.parse(can_rx_frame));
|
||||
ASSERT_EQ(123, rx_frame.getMonotonicTimestamp().toUSec());
|
||||
ASSERT_EQ(2, rx_frame.getIfaceIndex());
|
||||
ASSERT_EQ(456, rx_frame.getDataTypeID());
|
||||
ASSERT_EQ(uavcan::TransferTypeMessageBroadcast, rx_frame.getTransferType());
|
||||
}
|
||||
|
||||
|
||||
TEST(Frame, FrameToString)
|
||||
{
|
||||
using uavcan::Frame;
|
||||
using uavcan::RxFrame;
|
||||
|
||||
// RX frame default
|
||||
RxFrame rx_frame;
|
||||
EXPECT_EQ("dtid=0 tt=4 snid=255 dnid=255 idx=0 last=0 tid=0 payload=[] ts_m=0.000000 ts_utc=0.000000 iface=0", rx_frame.toString());
|
||||
|
||||
// RX frame max len
|
||||
rx_frame = RxFrame(Frame(uavcan::DataTypeDescriptor::MaxDataTypeID, uavcan::TransferTypeMessageUnicast,
|
||||
uavcan::NodeID::Max, uavcan::NodeID::Max - 1, Frame::MaxIndex,
|
||||
uavcan::TransferID::Max, true),
|
||||
uavcan::MonotonicTime::getMax(), uavcan::UtcTime::getMax(), 3);
|
||||
|
||||
uint8_t data[8];
|
||||
for (unsigned int i = 0; i < sizeof(data); i++)
|
||||
data[i] = i;
|
||||
rx_frame.setPayload(data, sizeof(data));
|
||||
|
||||
EXPECT_EQ(
|
||||
"dtid=1023 tt=3 snid=127 dnid=126 idx=62 last=1 tid=7 payload=[00 01 02 03 04 05 06] ts_m=18446744073709.551615 ts_utc=18446744073709.551615 iface=3",
|
||||
rx_frame.toString());
|
||||
|
||||
// Plain frame default
|
||||
Frame frame;
|
||||
EXPECT_EQ("dtid=0 tt=4 snid=255 dnid=255 idx=0 last=0 tid=0 payload=[]", frame.toString());
|
||||
|
||||
// Plain frame max len
|
||||
frame = rx_frame;
|
||||
EXPECT_EQ("dtid=1023 tt=3 snid=127 dnid=126 idx=62 last=1 tid=7 payload=[00 01 02 03 04 05 06]", frame.toString());
|
||||
}
|
||||
@ -5,8 +5,6 @@
|
||||
#include <string>
|
||||
#include <gtest/gtest.h>
|
||||
#include <uavcan/internal/transport/transfer.hpp>
|
||||
#include "../../clock.hpp"
|
||||
#include "can/can.hpp"
|
||||
|
||||
|
||||
TEST(Transfer, TransferID)
|
||||
@ -49,231 +47,3 @@ TEST(Transfer, TransferID)
|
||||
ASSERT_EQ(0, tid.forwardDistance(tid));
|
||||
}
|
||||
}
|
||||
|
||||
TEST(Transfer, FrameParseCompile)
|
||||
{
|
||||
using uavcan::Frame;
|
||||
using uavcan::CanFrame;
|
||||
using uavcan::TransferID;
|
||||
using uavcan::TransferType;
|
||||
|
||||
Frame frame;
|
||||
|
||||
const uint32_t can_id =
|
||||
(2 << 0) | // Transfer ID
|
||||
(1 << 3) | // Last Frame
|
||||
(29 << 4) | // Frame Index
|
||||
(42 << 10) | // Source Node ID
|
||||
(uavcan::TransferTypeMessageBroadcast << 17) |
|
||||
(456 << 19); // Data Type ID
|
||||
|
||||
const std::string payload_string = "hello";
|
||||
|
||||
/*
|
||||
* Parse
|
||||
*/
|
||||
// Invalid CAN frames
|
||||
ASSERT_FALSE(frame.parse(CanFrame(can_id | CanFrame::FlagRTR, (const uint8_t*)"", 0)));
|
||||
ASSERT_FALSE(frame.parse(makeCanFrame(can_id, payload_string, STD)));
|
||||
|
||||
// Valid
|
||||
ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT)));
|
||||
|
||||
EXPECT_EQ(TransferID(2), frame.getTransferID());
|
||||
EXPECT_TRUE(frame.isLast());
|
||||
EXPECT_EQ(29, frame.getIndex());
|
||||
EXPECT_EQ(uavcan::NodeID(42), frame.getSrcNodeID());
|
||||
EXPECT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType());
|
||||
EXPECT_EQ(456, frame.getDataTypeID());
|
||||
|
||||
EXPECT_EQ(payload_string.length(), frame.getPayloadLen());
|
||||
EXPECT_TRUE(std::equal(frame.getPayloadPtr(), frame.getPayloadPtr() + frame.getPayloadLen(),
|
||||
payload_string.begin()));
|
||||
|
||||
/*
|
||||
* Compile
|
||||
*/
|
||||
CanFrame can_frame;
|
||||
ASSERT_TRUE(frame.parse(makeCanFrame(can_id, payload_string, EXT)));
|
||||
|
||||
ASSERT_TRUE(frame.compile(can_frame));
|
||||
ASSERT_EQ(can_frame, makeCanFrame(can_id, payload_string, EXT));
|
||||
|
||||
EXPECT_EQ(payload_string.length(), can_frame.dlc);
|
||||
EXPECT_TRUE(std::equal(can_frame.data, can_frame.data + can_frame.dlc, payload_string.begin()));
|
||||
|
||||
/*
|
||||
* Comparison
|
||||
*/
|
||||
ASSERT_FALSE(Frame() == frame);
|
||||
ASSERT_TRUE(Frame() != frame);
|
||||
frame = Frame();
|
||||
ASSERT_TRUE(Frame() == frame);
|
||||
ASSERT_FALSE(Frame() != frame);
|
||||
}
|
||||
|
||||
|
||||
TEST(Transfer, FrameParsing)
|
||||
{
|
||||
using uavcan::Frame;
|
||||
using uavcan::CanFrame;
|
||||
using uavcan::NodeID;
|
||||
using uavcan::TransferID;
|
||||
|
||||
CanFrame can;
|
||||
Frame frame;
|
||||
ASSERT_FALSE(frame.parse(can));
|
||||
|
||||
for (unsigned int i = 0; i < sizeof(CanFrame::data); i++)
|
||||
can.data[i] = i | (i << 4);
|
||||
|
||||
// CAN ID field order: Transfer ID, Last Frame, Frame Index, Source Node ID, Transfer Type, Data Type ID
|
||||
|
||||
/*
|
||||
* SFT broadcast
|
||||
*/
|
||||
can.id = CanFrame::FlagEFF |
|
||||
(2 << 0) | (1 << 3) | (0 << 4) | (42 << 10) | (uavcan::TransferTypeMessageBroadcast << 17) | (456 << 19);
|
||||
|
||||
ASSERT_TRUE(frame.parse(can));
|
||||
ASSERT_TRUE(frame.isFirst());
|
||||
ASSERT_TRUE(frame.isLast());
|
||||
ASSERT_EQ(0, frame.getIndex());
|
||||
ASSERT_EQ(NodeID(42), frame.getSrcNodeID());
|
||||
ASSERT_EQ(NodeID::Broadcast, frame.getDstNodeID());
|
||||
ASSERT_EQ(456, frame.getDataTypeID());
|
||||
ASSERT_EQ(TransferID(2), frame.getTransferID());
|
||||
ASSERT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType());
|
||||
ASSERT_EQ(sizeof(CanFrame::data), frame.getMaxPayloadLen());
|
||||
|
||||
/*
|
||||
* SFT addressed
|
||||
*/
|
||||
can.id = CanFrame::FlagEFF |
|
||||
(2 << 0) | (1 << 3) | (0 << 4) | (42 << 10) | (uavcan::TransferTypeMessageUnicast << 17) | (456 << 19);
|
||||
|
||||
ASSERT_FALSE(frame.parse(can)); // No payload - failure
|
||||
|
||||
can.dlc = 1;
|
||||
can.data[0] = 0xFF;
|
||||
ASSERT_FALSE(frame.parse(can)); // Invalid first byte - failure
|
||||
|
||||
can.data[0] = 127;
|
||||
ASSERT_TRUE(frame.parse(can));
|
||||
|
||||
ASSERT_TRUE(frame.isFirst());
|
||||
ASSERT_TRUE(frame.isLast());
|
||||
ASSERT_EQ(0, frame.getIndex());
|
||||
ASSERT_EQ(NodeID(42), frame.getSrcNodeID());
|
||||
ASSERT_EQ(NodeID(127), frame.getDstNodeID());
|
||||
ASSERT_EQ(456, frame.getDataTypeID());
|
||||
ASSERT_EQ(TransferID(2), frame.getTransferID());
|
||||
ASSERT_EQ(uavcan::TransferTypeMessageUnicast, frame.getTransferType());
|
||||
ASSERT_EQ(sizeof(CanFrame::data) - 1, frame.getMaxPayloadLen());
|
||||
|
||||
/*
|
||||
* MFT invalid - unterminated transfer
|
||||
*/
|
||||
can.id = CanFrame::FlagEFF |
|
||||
(2 << 0) | (0 << 3) | (Frame::MaxIndex << 4) | (42 << 10) |
|
||||
(uavcan::TransferTypeMessageUnicast << 17) | (456 << 19);
|
||||
|
||||
ASSERT_FALSE(frame.parse(can));
|
||||
|
||||
/*
|
||||
* MFT invalid - invalid frame index
|
||||
*/
|
||||
can.id = CanFrame::FlagEFF |
|
||||
(2 << 0) | (0 << 3) | (63 << 4) | (42 << 10) | (uavcan::TransferTypeMessageUnicast << 17) | (456 << 19);
|
||||
|
||||
ASSERT_FALSE(frame.parse(can));
|
||||
|
||||
/*
|
||||
* Malformed frames
|
||||
*/
|
||||
can.id = CanFrame::FlagEFF |
|
||||
(2 << 0) | (1 << 3) | (0 << 4) | (42 << 10) | (uavcan::TransferTypeMessageUnicast << 17) | (456 << 19);
|
||||
can.dlc = 3;
|
||||
can.data[0] = 42;
|
||||
ASSERT_FALSE(frame.parse(can)); // Src == Dst Node ID
|
||||
can.data[0] = 41;
|
||||
ASSERT_TRUE(frame.parse(can));
|
||||
|
||||
can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression
|
||||
(2 << 0) | (1 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageUnicast << 17) | (456 << 19);
|
||||
ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID
|
||||
}
|
||||
|
||||
|
||||
TEST(Transfer, RxFrameParse)
|
||||
{
|
||||
using uavcan::Frame;
|
||||
using uavcan::RxFrame;
|
||||
using uavcan::CanFrame;
|
||||
using uavcan::CanRxFrame;
|
||||
|
||||
CanRxFrame can_rx_frame;
|
||||
RxFrame rx_frame;
|
||||
|
||||
// Failure
|
||||
ASSERT_FALSE(rx_frame.parse(can_rx_frame));
|
||||
|
||||
// Valid
|
||||
can_rx_frame.id = CanFrame::FlagEFF |
|
||||
(2 << 0) | // Transfer ID
|
||||
(1 << 3) | // Last Frame
|
||||
(29 << 4) | // Frame Index
|
||||
(42 << 10) | // Source Node ID
|
||||
(uavcan::TransferTypeMessageBroadcast << 17) |
|
||||
(456 << 19); // Data Type ID
|
||||
|
||||
ASSERT_TRUE(rx_frame.parse(can_rx_frame));
|
||||
ASSERT_EQ(0, rx_frame.getMonotonicTimestamp().toUSec());
|
||||
ASSERT_EQ(0, rx_frame.getIfaceIndex());
|
||||
|
||||
can_rx_frame.ts_mono = tsMono(123);
|
||||
can_rx_frame.iface_index = 2;
|
||||
|
||||
Frame frame(456, uavcan::TransferTypeMessageBroadcast, 1, uavcan::NodeID::Broadcast, 0, 0);
|
||||
ASSERT_TRUE(frame.compile(can_rx_frame));
|
||||
|
||||
ASSERT_TRUE(rx_frame.parse(can_rx_frame));
|
||||
ASSERT_EQ(123, rx_frame.getMonotonicTimestamp().toUSec());
|
||||
ASSERT_EQ(2, rx_frame.getIfaceIndex());
|
||||
ASSERT_EQ(456, rx_frame.getDataTypeID());
|
||||
ASSERT_EQ(uavcan::TransferTypeMessageBroadcast, rx_frame.getTransferType());
|
||||
}
|
||||
|
||||
|
||||
TEST(Transfer, FrameToString)
|
||||
{
|
||||
using uavcan::Frame;
|
||||
using uavcan::RxFrame;
|
||||
|
||||
// RX frame default
|
||||
RxFrame rx_frame;
|
||||
EXPECT_EQ("dtid=0 tt=4 snid=255 dnid=255 idx=0 last=0 tid=0 payload=[] ts_m=0.000000 ts_utc=0.000000 iface=0", rx_frame.toString());
|
||||
|
||||
// RX frame max len
|
||||
rx_frame = RxFrame(Frame(Frame::MaxDataTypeID, uavcan::TransferTypeMessageUnicast,
|
||||
uavcan::NodeID::Max, uavcan::NodeID::Max - 1, Frame::MaxIndex,
|
||||
uavcan::TransferID::Max, true),
|
||||
uavcan::MonotonicTime::getMax(), uavcan::UtcTime::getMax(), 3);
|
||||
|
||||
uint8_t data[8];
|
||||
for (unsigned int i = 0; i < sizeof(data); i++)
|
||||
data[i] = i;
|
||||
rx_frame.setPayload(data, sizeof(data));
|
||||
|
||||
EXPECT_EQ(
|
||||
"dtid=1023 tt=3 snid=127 dnid=126 idx=62 last=1 tid=7 payload=[00 01 02 03 04 05 06] ts_m=18446744073709.551615 ts_utc=18446744073709.551615 iface=3",
|
||||
rx_frame.toString());
|
||||
|
||||
// Plain frame default
|
||||
Frame frame;
|
||||
EXPECT_EQ("dtid=0 tt=4 snid=255 dnid=255 idx=0 last=0 tid=0 payload=[]", frame.toString());
|
||||
|
||||
// Plain frame max len
|
||||
frame = rx_frame;
|
||||
EXPECT_EQ("dtid=1023 tt=3 snid=127 dnid=126 idx=62 last=1 tid=7 payload=[00 01 02 03 04 05 06]", frame.toString());
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user