mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-18 02:00:36 +08:00
Fixed inclusion loops
This commit is contained in:
Pavel Kirienko
@@ -5,6 +5,7 @@
|
||||
#include <cassert>
|
||||
#include <sstream>
|
||||
#include <uavcan/internal/marshal/bit_stream.hpp>
|
||||
#include <uavcan/internal/transport/transfer_buffer.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
@@ -0,0 +1,228 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
#include <sstream>
|
||||
#include <uavcan/internal/transport/frame.hpp>
|
||||
#include <uavcan/internal/transport/can_io.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
/**
|
||||
* Frame
|
||||
*/
|
||||
int Frame::getMaxPayloadLen() const
|
||||
{
|
||||
switch (getTransferType())
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
return sizeof(payload_);
|
||||
break;
|
||||
|
||||
case TransferTypeServiceResponse:
|
||||
case TransferTypeServiceRequest:
|
||||
case TransferTypeMessageUnicast:
|
||||
return sizeof(payload_) - 1;
|
||||
break;
|
||||
|
||||
default:
|
||||
assert(0);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
int Frame::setPayload(const uint8_t* data, int len)
|
||||
{
|
||||
len = std::min(getMaxPayloadLen(), len);
|
||||
if (len >= 0)
|
||||
{
|
||||
std::copy(data, data + len, payload_);
|
||||
payload_len_ = len;
|
||||
}
|
||||
return len;
|
||||
}
|
||||
|
||||
template <int OFFSET, int WIDTH>
|
||||
inline static uint32_t bitunpack(uint32_t val)
|
||||
{
|
||||
return (val >> OFFSET) & ((1UL << WIDTH) - 1);
|
||||
}
|
||||
|
||||
bool Frame::parse(const CanFrame& can_frame)
|
||||
{
|
||||
if ((can_frame.id & CanFrame::FlagRTR) || !(can_frame.id & CanFrame::FlagEFF))
|
||||
return false;
|
||||
|
||||
if (can_frame.dlc > sizeof(CanFrame::data))
|
||||
{
|
||||
assert(0); // This is not a protocol error, so assert() is ok
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
* CAN ID parsing
|
||||
*/
|
||||
const uint32_t id = can_frame.id & CanFrame::MaskExtID;
|
||||
transfer_id_ = bitunpack<0, 3>(id);
|
||||
last_frame_ = bitunpack<3, 1>(id);
|
||||
frame_index_ = bitunpack<4, 6>(id);
|
||||
src_node_id_ = bitunpack<10, 7>(id);
|
||||
transfer_type_ = TransferType(bitunpack<17, 2>(id));
|
||||
data_type_id_ = bitunpack<19, 10>(id);
|
||||
|
||||
/*
|
||||
* CAN payload parsing
|
||||
*/
|
||||
switch (transfer_type_)
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
dst_node_id_ = NodeID::Broadcast;
|
||||
payload_len_ = can_frame.dlc;
|
||||
std::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_ = can_frame.dlc - 1;
|
||||
std::copy(can_frame.data + 1, can_frame.data + can_frame.dlc, payload_);
|
||||
break;
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
|
||||
return isValid();
|
||||
}
|
||||
|
||||
template <int OFFSET, int WIDTH>
|
||||
inline static uint32_t bitpack(uint32_t field)
|
||||
{
|
||||
return (field & ((1UL << WIDTH) - 1)) << OFFSET;
|
||||
}
|
||||
|
||||
bool Frame::compile(CanFrame& out_can_frame) const
|
||||
{
|
||||
if (!isValid())
|
||||
{
|
||||
assert(0); // This is an application error, so we need to maximize it.
|
||||
return false;
|
||||
}
|
||||
|
||||
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_);
|
||||
|
||||
switch (transfer_type_)
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
out_can_frame.dlc = payload_len_;
|
||||
std::copy(payload_, payload_ + payload_len_, out_can_frame.data);
|
||||
break;
|
||||
|
||||
case TransferTypeServiceResponse:
|
||||
case TransferTypeServiceRequest:
|
||||
case TransferTypeMessageUnicast:
|
||||
assert((payload_len_ + 1) <= sizeof(CanFrame::data));
|
||||
out_can_frame.data[0] = dst_node_id_.get();
|
||||
out_can_frame.dlc = payload_len_ + 1;
|
||||
std::copy(payload_, payload_ + payload_len_, out_can_frame.data + 1);
|
||||
break;
|
||||
|
||||
default:
|
||||
assert(0);
|
||||
return false;
|
||||
}
|
||||
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_.isUnicast()) ||
|
||||
(!dst_node_id_.isValid()) ||
|
||||
(src_node_id_ == dst_node_id_) ||
|
||||
((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast()) ||
|
||||
(transfer_type_ >= NumTransferTypes) ||
|
||||
(payload_len_ > getMaxPayloadLen()) ||
|
||||
(data_type_id_ > DataTypeDescriptor::MaxDataTypeID);
|
||||
|
||||
return !invalid;
|
||||
}
|
||||
|
||||
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_) &&
|
||||
std::equal(payload_, payload_ + payload_len_, rhs.payload_);
|
||||
}
|
||||
|
||||
std::string Frame::toString() const
|
||||
{
|
||||
/*
|
||||
* Frame ID fields, according to UAVCAN specs:
|
||||
* - Data Type ID
|
||||
* - Transfer Type
|
||||
* - Source Node ID
|
||||
* - Frame Index
|
||||
* - Last Frame
|
||||
* - Transfer ID
|
||||
*/
|
||||
static const int BUFLEN = 100;
|
||||
char buf[BUFLEN];
|
||||
int ofs = std::snprintf(buf, BUFLEN, "dtid=%i tt=%i snid=%i dnid=%i idx=%i last=%i tid=%i payload=[",
|
||||
int(data_type_id_), int(transfer_type_), int(src_node_id_.get()), int(dst_node_id_.get()),
|
||||
int(frame_index_), int(last_frame_), int(transfer_id_.get()));
|
||||
|
||||
for (int i = 0; i < payload_len_; i++)
|
||||
{
|
||||
ofs += std::snprintf(buf + ofs, BUFLEN - ofs, "%02x", payload_[i]);
|
||||
if ((i + 1) < payload_len_)
|
||||
ofs += std::snprintf(buf + ofs, BUFLEN - ofs, " ");
|
||||
}
|
||||
ofs += std::snprintf(buf + ofs, BUFLEN - ofs, "]");
|
||||
return std::string(buf);
|
||||
}
|
||||
|
||||
/**
|
||||
* RxFrame
|
||||
*/
|
||||
bool RxFrame::parse(const CanRxFrame& can_frame)
|
||||
{
|
||||
if (!Frame::parse(can_frame))
|
||||
return false;
|
||||
ts_mono_ = can_frame.ts_mono;
|
||||
ts_utc_ = can_frame.ts_utc;
|
||||
iface_index_ = can_frame.iface_index;
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string RxFrame::toString() const
|
||||
{
|
||||
std::ostringstream os; // C++03 doesn't support long long, so we need ostream to print the timestamp
|
||||
os << Frame::toString() << " ts_m=" << ts_mono_ << " ts_utc=" << ts_utc_ << " iface=" << int(iface_index_);
|
||||
return os.str();
|
||||
}
|
||||
|
||||
}
|
||||
@@ -5,7 +5,7 @@
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
#include <sstream>
|
||||
#include <uavcan/internal/transport/transfer.hpp>
|
||||
#include <uavcan/internal/transport/frame.hpp>
|
||||
#include <uavcan/internal/transport/can_io.hpp>
|
||||
|
||||
namespace uavcan
|
||||
@@ -28,219 +28,4 @@ int TransferID::forwardDistance(TransferID rhs) const
|
||||
return d;
|
||||
}
|
||||
|
||||
/**
|
||||
* Frame
|
||||
*/
|
||||
int Frame::getMaxPayloadLen() const
|
||||
{
|
||||
switch (getTransferType())
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
return sizeof(payload_);
|
||||
break;
|
||||
|
||||
case TransferTypeServiceResponse:
|
||||
case TransferTypeServiceRequest:
|
||||
case TransferTypeMessageUnicast:
|
||||
return sizeof(payload_) - 1;
|
||||
break;
|
||||
|
||||
default:
|
||||
assert(0);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
int Frame::setPayload(const uint8_t* data, int len)
|
||||
{
|
||||
len = std::min(getMaxPayloadLen(), len);
|
||||
if (len >= 0)
|
||||
{
|
||||
std::copy(data, data + len, payload_);
|
||||
payload_len_ = len;
|
||||
}
|
||||
return len;
|
||||
}
|
||||
|
||||
template <int OFFSET, int WIDTH>
|
||||
inline static uint32_t bitunpack(uint32_t val)
|
||||
{
|
||||
return (val >> OFFSET) & ((1UL << WIDTH) - 1);
|
||||
}
|
||||
|
||||
bool Frame::parse(const CanFrame& can_frame)
|
||||
{
|
||||
if ((can_frame.id & CanFrame::FlagRTR) || !(can_frame.id & CanFrame::FlagEFF))
|
||||
return false;
|
||||
|
||||
if (can_frame.dlc > sizeof(CanFrame::data))
|
||||
{
|
||||
assert(0); // This is not a protocol error, so assert() is ok
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
* CAN ID parsing
|
||||
*/
|
||||
const uint32_t id = can_frame.id & CanFrame::MaskExtID;
|
||||
transfer_id_ = bitunpack<0, 3>(id);
|
||||
last_frame_ = bitunpack<3, 1>(id);
|
||||
frame_index_ = bitunpack<4, 6>(id);
|
||||
src_node_id_ = bitunpack<10, 7>(id);
|
||||
transfer_type_ = TransferType(bitunpack<17, 2>(id));
|
||||
data_type_id_ = bitunpack<19, 10>(id);
|
||||
|
||||
/*
|
||||
* CAN payload parsing
|
||||
*/
|
||||
switch (transfer_type_)
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
dst_node_id_ = NodeID::Broadcast;
|
||||
payload_len_ = can_frame.dlc;
|
||||
std::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_ = can_frame.dlc - 1;
|
||||
std::copy(can_frame.data + 1, can_frame.data + can_frame.dlc, payload_);
|
||||
break;
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
|
||||
return isValid();
|
||||
}
|
||||
|
||||
template <int OFFSET, int WIDTH>
|
||||
inline static uint32_t bitpack(uint32_t field)
|
||||
{
|
||||
return (field & ((1UL << WIDTH) - 1)) << OFFSET;
|
||||
}
|
||||
|
||||
bool Frame::compile(CanFrame& out_can_frame) const
|
||||
{
|
||||
if (!isValid())
|
||||
{
|
||||
assert(0); // This is an application error, so we need to maximize it.
|
||||
return false;
|
||||
}
|
||||
|
||||
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_);
|
||||
|
||||
switch (transfer_type_)
|
||||
{
|
||||
case TransferTypeMessageBroadcast:
|
||||
out_can_frame.dlc = payload_len_;
|
||||
std::copy(payload_, payload_ + payload_len_, out_can_frame.data);
|
||||
break;
|
||||
|
||||
case TransferTypeServiceResponse:
|
||||
case TransferTypeServiceRequest:
|
||||
case TransferTypeMessageUnicast:
|
||||
assert((payload_len_ + 1) <= sizeof(CanFrame::data));
|
||||
out_can_frame.data[0] = dst_node_id_.get();
|
||||
out_can_frame.dlc = payload_len_ + 1;
|
||||
std::copy(payload_, payload_ + payload_len_, out_can_frame.data + 1);
|
||||
break;
|
||||
|
||||
default:
|
||||
assert(0);
|
||||
return false;
|
||||
}
|
||||
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_.isUnicast()) ||
|
||||
(!dst_node_id_.isValid()) ||
|
||||
(src_node_id_ == dst_node_id_) ||
|
||||
((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast()) ||
|
||||
(transfer_type_ >= NumTransferTypes) ||
|
||||
(payload_len_ > getMaxPayloadLen()) ||
|
||||
(data_type_id_ > MaxDataTypeID);
|
||||
|
||||
return !invalid;
|
||||
}
|
||||
|
||||
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_) &&
|
||||
std::equal(payload_, payload_ + payload_len_, rhs.payload_);
|
||||
}
|
||||
|
||||
std::string Frame::toString() const
|
||||
{
|
||||
/*
|
||||
* Frame ID fields, according to UAVCAN specs:
|
||||
* - Data Type ID
|
||||
* - Transfer Type
|
||||
* - Source Node ID
|
||||
* - Frame Index
|
||||
* - Last Frame
|
||||
* - Transfer ID
|
||||
*/
|
||||
static const int BUFLEN = 100;
|
||||
char buf[BUFLEN];
|
||||
int ofs = std::snprintf(buf, BUFLEN, "dtid=%i tt=%i snid=%i dnid=%i idx=%i last=%i tid=%i payload=[",
|
||||
int(data_type_id_), int(transfer_type_), int(src_node_id_.get()), int(dst_node_id_.get()),
|
||||
int(frame_index_), int(last_frame_), int(transfer_id_.get()));
|
||||
|
||||
for (int i = 0; i < payload_len_; i++)
|
||||
{
|
||||
ofs += std::snprintf(buf + ofs, BUFLEN - ofs, "%02x", payload_[i]);
|
||||
if ((i + 1) < payload_len_)
|
||||
ofs += std::snprintf(buf + ofs, BUFLEN - ofs, " ");
|
||||
}
|
||||
ofs += std::snprintf(buf + ofs, BUFLEN - ofs, "]");
|
||||
return std::string(buf);
|
||||
}
|
||||
|
||||
/**
|
||||
* RxFrame
|
||||
*/
|
||||
bool RxFrame::parse(const CanRxFrame& can_frame)
|
||||
{
|
||||
if (!Frame::parse(can_frame))
|
||||
return false;
|
||||
ts_mono_ = can_frame.ts_mono;
|
||||
ts_utc_ = can_frame.ts_utc;
|
||||
iface_index_ = can_frame.iface_index;
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string RxFrame::toString() const
|
||||
{
|
||||
std::ostringstream os; // C++03 doesn't support long long, so we need ostream to print the timestamp
|
||||
os << Frame::toString() << " ts_m=" << ts_mono_ << " ts_utc=" << ts_utc_ << " iface=" << int(iface_index_);
|
||||
return os.str();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -1,31 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <uavcan/time.hpp>
|
||||
#include <uavcan/Timestamp.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
/*
|
||||
* UtcTime
|
||||
*/
|
||||
UtcTime::UtcTime(const Timestamp& ts) // Implicit
|
||||
{
|
||||
operator=(ts);
|
||||
}
|
||||
|
||||
UtcTime& UtcTime::operator=(const Timestamp& ts)
|
||||
{
|
||||
*this = fromUSec(ts.husec * Timestamp::USEC_PER_LSB);
|
||||
return *this;
|
||||
}
|
||||
|
||||
UtcTime::operator Timestamp() const
|
||||
{
|
||||
Timestamp ts;
|
||||
ts.husec = toUSec() / Timestamp::USEC_PER_LSB;
|
||||
return ts;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user