mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 12:23:34 +08:00
470 lines
12 KiB
C++
470 lines
12 KiB
C++
/*
|
|
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
|
*/
|
|
|
|
#include <uavcan/transport/frame.hpp>
|
|
#include <uavcan/transport/can_io.hpp>
|
|
#include <uavcan/debug.hpp>
|
|
#include <cassert>
|
|
|
|
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)
|
|
{
|
|
UAVCAN_ASSERT(priority < NumTransferPriorities);
|
|
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())
|
|
{
|
|
case TransferTypeMessageBroadcast:
|
|
{
|
|
return int(sizeof(payload_));
|
|
}
|
|
case TransferTypeServiceResponse:
|
|
case TransferTypeServiceRequest:
|
|
case TransferTypeMessageUnicast:
|
|
{
|
|
return int(sizeof(payload_)) - 1;
|
|
}
|
|
default:
|
|
{
|
|
UAVCAN_ASSERT(0);
|
|
return -ErrLogic;
|
|
}
|
|
}
|
|
}
|
|
|
|
int Frame::setPayload(const uint8_t* data, unsigned len)
|
|
{
|
|
const int maxlen = getMaxPayloadLen();
|
|
if (maxlen < 0)
|
|
{
|
|
return maxlen;
|
|
}
|
|
len = min(unsigned(maxlen), len);
|
|
(void)copy(data, data + len, payload_);
|
|
payload_len_ = uint_fast8_t(len);
|
|
return int(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);
|
|
}
|
|
|
|
bool Frame::parse(const CanFrame& can_frame)
|
|
{
|
|
if (can_frame.isErrorFrame() || can_frame.isRemoteTransmissionRequest() || !can_frame.isExtended())
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (can_frame.dlc > sizeof(can_frame.data))
|
|
{
|
|
UAVCAN_ASSERT(0); // This is not a protocol error, so UAVCAN_ASSERT() is ok
|
|
return false;
|
|
}
|
|
|
|
/*
|
|
* CAN ID parsing
|
|
*/
|
|
const uint32_t id = can_frame.id & CanFrame::MaskExtID;
|
|
transfer_id_ = uint8_t(bitunpack<0, 3>(id));
|
|
last_frame_ = bitunpack<3, 1>(id) != 0;
|
|
// 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
|
|
*/
|
|
switch (transfer_type_)
|
|
{
|
|
case TransferTypeMessageBroadcast:
|
|
{
|
|
dst_node_id_ = NodeID::Broadcast;
|
|
payload_len_ = can_frame.dlc;
|
|
(void)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_ = uint8_t(can_frame.dlc - 1);
|
|
(void)copy(can_frame.data + 1, can_frame.data + can_frame.dlc, payload_);
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* 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);
|
|
}
|
|
|
|
bool Frame::compile(CanFrame& out_can_frame) const
|
|
{
|
|
if (!isValid())
|
|
{
|
|
UAVCAN_ASSERT(0); // This is an application error, so we need to maximize it.
|
|
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_) |
|
|
/* 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:
|
|
{
|
|
out_can_frame.dlc = uint8_t(payload_len_);
|
|
(void)copy(payload_, payload_ + payload_len_, out_can_frame.data);
|
|
break;
|
|
}
|
|
case TransferTypeServiceResponse:
|
|
case TransferTypeServiceRequest:
|
|
case TransferTypeMessageUnicast:
|
|
{
|
|
UAVCAN_ASSERT((payload_len_ + 1U) <= sizeof(out_can_frame.data));
|
|
out_can_frame.data[0] = dst_node_id_.get();
|
|
out_can_frame.dlc = uint8_t(payload_len_ + 1);
|
|
(void)copy(payload_, payload_ + payload_len_, out_can_frame.data + 1);
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
UAVCAN_ASSERT(0);
|
|
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
|
|
{
|
|
/*
|
|
* Frame index
|
|
*/
|
|
if (frame_index_ > getMaxIndex())
|
|
{
|
|
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
|
return false;
|
|
}
|
|
|
|
if ((frame_index_ == getMaxIndex()) && !last_frame_)
|
|
{
|
|
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
|
return false;
|
|
}
|
|
|
|
/*
|
|
* Node ID
|
|
*/
|
|
if (!src_node_id_.isValid() ||
|
|
!dst_node_id_.isValid())
|
|
{
|
|
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
|
return false;
|
|
}
|
|
|
|
if (src_node_id_.isUnicast() &&
|
|
(src_node_id_ == dst_node_id_))
|
|
{
|
|
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
|
return false;
|
|
}
|
|
|
|
/*
|
|
* Transfer type
|
|
*/
|
|
if (transfer_type_ >= NumTransferTypes)
|
|
{
|
|
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
|
return false;
|
|
}
|
|
|
|
if ((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast())
|
|
{
|
|
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
|
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())
|
|
{
|
|
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
|
return false;
|
|
}
|
|
|
|
/*
|
|
* Data type ID
|
|
*/
|
|
if (!data_type_id_.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type_)))
|
|
{
|
|
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
|
return false;
|
|
}
|
|
|
|
/*
|
|
* Priority
|
|
*/
|
|
if (transfer_priority_ >= NumTransferPriorities)
|
|
{
|
|
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
|
return false;
|
|
}
|
|
|
|
if (transfer_type_ == TransferTypeServiceRequest ||
|
|
transfer_type_ == TransferTypeServiceResponse)
|
|
{
|
|
if (transfer_priority_ != TransferPriorityService)
|
|
{
|
|
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool Frame::operator==(const Frame& rhs) const
|
|
{
|
|
return
|
|
(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_);
|
|
}
|
|
|
|
#if UAVCAN_TOSTRING
|
|
std::string Frame::toString() const
|
|
{
|
|
/*
|
|
* - Priority
|
|
* - Data Type ID
|
|
* - Transfer Type
|
|
* - Source Node ID
|
|
* - Frame Index
|
|
* - Last Frame
|
|
* - Transfer ID
|
|
*/
|
|
static const int BUFLEN = 100;
|
|
char buf[BUFLEN];
|
|
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++)
|
|
{
|
|
ofs += snprintf(buf + ofs, unsigned(BUFLEN - ofs), "%02x", payload_[i]);
|
|
if ((i + 1) < payload_len_)
|
|
{
|
|
ofs += snprintf(buf + ofs, unsigned(BUFLEN - ofs), " ");
|
|
}
|
|
}
|
|
(void)snprintf(buf + ofs, unsigned(BUFLEN - ofs), "]");
|
|
return std::string(buf);
|
|
}
|
|
#endif
|
|
|
|
/**
|
|
* RxFrame
|
|
*/
|
|
bool RxFrame::parse(const CanRxFrame& can_frame)
|
|
{
|
|
if (!Frame::parse(can_frame))
|
|
{
|
|
return false;
|
|
}
|
|
if (can_frame.ts_mono.isZero()) // Monotonic timestamps are mandatory.
|
|
{
|
|
UAVCAN_ASSERT(0); // If it is not set, it's a driver failure.
|
|
return false;
|
|
}
|
|
ts_mono_ = can_frame.ts_mono;
|
|
ts_utc_ = can_frame.ts_utc;
|
|
iface_index_ = can_frame.iface_index;
|
|
return true;
|
|
}
|
|
|
|
#if UAVCAN_TOSTRING
|
|
std::string RxFrame::toString() const
|
|
{
|
|
std::string out = Frame::toString();
|
|
out.reserve(128);
|
|
out += " ts_m=" + ts_mono_.toString();
|
|
out += " ts_utc=" + ts_utc_.toString();
|
|
out += " iface=";
|
|
out += char('0' + iface_index_);
|
|
return out;
|
|
}
|
|
#endif
|
|
|
|
}
|