mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-16 21:20:36 +08:00
Refactoring: Frame Index field size increased, Trnasfer ID field size reduced. NodeID class added, Frame class rewritten with stricter runtime checks. All tests were updated accordingly.
This commit is contained in:
@@ -48,9 +48,9 @@ void Dispatcher::ListenerRegister::handleFrame(const RxFrame& frame)
|
||||
TransferListenerBase* p = list_.get();
|
||||
while (p)
|
||||
{
|
||||
if (p->getDataTypeDescriptor()->id == frame.data_type_id)
|
||||
if (p->getDataTypeDescriptor()->id == frame.getDataTypeID())
|
||||
p->handleFrame(frame);
|
||||
else if (p->getDataTypeDescriptor()->id < frame.data_type_id) // Listeners are ordered by data type id!
|
||||
else if (p->getDataTypeDescriptor()->id < frame.getDataTypeID()) // Listeners are ordered by data type id!
|
||||
break;
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
@@ -68,39 +68,13 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame)
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* Target Node ID checking
|
||||
*/
|
||||
switch (frame.transfer_type)
|
||||
if ((frame.getDstNodeID() != NodeID::BROADCAST) &&
|
||||
(frame.getDstNodeID() != getSelfNodeID()))
|
||||
{
|
||||
case TRANSFER_TYPE_MESSAGE_BROADCAST:
|
||||
// Always accepted
|
||||
break;
|
||||
|
||||
case TRANSFER_TYPE_SERVICE_RESPONSE:
|
||||
case TRANSFER_TYPE_SERVICE_REQUEST:
|
||||
case TRANSFER_TYPE_MESSAGE_UNICAST:
|
||||
/* Here we drop transfer headers that aren't addressed to us. Non-first frames must be accepted in
|
||||
* any case, because they lack address information in order to reduce payload overhead.
|
||||
*/
|
||||
if (frame.frame_index == 0)
|
||||
{
|
||||
const uint8_t target_node_id = frame.payload[0];
|
||||
if (target_node_id != getSelfNodeID())
|
||||
return;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
// This means that RxFrame::parse() accepted an invalid frame. Too bad!
|
||||
assert(0);
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* Target register selection
|
||||
*/
|
||||
switch (frame.transfer_type)
|
||||
switch (frame.getTransferType())
|
||||
{
|
||||
case TRANSFER_TYPE_MESSAGE_BROADCAST:
|
||||
case TRANSFER_TYPE_MESSAGE_UNICAST:
|
||||
@@ -143,13 +117,19 @@ int Dispatcher::spin(uint64_t monotonic_deadline)
|
||||
int Dispatcher::send(const Frame& frame, uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
|
||||
CanTxQueue::Qos qos)
|
||||
{
|
||||
if (frame.source_node_id != getSelfNodeID())
|
||||
if (frame.getSrcNodeID() != getSelfNodeID())
|
||||
{
|
||||
assert(0);
|
||||
return -1;
|
||||
}
|
||||
|
||||
const CanFrame can_frame = frame.compile();
|
||||
CanFrame can_frame;
|
||||
if (!frame.compile(can_frame))
|
||||
{
|
||||
UAVCAN_TRACE("Dispatcher", "Unable to send: frame is malformed: %s", frame.toString().c_str());
|
||||
assert(0);
|
||||
return -1;
|
||||
}
|
||||
const int iface_mask = (1 << canio_.getNumIfaces()) - 1;
|
||||
|
||||
return canio_.send(can_frame, monotonic_tx_deadline, monotonic_blocking_deadline, iface_mask, qos);
|
||||
|
||||
@@ -9,7 +9,14 @@
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
/**
|
||||
* NodeID
|
||||
*/
|
||||
const NodeID NodeID::BROADCAST(VALUE_BROADCAST);
|
||||
|
||||
/**
|
||||
* TransferID
|
||||
*/
|
||||
int TransferID::forwardDistance(TransferID rhs) const
|
||||
{
|
||||
int d = int(rhs.get()) - int(get());
|
||||
@@ -20,6 +27,39 @@ int TransferID::forwardDistance(TransferID rhs) const
|
||||
return d;
|
||||
}
|
||||
|
||||
/**
|
||||
* Frame
|
||||
*/
|
||||
int Frame::getMaxPayloadLen() const
|
||||
{
|
||||
switch (getTransferType())
|
||||
{
|
||||
case TRANSFER_TYPE_MESSAGE_BROADCAST:
|
||||
return sizeof(payload_);
|
||||
break;
|
||||
|
||||
case TRANSFER_TYPE_SERVICE_RESPONSE:
|
||||
case TRANSFER_TYPE_SERVICE_REQUEST:
|
||||
case TRANSFER_TYPE_MESSAGE_UNICAST:
|
||||
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)
|
||||
@@ -32,23 +72,65 @@ bool Frame::parse(const CanFrame& can_frame)
|
||||
if ((can_frame.id & CanFrame::FLAG_RTR) || !(can_frame.id & CanFrame::FLAG_EFF))
|
||||
return false;
|
||||
|
||||
if (can_frame.dlc > 8)
|
||||
if (can_frame.dlc > sizeof(CanFrame::data))
|
||||
{
|
||||
assert(0);
|
||||
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::MASK_EXTID;
|
||||
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);
|
||||
|
||||
transfer_id = bitunpack<0, 4>(id);
|
||||
last_frame = bitunpack<4, 1>(id);
|
||||
frame_index = bitunpack<5, 5>(id);
|
||||
source_node_id = bitunpack<10, 7>(id);
|
||||
transfer_type = TransferType(bitunpack<17, 2>(id));
|
||||
data_type_id = bitunpack<19, 10>(id);
|
||||
/*
|
||||
* Validation
|
||||
*/
|
||||
if (frame_index_ > FRAME_INDEX_MAX)
|
||||
return false;
|
||||
|
||||
payload_len = can_frame.dlc;
|
||||
std::copy(can_frame.data, can_frame.data + can_frame.dlc, payload);
|
||||
if ((frame_index_ == FRAME_INDEX_MAX) && !last_frame_) // Unterminated transfer
|
||||
return false;
|
||||
|
||||
if (!src_node_id_.isUnicast())
|
||||
return false;
|
||||
|
||||
/*
|
||||
* CAN payload parsing
|
||||
*/
|
||||
switch (transfer_type_)
|
||||
{
|
||||
case TRANSFER_TYPE_MESSAGE_BROADCAST:
|
||||
dst_node_id_ = NodeID::BROADCAST;
|
||||
payload_len_ = can_frame.dlc;
|
||||
std::copy(can_frame.data, can_frame.data + can_frame.dlc, payload_);
|
||||
break;
|
||||
|
||||
case TRANSFER_TYPE_SERVICE_RESPONSE:
|
||||
case TRANSFER_TYPE_SERVICE_REQUEST:
|
||||
case TRANSFER_TYPE_MESSAGE_UNICAST:
|
||||
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;
|
||||
if (!dst_node_id_.isUnicast() || (dst_node_id_ == src_node_id_))
|
||||
return false;
|
||||
|
||||
payload_len_ = can_frame.dlc - 1;
|
||||
std::copy(can_frame.data + 1, can_frame.data + can_frame.dlc, payload_);
|
||||
break;
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -58,22 +140,74 @@ inline static uint32_t bitpack(uint32_t field)
|
||||
return (field & ((1UL << WIDTH) - 1)) << OFFSET;
|
||||
}
|
||||
|
||||
CanFrame Frame::compile() const
|
||||
bool Frame::compile(CanFrame& out_can_frame) const
|
||||
{
|
||||
CanFrame frame;
|
||||
if (!isValid())
|
||||
{
|
||||
assert(0); // This is an application error, so we need to maximize it.
|
||||
return false;
|
||||
}
|
||||
|
||||
frame.id = CanFrame::FLAG_EFF |
|
||||
bitpack<0, 4>(transfer_id.get()) |
|
||||
bitpack<4, 1>(last_frame) |
|
||||
bitpack<5, 5>(frame_index) |
|
||||
bitpack<10, 7>(source_node_id) |
|
||||
bitpack<17, 2>(transfer_type) |
|
||||
bitpack<19, 10>(data_type_id);
|
||||
out_can_frame.id = CanFrame::FLAG_EFF |
|
||||
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_);
|
||||
|
||||
assert(payload_len <= sizeof(payload));
|
||||
frame.dlc = payload_len;
|
||||
std::copy(payload, payload + payload_len, frame.data);
|
||||
return frame;
|
||||
switch (transfer_type_)
|
||||
{
|
||||
case TRANSFER_TYPE_MESSAGE_BROADCAST:
|
||||
out_can_frame.dlc = payload_len_;
|
||||
std::copy(payload_, payload_ + payload_len_, out_can_frame.data);
|
||||
break;
|
||||
|
||||
case TRANSFER_TYPE_SERVICE_RESPONSE:
|
||||
case TRANSFER_TYPE_SERVICE_REQUEST:
|
||||
case TRANSFER_TYPE_MESSAGE_UNICAST:
|
||||
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_ > FRAME_INDEX_MAX) ||
|
||||
((frame_index_ == FRAME_INDEX_MAX) && !last_frame_) ||
|
||||
(!src_node_id_.isUnicast()) ||
|
||||
(!dst_node_id_.isValid()) ||
|
||||
(src_node_id_ == dst_node_id_) ||
|
||||
((transfer_type_ == TRANSFER_TYPE_MESSAGE_BROADCAST) != dst_node_id_.isBroadcast()) ||
|
||||
(transfer_type_ >= NUM_TRANSFER_TYPES) ||
|
||||
(payload_len_ > getMaxPayloadLen()) ||
|
||||
(data_type_id_ > DATA_TYPE_ID_MAX);
|
||||
|
||||
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
|
||||
@@ -89,24 +223,27 @@ std::string Frame::toString() const
|
||||
*/
|
||||
static const int BUFLEN = 100;
|
||||
char buf[BUFLEN];
|
||||
int ofs = std::snprintf(buf, BUFLEN, "dtid=%i tt=%i snid=%i idx=%i last=%i tid=%i payload=[",
|
||||
int(data_type_id), int(transfer_type), int(source_node_id), int(frame_index),
|
||||
int(last_frame), int(transfer_id.get()));
|
||||
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++)
|
||||
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, "%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
|
||||
*/
|
||||
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_monotonic << " ts_utc=" << ts_utc << " iface=" << int(iface_index);
|
||||
os << Frame::toString() << " ts_m=" << ts_monotonic_ << " ts_utc=" << ts_utc_ << " iface=" << int(iface_index_);
|
||||
return os.str();
|
||||
}
|
||||
|
||||
|
||||
@@ -16,7 +16,7 @@ namespace uavcan
|
||||
std::string TransferBufferManagerKey::toString() const
|
||||
{
|
||||
char buf[24];
|
||||
std::snprintf(buf, sizeof(buf), "nid=%i tt=%i", int(node_id_), int(transfer_type_));
|
||||
std::snprintf(buf, sizeof(buf), "nid=%i tt=%i", int(node_id_.get()), int(transfer_type_));
|
||||
return std::string(buf);
|
||||
}
|
||||
|
||||
|
||||
@@ -3,7 +3,9 @@
|
||||
*/
|
||||
|
||||
#include <cstdlib>
|
||||
#include <cassert>
|
||||
#include <uavcan/internal/transport/transfer_listener.hpp>
|
||||
#include <uavcan/internal/debug.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
@@ -11,12 +13,14 @@ namespace uavcan
|
||||
/*
|
||||
* SingleFrameIncomingTransfer
|
||||
*/
|
||||
SingleFrameIncomingTransfer::SingleFrameIncomingTransfer(const RxFrame& frm, const uint8_t* payload,
|
||||
unsigned int payload_len)
|
||||
: IncomingTransfer(frm.ts_monotonic, frm.ts_utc, frm.transfer_type, frm.transfer_id, frm.source_node_id)
|
||||
, payload_(payload)
|
||||
, payload_len_(payload_len)
|
||||
{ }
|
||||
SingleFrameIncomingTransfer::SingleFrameIncomingTransfer(const RxFrame& frm)
|
||||
: IncomingTransfer(frm.getMonotonicTimestamp(), frm.getUtcTimestamp(), frm.getTransferType(),
|
||||
frm.getTransferID(), frm.getSrcNodeID())
|
||||
, payload_(frm.getPayloadPtr())
|
||||
, payload_len_(frm.getPayloadLen())
|
||||
{
|
||||
assert(frm.isValid());
|
||||
}
|
||||
|
||||
int SingleFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsigned int len) const
|
||||
{
|
||||
@@ -39,10 +43,12 @@ int SingleFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsign
|
||||
*/
|
||||
MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(uint64_t ts_monotonic, uint64_t ts_utc,
|
||||
const RxFrame& last_frame, TransferBufferAccessor& tba)
|
||||
: IncomingTransfer(ts_monotonic, ts_utc, last_frame.transfer_type, last_frame.transfer_id, last_frame.source_node_id)
|
||||
: IncomingTransfer(ts_monotonic, ts_utc, last_frame.getTransferType(), last_frame.getTransferID(),
|
||||
last_frame.getSrcNodeID())
|
||||
, buf_acc_(tba)
|
||||
{
|
||||
assert(last_frame.last_frame);
|
||||
assert(last_frame.isValid());
|
||||
assert(last_frame.isLastFrame());
|
||||
}
|
||||
|
||||
int MultiFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsigned int len) const
|
||||
@@ -90,17 +96,7 @@ void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxF
|
||||
|
||||
case TransferReceiver::RESULT_SINGLE_FRAME:
|
||||
{
|
||||
uint8_t payload[Frame::PAYLOAD_LEN_MAX];
|
||||
unsigned int payload_len = 0;
|
||||
const bool success = TransferReceiver::extractSingleFrameTransferPayload(frame, payload, payload_len);
|
||||
if (!success)
|
||||
{
|
||||
UAVCAN_TRACE("TransferListenerBase", "SFT payload extraction failed, frame: %s", frame.toString().c_str());
|
||||
return;
|
||||
}
|
||||
assert(payload_len <= Frame::PAYLOAD_LEN_MAX);
|
||||
|
||||
SingleFrameIncomingTransfer it(frame, payload, payload_len);
|
||||
SingleFrameIncomingTransfer it(frame);
|
||||
handleIncomingTransfer(it);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -12,13 +12,15 @@
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
static const int CRC_LEN = 2;
|
||||
|
||||
const uint32_t TransferReceiver::DEFAULT_TRANSFER_INTERVAL;
|
||||
const uint32_t TransferReceiver::MIN_TRANSFER_INTERVAL;
|
||||
const uint32_t TransferReceiver::MAX_TRANSFER_INTERVAL;
|
||||
|
||||
TransferReceiver::TidRelation TransferReceiver::getTidRelation(const RxFrame& frame) const
|
||||
{
|
||||
const int distance = tid_.forwardDistance(frame.transfer_id);
|
||||
const int distance = tid_.forwardDistance(frame.getTransferID());
|
||||
if (distance == 0)
|
||||
return TID_SAME;
|
||||
if (distance < ((1 << TransferID::BITLEN) / 2))
|
||||
@@ -52,36 +54,31 @@ void TransferReceiver::prepareForNextTransfer()
|
||||
|
||||
bool TransferReceiver::validate(const RxFrame& frame) const
|
||||
{
|
||||
if (iface_index_ != frame.iface_index)
|
||||
if (iface_index_ != frame.getIfaceIndex())
|
||||
return false;
|
||||
|
||||
if (frame.source_node_id == 0)
|
||||
if (frame.isFirstFrame() && !frame.isLastFrame() && (frame.getPayloadLen() < CRC_LEN))
|
||||
{
|
||||
UAVCAN_TRACE("TransferReceiver", "Invalid frame NID, %s", frame.toString().c_str());
|
||||
UAVCAN_TRACE("TransferReceiver", "CRC expected, %s", frame.toString().c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
if (frame.frame_index != next_frame_index_)
|
||||
if ((frame.getFrameIndex() == Frame::FRAME_INDEX_MAX) && !frame.isLastFrame())
|
||||
{
|
||||
UAVCAN_TRACE("TransferReceiver", "Unexpected frame index, %s", frame.toString().c_str());
|
||||
UAVCAN_TRACE("TransferReceiver", "Unterminated transfer, %s", frame.toString().c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!frame.last_frame && frame.payload_len != Frame::PAYLOAD_LEN_MAX)
|
||||
if (frame.getFrameIndex() != next_frame_index_)
|
||||
{
|
||||
UAVCAN_TRACE("TransferReceiver", "Unexpected payload len, %s", frame.toString().c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!frame.last_frame && frame.frame_index == Frame::FRAME_INDEX_MAX)
|
||||
{
|
||||
UAVCAN_TRACE("TransferReceiver", "Expected end of transfer, %s", frame.toString().c_str());
|
||||
UAVCAN_TRACE("TransferReceiver", "Unexpected frame index (not %i), %s",
|
||||
int(next_frame_index_), frame.toString().c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
if (getTidRelation(frame) != TID_SAME)
|
||||
{
|
||||
UAVCAN_TRACE("TransferReceiver", "Unexpected TID, %s", frame.toString().c_str());
|
||||
UAVCAN_TRACE("TransferReceiver", "Unexpected TID (current %i), %s", tid_.get(), frame.toString().c_str());
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
@@ -89,34 +86,18 @@ bool TransferReceiver::validate(const RxFrame& frame) const
|
||||
|
||||
bool TransferReceiver::writePayload(const RxFrame& frame, TransferBufferBase& buf)
|
||||
{
|
||||
if (frame.frame_index == 0)
|
||||
const uint8_t* const payload = frame.getPayloadPtr();
|
||||
const int payload_len = frame.getPayloadLen();
|
||||
|
||||
if (frame.isFirstFrame()) // First frame contains CRC, we need to extract it now
|
||||
{
|
||||
unsigned int payload_offset = 0;
|
||||
unsigned int crc_offset = 0;
|
||||
if (frame.getPayloadLen() < CRC_LEN) // Must have been validated earlier though. I think I'm paranoid.
|
||||
return false;
|
||||
|
||||
switch (frame.transfer_type)
|
||||
{
|
||||
case TRANSFER_TYPE_MESSAGE_BROADCAST:
|
||||
payload_offset = 2;
|
||||
crc_offset = 0;
|
||||
break;
|
||||
case TRANSFER_TYPE_SERVICE_RESPONSE: // Addressed transfers have 1-byte overhead for Destination Node ID
|
||||
case TRANSFER_TYPE_SERVICE_REQUEST:
|
||||
case TRANSFER_TYPE_MESSAGE_UNICAST:
|
||||
payload_offset = 3;
|
||||
crc_offset = 1;
|
||||
break;
|
||||
default:
|
||||
UAVCAN_TRACE("TransferReceiver", "Invalid transfer type, %s", frame.toString().c_str());
|
||||
return RESULT_NOT_COMPLETE;
|
||||
}
|
||||
this_transfer_crc_ = (payload[0] & 0xFF) | (uint16_t(payload[1] & 0xFF) << 8); // Little endian.
|
||||
|
||||
this_transfer_crc_ =
|
||||
(frame.payload[crc_offset] & 0xFF) |
|
||||
(uint16_t(frame.payload[crc_offset + 1] & 0xFF) << 8); // little endian
|
||||
|
||||
const int effective_payload_len = frame.payload_len - payload_offset;
|
||||
const int res = buf.write(buffer_write_pos_, frame.payload + payload_offset, effective_payload_len);
|
||||
const int effective_payload_len = payload_len - CRC_LEN;
|
||||
const int res = buf.write(buffer_write_pos_, payload + CRC_LEN, effective_payload_len);
|
||||
const bool success = res == effective_payload_len;
|
||||
if (success)
|
||||
buffer_write_pos_ += effective_payload_len;
|
||||
@@ -124,10 +105,10 @@ bool TransferReceiver::writePayload(const RxFrame& frame, TransferBufferBase& bu
|
||||
}
|
||||
else
|
||||
{
|
||||
const int res = buf.write(buffer_write_pos_, frame.payload, frame.payload_len);
|
||||
const bool success = res == frame.payload_len;
|
||||
const int res = buf.write(buffer_write_pos_, payload, payload_len);
|
||||
const bool success = res == payload_len;
|
||||
if (success)
|
||||
buffer_write_pos_ += frame.payload_len;
|
||||
buffer_write_pos_ += payload_len;
|
||||
return success;
|
||||
}
|
||||
}
|
||||
@@ -135,14 +116,13 @@ bool TransferReceiver::writePayload(const RxFrame& frame, TransferBufferBase& bu
|
||||
TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, TransferBufferAccessor& tba)
|
||||
{
|
||||
// Transfer timestamps are derived from the first frame
|
||||
if (frame.frame_index == 0)
|
||||
if (frame.isFirstFrame())
|
||||
{
|
||||
this_transfer_ts_monotonic_ = frame.ts_monotonic;
|
||||
first_frame_ts_utc_ = frame.ts_utc;
|
||||
this_transfer_ts_monotonic_ = frame.getMonotonicTimestamp();
|
||||
first_frame_ts_utc_ = frame.getUtcTimestamp();
|
||||
}
|
||||
|
||||
// Single-frame transfer
|
||||
if ((frame.frame_index == 0) && frame.last_frame)
|
||||
if (frame.isFirstFrame() && frame.isLastFrame())
|
||||
{
|
||||
tba.remove();
|
||||
updateTransferTimings();
|
||||
@@ -170,7 +150,7 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, Tra
|
||||
}
|
||||
next_frame_index_++;
|
||||
|
||||
if (frame.last_frame)
|
||||
if (frame.isLastFrame())
|
||||
{
|
||||
updateTransferTimings();
|
||||
prepareForNextTransfer();
|
||||
@@ -181,7 +161,7 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, Tra
|
||||
|
||||
bool TransferReceiver::isTimedOut(uint64_t ts_monotonic) const
|
||||
{
|
||||
static const uint64_t INTERVAL_MULT = (1 << TransferID::BITLEN) / 2 - 1;
|
||||
static const uint64_t INTERVAL_MULT = (1 << TransferID::BITLEN) / 2 + 1;
|
||||
const uint64_t ts = this_transfer_ts_monotonic_;
|
||||
if (ts_monotonic <= ts)
|
||||
return false;
|
||||
@@ -190,19 +170,20 @@ bool TransferReceiver::isTimedOut(uint64_t ts_monotonic) const
|
||||
|
||||
TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba)
|
||||
{
|
||||
if ((frame.ts_monotonic == 0) ||
|
||||
(frame.ts_monotonic < prev_transfer_ts_monotonic_) ||
|
||||
(frame.ts_monotonic < this_transfer_ts_monotonic_))
|
||||
if ((frame.getMonotonicTimestamp() == 0) ||
|
||||
(frame.getMonotonicTimestamp() < prev_transfer_ts_monotonic_) ||
|
||||
(frame.getMonotonicTimestamp() < this_transfer_ts_monotonic_))
|
||||
{
|
||||
return RESULT_NOT_COMPLETE;
|
||||
}
|
||||
|
||||
const bool not_initialized = !isInitialized();
|
||||
const bool iface_timed_out = (frame.ts_monotonic - this_transfer_ts_monotonic_) > (uint64_t(transfer_interval_) * 2);
|
||||
const bool receiver_timed_out = isTimedOut(frame.ts_monotonic);
|
||||
const bool same_iface = frame.iface_index == iface_index_;
|
||||
const bool first_fame = frame.frame_index == 0;
|
||||
const bool receiver_timed_out = isTimedOut(frame.getMonotonicTimestamp());
|
||||
const bool same_iface = frame.getIfaceIndex() == iface_index_;
|
||||
const bool first_fame = frame.isFirstFrame();
|
||||
const TidRelation tid_rel = getTidRelation(frame);
|
||||
const bool iface_timed_out =
|
||||
(frame.getMonotonicTimestamp() - this_transfer_ts_monotonic_) > (uint64_t(transfer_interval_) * 2);
|
||||
|
||||
const bool need_restart = // FSM, the hard way
|
||||
(not_initialized) ||
|
||||
@@ -217,8 +198,8 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr
|
||||
int(not_initialized), int(iface_timed_out), int(receiver_timed_out), int(same_iface), int(first_fame),
|
||||
int(tid_rel), frame.toString().c_str());
|
||||
tba.remove();
|
||||
iface_index_ = frame.iface_index;
|
||||
tid_ = frame.transfer_id;
|
||||
iface_index_ = frame.getIfaceIndex();
|
||||
tid_ = frame.getTransferID();
|
||||
next_frame_index_ = 0;
|
||||
buffer_write_pos_ = 0;
|
||||
this_transfer_crc_ = 0;
|
||||
@@ -235,37 +216,4 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr
|
||||
return receive(frame, tba);
|
||||
}
|
||||
|
||||
bool TransferReceiver::extractSingleFrameTransferPayload(const RxFrame& frame, uint8_t* out_data,
|
||||
unsigned int& out_len)
|
||||
{
|
||||
if (out_data == NULL)
|
||||
{
|
||||
assert(0);
|
||||
return false;
|
||||
}
|
||||
|
||||
out_len = 0;
|
||||
unsigned int offset = 0;
|
||||
switch (frame.transfer_type)
|
||||
{
|
||||
case TRANSFER_TYPE_MESSAGE_BROADCAST:
|
||||
offset = 0;
|
||||
break;
|
||||
case TRANSFER_TYPE_SERVICE_RESPONSE: // Addressed transfers have 1-byte overhead for Destination Node ID
|
||||
case TRANSFER_TYPE_SERVICE_REQUEST:
|
||||
case TRANSFER_TYPE_MESSAGE_UNICAST:
|
||||
offset = 1;
|
||||
break;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
|
||||
if (frame.payload_len < offset)
|
||||
return false;
|
||||
|
||||
out_len = frame.payload_len - offset;
|
||||
std::copy(frame.payload + offset, frame.payload + offset + out_len, out_data);
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user