fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 07:23:33 +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/include/uavcan/data_type.hpp
+7 -8
View File
@@ -31,8 +31,7 @@ static inline DataTypeKind getDataTypeKindForTransferType(const TransferType tt)
{
return DataTypeKindService;
}
else if (tt == TransferTypeMessageBroadcast ||
tt == TransferTypeMessageUnicast)
else if (tt == TransferTypeMessageBroadcast)
{
return DataTypeKindMessage;
}
@@ -46,19 +45,19 @@ static inline DataTypeKind getDataTypeKindForTransferType(const TransferType tt)
class UAVCAN_EXPORT DataTypeID
{
uint16_t value_;
uint32_t value_;
public:
static const uint16_t MaxServiceDataTypeIDValue = 511;
static const uint16_t MaxMessageDataTypeIDValue = 2047;
static const uint16_t MaxServiceDataTypeIDValue = 255;
static const uint16_t MaxMessageDataTypeIDValue = 65535;
static const uint16_t MaxPossibleDataTypeIDValue = MaxMessageDataTypeIDValue;
DataTypeID() : value_(0xFFFF) { }
DataTypeID() : value_(0xFFFFFFFFUL) { }
DataTypeID(uint16_t id) // Implicit
: value_(id)
{
UAVCAN_ASSERT(id < 0xFFFF);
UAVCAN_ASSERT(id <= MaxPossibleDataTypeIDValue);
}
static DataTypeID getMaxValueForDataTypeKind(const DataTypeKind dtkind);
@@ -68,7 +67,7 @@ public:
return value_ <= getMaxValueForDataTypeKind(dtkind).get();
}
uint16_t get() const { return value_; }
uint16_t get() const { return static_cast<uint16_t>(value_); }
bool operator==(DataTypeID rhs) const { return value_ == rhs.value_; }
bool operator!=(DataTypeID rhs) const { return value_ != rhs.value_; }
libuavcan/include/uavcan/transport/frame.hpp
+36 -41
View File
@@ -16,64 +16,57 @@ namespace uavcan
class UAVCAN_EXPORT Frame
{
uint8_t payload_[sizeof(CanFrame::data)];
enum { PayloadCapacity = 7 }; // Will be redefined when CAN FD is available
uint8_t payload_[PayloadCapacity];
TransferPriority transfer_priority_;
TransferType transfer_type_;
DataTypeID 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_;
bool start_of_transfer_;
bool end_of_transfer_;
bool toggle_;
public:
static const uint8_t MaxIndexForService = 62; // 63 is reserved
static const uint8_t MaxIndexForMessage = 15;
Frame()
: transfer_priority_(TransferPriority(NumTransferPriorities)) // Invalid value
, transfer_type_(TransferType(NumTransferTypes)) // Invalid value
, payload_len_(0)
, frame_index_(0)
, transfer_id_(0)
, last_frame_(false)
Frame() :
transfer_type_(TransferType(NumTransferTypes)), // Invalid value
payload_len_(0),
start_of_transfer_(false),
end_of_transfer_(false),
toggle_(false)
{ }
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_priority_(getDefaultPriorityForTransferType(transfer_type))
, 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)
Frame(DataTypeID data_type_id,
TransferType transfer_type,
NodeID src_node_id,
NodeID dst_node_id,
TransferID transfer_id) :
transfer_priority_(TransferPriority::Default),
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),
transfer_id_(transfer_id),
start_of_transfer_(false),
end_of_transfer_(false),
toggle_(false)
{
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 <= 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);
void setPriority(TransferPriority priority) { transfer_priority_ = priority; }
TransferPriority getPriority() const { return transfer_priority_; }
/**
* Max payload length depends on the transfer type and frame index.
*/
int getMaxPayloadLen() const;
uint8_t getPayloadCapacity() const { return PayloadCapacity; }
int setPayload(const uint8_t* data, unsigned len);
unsigned getPayloadLen() const { return payload_len_; }
@@ -84,13 +77,15 @@ public:
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(int index) { frame_index_ = uint_fast8_t(index); }
void setStartOfTransfer(bool x) { start_of_transfer_ = x; }
void setEndOfTransfer(bool x) { end_of_transfer_ = x; }
bool isFirst() const { return frame_index_ == 0; }
bool isStartOfTransfer() const { return start_of_transfer_; }
bool isEndOfTransfer() const { return end_of_transfer_; }
void flipToggle() { toggle_ = !toggle_; }
bool getToggle() const { return toggle_; }
bool parse(const CanFrame& can_frame);
bool compile(CanFrame& can_frame) const;
libuavcan/include/uavcan/transport/transfer.hpp
+21 -53
View File
@@ -11,81 +11,49 @@
namespace uavcan
{
/**
* Refer to the UAVCAN specification for more info about transfers.
*/
static const unsigned MaxMessageBroadcastTransferPayloadLen = 126; ///< 16 frames, 8 bytes per frame, 2 byte CRC
static const unsigned MaxMessageUnicastTransferPayloadLen = 110; ///< 16 frames, 7 bytes per frame, 2 byte CRC
static const unsigned MaxServiceTransferPayloadLen = 439; ///< 63 frames, 7 bytes per frame, 2 byte CRC
static const unsigned GuaranteedPayloadLenPerFrame = 7; ///< Guaranteed for all transfers, all CAN standards
static const unsigned MaxPossibleTransferPayloadLen = MaxServiceTransferPayloadLen;
enum TransferType
{
TransferTypeServiceResponse = 0,
TransferTypeServiceRequest = 1,
TransferTypeMessageBroadcast = 2,
TransferTypeMessageUnicast = 3,
NumTransferTypes = 4
NumTransferTypes = 3
};
static inline unsigned getMaxPayloadLenForTransferType(const TransferType type)
class UAVCAN_EXPORT TransferPriority
{
static const unsigned lens[NumTransferTypes] =
{
MaxServiceTransferPayloadLen,
MaxServiceTransferPayloadLen,
MaxMessageBroadcastTransferPayloadLen,
MaxMessageUnicastTransferPayloadLen
};
if (static_cast<int>(type) < NumTransferTypes)
{
return lens[static_cast<int>(type)];
}
else
{
UAVCAN_ASSERT(0);
return 0;
}
}
uint8_t value_;
public:
static const uint8_t BitLen = 5U;
static const TransferPriority Default;
enum TransferPriority
{
TransferPriorityHigh = 0,
TransferPriorityNormal = 1,
TransferPriorityService = 2,
TransferPriorityLow = 3,
NumTransferPriorities = 4
TransferPriority() : value_(0xFF) { }
TransferPriority(uint8_t value) // Implicit
: value_(value)
{
UAVCAN_ASSERT(isValid());
}
uint8_t get() const { return value_; }
bool isValid() const { return value_ < (1U << BitLen); }
bool operator!=(TransferPriority rhs) const { return value_ != rhs.value_; }
bool operator==(TransferPriority rhs) const { return value_ == rhs.value_; }
};
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_;
public:
static const uint8_t BitLen = 3U;
static const uint8_t BitLen = 5U;
static const uint8_t Max = (1U << BitLen) - 1U;
TransferID()
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
*/