mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-26 04:43:34 +08:00
Basic modifications; the build is terribly broken
This commit is contained in:
Pavel Kirienko
@@ -4,6 +4,7 @@
|
||||
|
||||
#include <uavcan/transport/frame.hpp>
|
||||
#include <uavcan/transport/can_io.hpp>
|
||||
#include <uavcan/transport/crc.hpp>
|
||||
#include <uavcan/debug.hpp>
|
||||
#include <cassert>
|
||||
|
||||
@@ -12,78 +13,9 @@ 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;
|
||||
}
|
||||
const uint8_t maxlen = getPayloadCapacity();
|
||||
len = min(unsigned(maxlen), len);
|
||||
(void)copy(data, data + len, payload_);
|
||||
payload_len_ = uint_fast8_t(len);
|
||||
@@ -112,79 +44,55 @@ bool Frame::parse(const CanFrame& can_frame)
|
||||
return false;
|
||||
}
|
||||
|
||||
if (can_frame.dlc < 1)
|
||||
{
|
||||
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)
|
||||
transfer_priority_ = static_cast<uint8_t>(bitunpack<24, 5>(id));
|
||||
src_node_id_ = static_cast<uint8_t>(bitunpack<0, 7>(id));
|
||||
|
||||
const bool service_not_message = bitunpack<7, 1>(id) != 0U;
|
||||
if (service_not_message)
|
||||
{
|
||||
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;
|
||||
const bool request_not_response = bitunpack<15, 1>(id) != 0U;
|
||||
transfer_type_ = request_not_response ? TransferTypeServiceRequest : TransferTypeServiceResponse;
|
||||
|
||||
dst_node_id_ = static_cast<uint8_t>(bitunpack<8, 7>(id));
|
||||
data_type_id_ = static_cast<uint16_t>(bitunpack<16, 8>(id));
|
||||
}
|
||||
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));
|
||||
transfer_type_ = TransferTypeMessageBroadcast;
|
||||
dst_node_id_ = NodeID::Broadcast;
|
||||
|
||||
data_type_id_ = static_cast<uint16_t>(bitunpack<8, 16>(id));
|
||||
|
||||
if (src_node_id_.isBroadcast())
|
||||
{
|
||||
// Removing the discriminator
|
||||
data_type_id_ = static_cast<uint16_t>(data_type_id_.get() & 3U);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* 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;
|
||||
}
|
||||
}
|
||||
payload_len_ = static_cast<uint8_t>(can_frame.dlc - 1U);
|
||||
(void)copy(can_frame.data, can_frame.data + payload_len_, payload_);
|
||||
|
||||
/*
|
||||
* 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;
|
||||
}
|
||||
const uint8_t tail = can_frame.data[can_frame.dlc - 1U];
|
||||
|
||||
start_of_transfer_ = (tail & (1U << 7)) != 0;
|
||||
end_of_transfer_ = (tail & (1U << 6)) != 0;
|
||||
toggle_ = (tail & (1U << 5)) != 0;
|
||||
|
||||
transfer_id_ = tail & TransferID::Max;
|
||||
|
||||
return isValid();
|
||||
}
|
||||
@@ -208,84 +116,58 @@ bool Frame::compile(CanFrame& out_can_frame) const
|
||||
}
|
||||
|
||||
/*
|
||||
* Setting CAN ID field
|
||||
* 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_);
|
||||
out_can_frame.id = CanFrame::FlagEFF |
|
||||
bitpack<0, 7>(src_node_id_.get()) |
|
||||
bitpack<24, 5>(transfer_priority_.get());
|
||||
|
||||
if (transfer_type_ == TransferTypeServiceRequest ||
|
||||
transfer_type_ == TransferTypeServiceResponse)
|
||||
if (transfer_type_ == TransferTypeMessageBroadcast)
|
||||
{
|
||||
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());
|
||||
bitpack<7, 1>(0U) |
|
||||
bitpack<8, 16>(data_type_id_.get());
|
||||
}
|
||||
else
|
||||
{
|
||||
UAVCAN_ASSERT(0);
|
||||
return false;
|
||||
const bool request_not_response = transfer_type_ == TransferTypeServiceRequest;
|
||||
out_can_frame.id |=
|
||||
bitpack<7, 1>(1U) |
|
||||
bitpack<8, 7>(dst_node_id_.get()) |
|
||||
bitpack<15, 1>(request_not_response ? 1U : 0U) |
|
||||
bitpack<16, 8>(data_type_id_.get());
|
||||
}
|
||||
|
||||
/*
|
||||
* Setting payload
|
||||
* Payload
|
||||
*/
|
||||
switch (transfer_type_)
|
||||
uint8_t tail = transfer_id_.get();
|
||||
if (start_of_transfer_)
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
{
|
||||
out_can_frame.dlc = uint8_t(payload_len_);
|
||||
(void)copy(payload_, payload_ + payload_len_, out_can_frame.data);
|
||||
break;
|
||||
tail |= (1U << 7);
|
||||
}
|
||||
case TransferTypeServiceResponse:
|
||||
case TransferTypeServiceRequest:
|
||||
case TransferTypeMessageUnicast:
|
||||
if (end_of_transfer_)
|
||||
{
|
||||
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;
|
||||
tail |= (1U << 6);
|
||||
}
|
||||
default:
|
||||
if (toggle_)
|
||||
{
|
||||
UAVCAN_ASSERT(0);
|
||||
return false;
|
||||
}
|
||||
tail |= (1U << 5);
|
||||
}
|
||||
|
||||
out_can_frame.dlc = static_cast<uint8_t>(payload_len_);
|
||||
(void)copy(payload_, payload_ + payload_len_, out_can_frame.data);
|
||||
|
||||
out_can_frame.data[out_can_frame.dlc] = tail;
|
||||
|
||||
/*
|
||||
* 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)
|
||||
* Discriminator
|
||||
*/
|
||||
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
|
||||
TransferCRC crc;
|
||||
crc.add(out_can_frame.data, out_can_frame.dlc);
|
||||
out_can_frame.id |= bitpack<10, 14>(crc.get() & ((1U << 14) - 1U));
|
||||
}
|
||||
|
||||
return true;
|
||||
@@ -294,15 +176,9 @@ bool Frame::compile(CanFrame& out_can_frame) const
|
||||
bool Frame::isValid() const
|
||||
{
|
||||
/*
|
||||
* Frame index
|
||||
* Toggle
|
||||
*/
|
||||
if (frame_index_ > getMaxIndex())
|
||||
{
|
||||
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
||||
return false;
|
||||
}
|
||||
|
||||
if ((frame_index_ == getMaxIndex()) && !last_frame_)
|
||||
if (start_of_transfer_ && toggle_)
|
||||
{
|
||||
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
||||
return false;
|
||||
@@ -311,15 +187,13 @@ bool Frame::isValid() const
|
||||
/*
|
||||
* Node ID
|
||||
*/
|
||||
if (!src_node_id_.isValid() ||
|
||||
!dst_node_id_.isValid())
|
||||
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_))
|
||||
if (src_node_id_.isUnicast() && (src_node_id_ == dst_node_id_))
|
||||
{
|
||||
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
||||
return false;
|
||||
@@ -342,7 +216,7 @@ bool Frame::isValid() const
|
||||
|
||||
// Anonymous transfers
|
||||
if (src_node_id_.isBroadcast() &&
|
||||
(!last_frame_ || (frame_index_ > 0) || (transfer_type_ != TransferTypeMessageBroadcast)))
|
||||
(!start_of_transfer_ || !end_of_transfer_ || (transfer_type_ != TransferTypeMessageBroadcast)))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
@@ -350,7 +224,7 @@ bool Frame::isValid() const
|
||||
/*
|
||||
* Payload
|
||||
*/
|
||||
if (static_cast<int>(payload_len_) > getMaxPayloadLen())
|
||||
if (payload_len_ > getPayloadCapacity())
|
||||
{
|
||||
UAVCAN_TRACE("Frame", "Validness check failed at line %d", __LINE__);
|
||||
return false;
|
||||
@@ -368,22 +242,12 @@ bool Frame::isValid() const
|
||||
/*
|
||||
* Priority
|
||||
*/
|
||||
if (transfer_priority_ >= NumTransferPriorities)
|
||||
if (!transfer_priority_.isValid())
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -395,9 +259,10 @@ bool Frame::operator==(const Frame& rhs) const
|
||||
(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_) &&
|
||||
(toggle_ == rhs.toggle_) &&
|
||||
(start_of_transfer_ == rhs.start_of_transfer_) &&
|
||||
(end_of_transfer_ == rhs.end_of_transfer_) &&
|
||||
(payload_len_ == rhs.payload_len_) &&
|
||||
equal(payload_, payload_ + payload_len_, rhs.payload_);
|
||||
}
|
||||
@@ -405,20 +270,12 @@ bool Frame::operator==(const Frame& rhs) const
|
||||
#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()));
|
||||
int ofs = snprintf(buf, BUFLEN, "prio=%d dtid=%d tt=%d snid=%d dnid=%d sot=%d eot=%d togl=%d tid=%d payload=[",
|
||||
int(transfer_priority_.get()), int(data_type_id_.get()), int(transfer_type_),
|
||||
int(src_node_id_.get()), int(dst_node_id_.get()),
|
||||
int(start_of_transfer_), int(end_of_transfer_), int(toggle_), int(transfer_id_.get()));
|
||||
|
||||
for (unsigned i = 0; i < payload_len_; i++)
|
||||
{
|
||||
|
||||
@@ -8,6 +8,12 @@
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
/**
|
||||
* TransferPriority
|
||||
*/
|
||||
const uint8_t TransferPriority::BitLen;
|
||||
const TransferPriority TransferPriority::Default((1U << BitLen) / 2);
|
||||
|
||||
/**
|
||||
* TransferID
|
||||
*/
|
||||
|
||||
Reference in New Issue
Block a user