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

Basic support for frame prioritization; tests are failing

Browse Source
This commit is contained in:
Pavel Kirienko 2015-04-30 08:26:25 +03:00
parent eafcfa1733
commit 2843c0a35e
7 changed files with 288 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libuavcan/include/uavcan/transport/frame.hpp
View File

@ -17,6 +17,7 @@ namespace uavcan
class UAVCAN_EXPORT Frame
{
uint8_t payload_[sizeof(CanFrame::data)];
TransferPriority transfer_priority_;
TransferType transfer_type_;
DataTypeID data_type_id_;
uint_fast8_t payload_len_;
@ -27,10 +28,12 @@ class UAVCAN_EXPORT Frame
bool last_frame_;
public:
enum { MaxIndex = 62 }; // 63 (or 0b111111) is reserved
static const uint8_t MaxIndexForService = 62; // 63 is reserved
static const uint8_t MaxIndexForMessage = 15;
Frame()
: transfer_type_(TransferType(NumTransferTypes)) // That is invalid value
: transfer_priority_(TransferPriority(NumTransferPriorities)) // Invalid value
, transfer_type_(TransferType(NumTransferTypes)) // Invalid value
, payload_len_(0)
, frame_index_(0)
, transfer_id_(0)
@ -39,7 +42,8 @@ public:
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)
: transfer_priority_(getDefaultPriorityForTransferType(transfer_type))
, transfer_type_(transfer_type)
, data_type_id_(data_type_id)
, payload_len_(0)
, src_node_id_(src_node_id)
@ -51,9 +55,21 @@ public:
UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast());
UAVCAN_ASSERT(data_type_id.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type)));
UAVCAN_ASSERT(src_node_id.isUnicast() ? (src_node_id != dst_node_id) : true);
UAVCAN_ASSERT(frame_index <= MaxIndex);
UAVCAN_ASSERT(frame_index <= getMaxIndex());
}
static uint_fast8_t getMaxIndexForTransferType(const TransferType type);
uint_fast8_t getMaxIndex() const { return getMaxIndexForTransferType(transfer_type_); }
/**
* Priority can be set only for message transfers.
* Attempt to set priority of a service transfer will cause assertion failure in debug build; in release build
* it will be ignored.
*/
void setPriority(TransferPriority priority);
TransferPriority getPriority() const { return transfer_priority_; }
/**
* Max payload length depends on the transfer type and frame index.
*/

27
libuavcan/include/uavcan/transport/transfer.hpp
View File

@ -53,6 +53,33 @@ static inline unsigned getMaxPayloadLenForTransferType(const TransferType type)
}
enum TransferPriority
{
TransferPriorityHigh = 0,
TransferPriorityNormal = 1,
TransferPriorityService = 2,
TransferPriorityLow = 3,
NumTransferPriorities = 4
};
static inline TransferPriority getDefaultPriorityForTransferType(const TransferType type)
{
if (type == TransferTypeServiceResponse || type == TransferTypeServiceRequest)
{
return TransferPriorityService;
}
else if (type == TransferTypeMessageBroadcast || type == TransferTypeMessageUnicast)
{
return TransferPriorityNormal;
}
else
{
UAVCAN_ASSERT(0);
return TransferPriority(0); // whatever
}
}
class UAVCAN_EXPORT TransferID
{
uint8_t value_;

260
libuavcan/src/transport/uc_frame.cpp
View File

@ -11,6 +11,48 @@ namespace uavcan
/**
* Frame
*/
const uint8_t Frame::MaxIndexForService;
const uint8_t Frame::MaxIndexForMessage;
uint_fast8_t Frame::getMaxIndexForTransferType(const TransferType type)
{
if (type == TransferTypeMessageBroadcast ||
type == TransferTypeMessageUnicast)
{
return MaxIndexForMessage;
}
else if (type == TransferTypeServiceRequest ||
type == TransferTypeServiceResponse)
{
return MaxIndexForService;
}
else
{
UAVCAN_ASSERT(0);
return 0;
}
}
void Frame::setPriority(const TransferPriority priority)
{
if (transfer_type_ == TransferTypeMessageBroadcast ||
transfer_type_ == TransferTypeMessageUnicast)
{
if (priority != TransferPriorityService)
{
transfer_priority_ = priority;
}
else
{
UAVCAN_ASSERT(0);
}
}
else
{
UAVCAN_ASSERT(0);
}
}
int Frame::getMaxPayloadLen() const
{
switch (getTransferType())
@ -49,6 +91,9 @@ int Frame::setPayload(const uint8_t* data, unsigned len)
template <int OFFSET, int WIDTH>
inline static uint32_t bitunpack(uint32_t val)
{
StaticAssert<(OFFSET >= 0)>::check();
StaticAssert<(WIDTH > 0)>::check();
StaticAssert<((OFFSET + WIDTH) <= 29)>::check();
return (val >> OFFSET) & ((1UL << WIDTH) - 1);
}
@ -71,10 +116,25 @@ bool Frame::parse(const CanFrame& can_frame)
const uint32_t id = can_frame.id & CanFrame::MaskExtID;
transfer_id_ = uint8_t(bitunpack<0, 3>(id));
last_frame_ = bitunpack<3, 1>(id) != 0;
frame_index_ = uint8_t(bitunpack<4, 6>(id));
src_node_id_ = uint8_t(bitunpack<10, 7>(id));
transfer_type_ = TransferType(bitunpack<17, 2>(id));
data_type_id_ = uint16_t(bitunpack<19, 10>(id));
// TT-specific fields skipped
transfer_priority_ = TransferPriority(bitunpack<27, 2>(id));
if (transfer_priority_ == TransferPriorityService)
{
frame_index_ = uint_fast8_t(bitunpack<4, 6>(id));
src_node_id_ = uint8_t(bitunpack<10, 7>(id));
data_type_id_ = uint16_t(bitunpack<17, 9>(id));
// RequestNotResponse
transfer_type_ = (bitunpack<26, 1>(id) == 1U) ? TransferTypeServiceRequest : TransferTypeServiceResponse;
}
else
{
frame_index_ = uint_fast8_t(bitunpack<4, 4>(id));
// BroadcastNotUnicast
transfer_type_ = (bitunpack<8, 1>(id) == 1U) ? TransferTypeMessageBroadcast : TransferTypeMessageUnicast;
src_node_id_ = uint8_t(bitunpack<9, 7>(id));
data_type_id_ = uint16_t(bitunpack<16, 11>(id));
}
/*
* CAN payload parsing
@ -111,12 +171,29 @@ bool Frame::parse(const CanFrame& can_frame)
}
}
/*
* Special case for anonymous transfers - trailing 8 bits of CAN ID must be ignored
* - Transfer ID (assumed zero)
* - Last Frame (assumed true)
* - Frame Index (assumed zero)
*/
if (src_node_id_.isBroadcast())
{
transfer_id_ = TransferID(0);
last_frame_ = true;
frame_index_ = 0;
}
return isValid();
}
template <int OFFSET, int WIDTH>
inline static uint32_t bitpack(uint32_t field)
{
StaticAssert<(OFFSET >= 0)>::check();
StaticAssert<(WIDTH > 0)>::check();
StaticAssert<((OFFSET + WIDTH) <= 29)>::check();
UAVCAN_ASSERT((field & ((1UL << WIDTH) - 1)) == field);
return uint32_t((field & ((1UL << WIDTH) - 1)) << OFFSET);
}
@ -128,15 +205,44 @@ bool Frame::compile(CanFrame& out_can_frame) const
return false;
}
/*
* Setting CAN ID field
*/
// Common fields for messages and services
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_.get());
/* TT-specific fields skipped */
bitpack<27, 2>(transfer_priority_);
if (transfer_type_ == TransferTypeServiceRequest ||
transfer_type_ == TransferTypeServiceResponse)
{
out_can_frame.id |=
bitpack<4, 6>(frame_index_) |
bitpack<10, 7>(src_node_id_.get()) |
bitpack<17, 9>(data_type_id_.get()) |
bitpack<26, 1>((transfer_type_ == TransferTypeServiceRequest) ? 1U : 0U);
}
else if (transfer_type_ == TransferTypeMessageBroadcast ||
transfer_type_ == TransferTypeMessageUnicast)
{
out_can_frame.id |=
bitpack<4, 4>(frame_index_) |
bitpack<8, 1>((transfer_type_ == TransferTypeMessageBroadcast) ? 1U : 0U) |
bitpack<9, 7>(src_node_id_.get()) |
bitpack<16, 11>(data_type_id_.get());
}
else
{
UAVCAN_ASSERT(0);
return false;
}
/*
* Setting payload
*/
switch (transfer_type_)
{
case TransferTypeMessageBroadcast:
@ -161,40 +267,126 @@ bool Frame::compile(CanFrame& out_can_frame) const
return false;
}
}
/*
* Setting trailing bits of CAN ID for anonymous message
* This overrides the following fields:
* - Transfer ID (assumed zero)
* - Last Frame (assumed true)
* - Frame Index (assumed zero)
*/
if (src_node_id_.isBroadcast())
{
uint8_t sum = 0;
out_can_frame.id &= ~bitpack<0, 8>(sum); // Clearing bits
for (uint_fast8_t i = 0; i < payload_len_; i++)
{
sum = static_cast<uint8_t>(sum + payload_[i]);
}
out_can_frame.id &= ~bitpack<0, 8>(sum); // Setting the checksum
}
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_.isValid()) ||
(!dst_node_id_.isValid()) ||
(src_node_id_.isUnicast() ? (src_node_id_ == dst_node_id_) : false) ||
(src_node_id_.isBroadcast()
? (!last_frame_ || (frame_index_ > 0) || (transfer_type_ != TransferTypeMessageBroadcast))
: false) ||
((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast()) ||
(transfer_type_ >= NumTransferTypes) ||
(static_cast<int>(payload_len_) > getMaxPayloadLen()) ||
(!data_type_id_.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type_)));
/*
* Frame index
*/
if (frame_index_ > getMaxIndex())
{
return false;
}
return !invalid;
if ((frame_index_ == getMaxIndex()) && !last_frame_)
{
return false;
}
/*
* Node ID
*/
if (!src_node_id_.isValid() ||
!dst_node_id_.isValid())
{
return false;
}
if (src_node_id_.isUnicast() &&
(src_node_id_ == dst_node_id_))
{
return false;
}
/*
* Transfer type
*/
if (transfer_type_ >= NumTransferTypes)
{
return false;
}
if ((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast())
{
return false;
}
// Anonymous transfers
if (src_node_id_.isBroadcast() &&
(!last_frame_ || (frame_index_ > 0) || (transfer_type_ != TransferTypeMessageBroadcast)))
{
return false;
}
/*
* Payload
*/
if (static_cast<int>(payload_len_) > getMaxPayloadLen())
{
return false;
}
/*
* Data type ID
*/
if (!data_type_id_.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type_)))
{
return false;
}
/*
* Priority
*/
if (transfer_priority_ >= NumTransferPriorities)
{
return false;
}
if (transfer_type_ == TransferTypeServiceRequest ||
transfer_type_ == TransferTypeServiceResponse)
{
if (transfer_priority_ != TransferPriorityService)
{
return false;
}
}
return true;
}
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_) &&
(transfer_priority_ == rhs.transfer_priority_) &&
(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_) &&
equal(payload_, payload_ + payload_len_, rhs.payload_);
}
@ -202,7 +394,7 @@ bool Frame::operator==(const Frame& rhs) const
std::string Frame::toString() const
{
/*
* Frame ID fields, according to UAVCAN specs:
* - Priority
* - Data Type ID
* - Transfer Type
* - Source Node ID
@ -212,8 +404,8 @@ std::string Frame::toString() const
*/
static const int BUFLEN = 100;
char buf[BUFLEN];
int ofs = snprintf(buf, BUFLEN, "dtid=%i tt=%i snid=%i dnid=%i idx=%i last=%i tid=%i payload=[",
int(data_type_id_.get()), int(transfer_type_), int(src_node_id_.get()),
int ofs = snprintf(buf, BUFLEN, "prio=%d dtid=%d tt=%d snid=%d dnid=%d idx=%d last=%d tid=%d payload=[",
int(transfer_priority_), int(data_type_id_.get()), int(transfer_type_), int(src_node_id_.get()),
int(dst_node_id_.get()), int(frame_index_), int(last_frame_), int(transfer_id_.get()));
for (unsigned i = 0; i < payload_len_; i++)

2
libuavcan/src/transport/uc_transfer_receiver.cpp
View File

@ -77,7 +77,7 @@ bool TransferReceiver::validate(const RxFrame& frame) const
registerError();
return false;
}
if ((frame.getIndex() == Frame::MaxIndex) && !frame.isLast())
if ((frame.getIndex() == frame.getMaxIndex()) && !frame.isLast())
{
UAVCAN_TRACE("TransferReceiver", "Unterminated transfer, %s", frame.toString().c_str());
registerError();

15
libuavcan/test/transport/frame.cpp
View File

@ -136,7 +136,8 @@ TEST(Frame, FrameParsing)
* MFT invalid - unterminated transfer
*/
can.id = CanFrame::FlagEFF |
(2 << 0) | (0 << 3) | (Frame::MaxIndex << 4) | (42 << 10) |
(2 << 0) | (0 << 3) |
(unsigned(Frame::getMaxIndexForTransferType(uavcan::TransferTypeMessageUnicast)) << 4) | (42 << 10) |
(uavcan::TransferTypeMessageUnicast << 17) | (456 << 19);
ASSERT_FALSE(frame.parse(can));
@ -226,12 +227,13 @@ TEST(Frame, FrameToString)
// RX frame default
RxFrame rx_frame;
EXPECT_EQ("dtid=65535 tt=4 snid=255 dnid=255 idx=0 last=0 tid=0 payload=[] ts_m=0.000000 ts_utc=0.000000 iface=0",
EXPECT_EQ("prio=4 dtid=65535 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::DataTypeID::MaxPossibleDataTypeIDValue, uavcan::TransferTypeMessageUnicast,
uavcan::NodeID::Max, uavcan::NodeID::Max - 1, Frame::MaxIndex,
uavcan::NodeID::Max, uavcan::NodeID::Max - 1,
Frame::getMaxIndexForTransferType(uavcan::TransferTypeMessageUnicast),
uavcan::TransferID::Max, true),
uavcan::MonotonicTime::getMax(), uavcan::UtcTime::getMax(), 3);
@ -242,15 +244,16 @@ TEST(Frame, FrameToString)
}
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] "
EXPECT_EQ("prio=1 dtid=2047 tt=3 snid=127 dnid=126 idx=15 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=65535 tt=4 snid=255 dnid=255 idx=0 last=0 tid=0 payload=[]", frame.toString());
EXPECT_EQ("prio=4 dtid=65535 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());
EXPECT_EQ("prio=1 dtid=2047 tt=3 snid=127 dnid=126 idx=15 last=1 tid=7 payload=[00 01 02 03 04 05 06]",
frame.toString());
}

6
libuavcan/test/transport/transfer_receiver.cpp
View File

@ -260,13 +260,15 @@ TEST(TransferReceiver, UnterminatedTransfer)
uavcan::ITransferBufferManager& bufmgr = context.bufmgr;
uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY);
const uint8_t MaxIndex = uavcan::Frame::getMaxIndexForTransferType(RxFrameGenerator::DEFAULT_KEY.getTransferType());
std::string content;
for (uint8_t i = 0; i <= uavcan::Frame::MaxIndex; i++)
for (uint8_t i = 0; i <= MaxIndex; i++)
{
CHECK_NOT_COMPLETE(rcv.addFrame(gen(1, "12345678", i, false, 0, 1000U + i), bk)); // Last one will be dropped
content += "12345678";
}
CHECK_COMPLETE(rcv.addFrame(gen(1, "12345678", uavcan::Frame::MaxIndex, true, 0, 1100), bk));
CHECK_COMPLETE(rcv.addFrame(gen(1, "12345678", MaxIndex, true, 0, 1100), bk));
ASSERT_EQ(1000, rcv.getLastTransferTimestampMonotonic().toUSec());
ASSERT_TRUE(matchBufferContent(bufmgr.access(gen.bufmgr_key), std::string(content, 2)));
ASSERT_EQ(0x3231, rcv.getLastTransferCrc());

2
libuavcan/test/transport/transfer_test_helpers.hpp
View File

@ -226,7 +226,7 @@ std::vector<uavcan::RxFrame> serializeTransfer(const Transfer& transfer)
}
offset += unsigned(spres);
EXPECT_GE(uavcan::Frame::MaxIndex, frame_index);
EXPECT_GE(uavcan::Frame::getMaxIndexForTransferType(transfer.transfer_type), frame_index);
frame_index++;
const uavcan::RxFrame rxfrm(frm, ts_monotonic, ts_utc, 0);