mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-31 21:41:36 +08:00
TransferReceiver updated; build is still broken
This commit is contained in:
@@ -34,8 +34,7 @@ private:
|
|||||||
|
|
||||||
enum { IfaceIndexNotSet = MaxCanIfaces };
|
enum { IfaceIndexNotSet = MaxCanIfaces };
|
||||||
|
|
||||||
enum { BufferWritePosMask = 4095 };
|
enum { ErrorCntMask = 31 };
|
||||||
enum { ErrorCntMask = 15 };
|
|
||||||
enum { IfaceIndexMask = MaxCanIfaces };
|
enum { IfaceIndexMask = MaxCanIfaces };
|
||||||
|
|
||||||
MonotonicTime prev_transfer_ts_;
|
MonotonicTime prev_transfer_ts_;
|
||||||
@@ -44,15 +43,14 @@ private:
|
|||||||
uint16_t transfer_interval_msec_;
|
uint16_t transfer_interval_msec_;
|
||||||
uint16_t this_transfer_crc_;
|
uint16_t this_transfer_crc_;
|
||||||
|
|
||||||
// 2 byte aligned bitfields:
|
uint16_t buffer_write_pos_;
|
||||||
uint16_t buffer_write_pos_ : 12;
|
|
||||||
mutable uint16_t error_cnt_ : 4;
|
|
||||||
|
|
||||||
TransferID tid_; // 1 byte field
|
TransferID tid_; // 1 byte field
|
||||||
|
|
||||||
// 1 byte aligned bitfields:
|
// 1 byte aligned bitfields:
|
||||||
uint8_t next_frame_index_ : 6;
|
uint8_t next_toggle_ : 1;
|
||||||
uint8_t iface_index_ : 2;
|
uint8_t iface_index_ : 2;
|
||||||
|
mutable uint8_t error_cnt_ : 5;
|
||||||
|
|
||||||
bool isInitialized() const { return iface_index_ != IfaceIndexNotSet; }
|
bool isInitialized() const { return iface_index_ != IfaceIndexNotSet; }
|
||||||
|
|
||||||
@@ -72,9 +70,9 @@ public:
|
|||||||
transfer_interval_msec_(DefaultTransferIntervalMSec),
|
transfer_interval_msec_(DefaultTransferIntervalMSec),
|
||||||
this_transfer_crc_(0),
|
this_transfer_crc_(0),
|
||||||
buffer_write_pos_(0),
|
buffer_write_pos_(0),
|
||||||
error_cnt_(0),
|
next_toggle_(false),
|
||||||
next_frame_index_(0),
|
iface_index_(IfaceIndexNotSet),
|
||||||
iface_index_(IfaceIndexNotSet)
|
error_cnt_(0)
|
||||||
{
|
{
|
||||||
#if UAVCAN_DEBUG
|
#if UAVCAN_DEBUG
|
||||||
StaticAssert<sizeof(TransferReceiver) == 32>::check();
|
StaticAssert<sizeof(TransferReceiver) == 32>::check();
|
||||||
|
|||||||
@@ -53,7 +53,7 @@ void TransferReceiver::updateTransferTimings()
|
|||||||
void TransferReceiver::prepareForNextTransfer()
|
void TransferReceiver::prepareForNextTransfer()
|
||||||
{
|
{
|
||||||
tid_.increment();
|
tid_.increment();
|
||||||
next_frame_index_ = 0;
|
next_toggle_ = false;
|
||||||
buffer_write_pos_ = 0;
|
buffer_write_pos_ = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -63,22 +63,22 @@ bool TransferReceiver::validate(const RxFrame& frame) const
|
|||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (frame.isFirst() && !frame.isLast() && (frame.getPayloadLen() < TransferCRC::NumBytes))
|
if (frame.isStartOfTransfer() && !frame.isEndOfTransfer() && (frame.getPayloadLen() < TransferCRC::NumBytes))
|
||||||
{
|
{
|
||||||
UAVCAN_TRACE("TransferReceiver", "CRC expected, %s", frame.toString().c_str());
|
UAVCAN_TRACE("TransferReceiver", "CRC expected, %s", frame.toString().c_str());
|
||||||
registerError();
|
registerError();
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if ((frame.getIndex() == frame.getMaxIndex()) && !frame.isLast())
|
if (frame.isStartOfTransfer() && frame.getToggle())
|
||||||
{
|
{
|
||||||
UAVCAN_TRACE("TransferReceiver", "Unterminated transfer, %s", frame.toString().c_str());
|
UAVCAN_TRACE("TransferReceiver", "Toggle bit is not cleared, %s", frame.toString().c_str());
|
||||||
registerError();
|
registerError();
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
if (frame.getIndex() != next_frame_index_)
|
if (frame.getToggle() != next_toggle_)
|
||||||
{
|
{
|
||||||
UAVCAN_TRACE("TransferReceiver", "Unexpected frame index (not %i), %s",
|
UAVCAN_TRACE("TransferReceiver", "Unexpected toggle bit (not %i), %s",
|
||||||
int(next_frame_index_), frame.toString().c_str());
|
int(next_toggle_), frame.toString().c_str());
|
||||||
registerError();
|
registerError();
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
@@ -96,7 +96,7 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
|
|||||||
const uint8_t* const payload = frame.getPayloadPtr();
|
const uint8_t* const payload = frame.getPayloadPtr();
|
||||||
const unsigned payload_len = frame.getPayloadLen();
|
const unsigned payload_len = frame.getPayloadLen();
|
||||||
|
|
||||||
if (frame.isFirst()) // First frame contains CRC, we need to extract it now
|
if (frame.isStartOfTransfer()) // First frame contains CRC, we need to extract it now
|
||||||
{
|
{
|
||||||
if (frame.getPayloadLen() < TransferCRC::NumBytes)
|
if (frame.getPayloadLen() < TransferCRC::NumBytes)
|
||||||
{
|
{
|
||||||
@@ -110,7 +110,7 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
|
|||||||
const bool success = res == static_cast<int>(effective_payload_len);
|
const bool success = res == static_cast<int>(effective_payload_len);
|
||||||
if (success)
|
if (success)
|
||||||
{
|
{
|
||||||
buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + effective_payload_len) & BufferWritePosMask;
|
buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + effective_payload_len);
|
||||||
}
|
}
|
||||||
return success;
|
return success;
|
||||||
}
|
}
|
||||||
@@ -120,7 +120,7 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
|
|||||||
const bool success = res == static_cast<int>(payload_len);
|
const bool success = res == static_cast<int>(payload_len);
|
||||||
if (success)
|
if (success)
|
||||||
{
|
{
|
||||||
buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + payload_len) & BufferWritePosMask;
|
buffer_write_pos_ = static_cast<uint16_t>(buffer_write_pos_ + payload_len);
|
||||||
}
|
}
|
||||||
return success;
|
return success;
|
||||||
}
|
}
|
||||||
@@ -129,13 +129,13 @@ bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
|
|||||||
TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, TransferBufferAccessor& tba)
|
TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, TransferBufferAccessor& tba)
|
||||||
{
|
{
|
||||||
// Transfer timestamps are derived from the first frame
|
// Transfer timestamps are derived from the first frame
|
||||||
if (frame.isFirst())
|
if (frame.isStartOfTransfer())
|
||||||
{
|
{
|
||||||
this_transfer_ts_ = frame.getMonotonicTimestamp();
|
this_transfer_ts_ = frame.getMonotonicTimestamp();
|
||||||
first_frame_ts_ = frame.getUtcTimestamp();
|
first_frame_ts_ = frame.getUtcTimestamp();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (frame.isFirst() && frame.isLast())
|
if (frame.isStartOfTransfer() && frame.isEndOfTransfer())
|
||||||
{
|
{
|
||||||
tba.remove();
|
tba.remove();
|
||||||
updateTransferTimings();
|
updateTransferTimings();
|
||||||
@@ -165,9 +165,9 @@ TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, Tra
|
|||||||
registerError();
|
registerError();
|
||||||
return ResultNotComplete;
|
return ResultNotComplete;
|
||||||
}
|
}
|
||||||
next_frame_index_++;
|
next_toggle_ = !next_toggle_;
|
||||||
|
|
||||||
if (frame.isLast())
|
if (frame.isEndOfTransfer())
|
||||||
{
|
{
|
||||||
updateTransferTimings();
|
updateTransferTimings();
|
||||||
prepareForNextTransfer();
|
prepareForNextTransfer();
|
||||||
@@ -198,7 +198,7 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr
|
|||||||
const bool not_initialized = !isInitialized();
|
const bool not_initialized = !isInitialized();
|
||||||
const bool receiver_timed_out = isTimedOut(frame.getMonotonicTimestamp());
|
const bool receiver_timed_out = isTimedOut(frame.getMonotonicTimestamp());
|
||||||
const bool same_iface = frame.getIfaceIndex() == iface_index_;
|
const bool same_iface = frame.getIfaceIndex() == iface_index_;
|
||||||
const bool first_fame = frame.isFirst();
|
const bool first_fame = frame.isStartOfTransfer();
|
||||||
const TidRelation tid_rel = getTidRelation(frame);
|
const TidRelation tid_rel = getTidRelation(frame);
|
||||||
const bool iface_timed_out =
|
const bool iface_timed_out =
|
||||||
(frame.getMonotonicTimestamp() - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_msec_) * 1000 * 2);
|
(frame.getMonotonicTimestamp() - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_msec_) * 1000 * 2);
|
||||||
@@ -224,7 +224,7 @@ TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, Tr
|
|||||||
tba.remove();
|
tba.remove();
|
||||||
iface_index_ = frame.getIfaceIndex() & IfaceIndexMask;
|
iface_index_ = frame.getIfaceIndex() & IfaceIndexMask;
|
||||||
tid_ = frame.getTransferID();
|
tid_ = frame.getTransferID();
|
||||||
next_frame_index_ = 0;
|
next_toggle_ = false;
|
||||||
buffer_write_pos_ = 0;
|
buffer_write_pos_ = 0;
|
||||||
this_transfer_crc_ = 0;
|
this_transfer_crc_ = 0;
|
||||||
if (!first_fame)
|
if (!first_fame)
|
||||||
|
|||||||
Reference in New Issue
Block a user