fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 03:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Basic modifications; the build is terribly broken

Browse Source
This commit is contained in:
Pavel Kirienko
2015-06-29 19:01:33 +03:00
parent 7502a451e4
commit 350761fa7a
5 changed files with 150 additions and 325 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/src/transport/uc_frame.cpp
+80 -223
View File
@@ -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++)
{
libuavcan/src/transport/uc_transfer.cpp
+6
View File
@@ -8,6 +8,12 @@
namespace uavcan
{
/**
* TransferPriority
*/
const uint8_t TransferPriority::BitLen;
const TransferPriority TransferPriority::Default((1U << BitLen) / 2);
/**
* TransferID
*/