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

Reorganized directory structure for tests and src

Browse Source
This commit is contained in:
Pavel Kirienko
2014-03-10 19:25:28 +04:00
parent 21fda96978
commit b86ea67563
43 changed files with 26 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/src/transport/can_io.cpp
-389
View File
@@ -1,389 +0,0 @@
/*
* CAN bus IO logic.
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <algorithm>
#include <sstream>
#include <cassert>
#include <limits>
#include <uavcan/internal/transport/can_io.hpp>
#include <uavcan/internal/debug.hpp>
namespace uavcan
{
/*
* CanRxFrame
*/
std::string CanRxFrame::toString(StringRepresentation mode) const
{
std::ostringstream os;
os << CanFrame::toString(mode)
<< " ts_m=" << ts_monotonic << " ts_utc=" << ts_utc << " iface=" << int(iface_index);
return os.str();
}
/*
* CanTxQueue::Entry
*/
void CanTxQueue::Entry::destroy(Entry*& obj, IAllocator& allocator)
{
if (obj != NULL)
{
obj->~Entry();
allocator.deallocate(obj);
obj = NULL;
}
}
bool CanTxQueue::Entry::qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const
{
if (qos != rhs_qos)
return qos > rhs_qos;
return frame.priorityHigherThan(rhs_frame);
}
bool CanTxQueue::Entry::qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const
{
if (qos != rhs_qos)
return qos < rhs_qos;
return frame.priorityLowerThan(rhs_frame);
}
std::string CanTxQueue::Entry::toString() const
{
std::string str_qos;
switch (qos)
{
case Volatile:
str_qos = "<volat> ";
break;
case Persistent:
str_qos = "<perst> ";
break;
default:
assert(0);
str_qos = "<?WTF?> ";
}
return str_qos + frame.toString();
}
/*
* CanTxQueue
*/
CanTxQueue::~CanTxQueue()
{
Entry* p = queue_.get();
while (p)
{
Entry* const next = p->getNextListNode();
remove(p);
p = next;
}
}
void CanTxQueue::registerRejectedFrame()
{
if (rejected_frames_cnt_ < std::numeric_limits<uint32_t>::max())
rejected_frames_cnt_++;
}
void CanTxQueue::push(const CanFrame& frame, uint64_t monotonic_tx_deadline, Qos qos)
{
const uint64_t timestamp = sysclock_->getMonotonicMicroseconds();
if (timestamp >= monotonic_tx_deadline)
{
UAVCAN_TRACE("CanTxQueue", "Push rejected: already expired");
registerRejectedFrame();
return;
}
void* praw = allocator_->allocate(sizeof(Entry));
if (praw == NULL)
{
UAVCAN_TRACE("CanTxQueue", "Push OOM #1, cleanup");
// No memory left in the pool, so we try to remove expired frames
Entry* p = queue_.get();
while (p)
{
Entry* const next = p->getNextListNode();
if (p->isExpired(timestamp))
{
UAVCAN_TRACE("CanTxQueue", "Push: Expired %s", p->toString().c_str());
registerRejectedFrame();
remove(p);
}
p = next;
}
praw = allocator_->allocate(sizeof(Entry)); // Try again
}
if (praw == NULL)
{
UAVCAN_TRACE("CanTxQueue", "Push OOM #2, QoS arbitration");
registerRejectedFrame();
// Find a frame with lowest QoS
Entry* p = queue_.get();
Entry* lowestqos = p;
while (p)
{
if (lowestqos->qosHigherThan(*p))
lowestqos = p;
p = p->getNextListNode();
}
// Note that frame with *equal* QoS will be replaced too.
if (lowestqos->qosHigherThan(frame, qos)) // Frame that we want to transmit has lowest QoS
{
UAVCAN_TRACE("CanTxQueue", "Push rejected: low QoS");
return; // What a loser.
}
UAVCAN_TRACE("CanTxQueue", "Push: Replacing %s", lowestqos->toString().c_str());
remove(lowestqos);
praw = allocator_->allocate(sizeof(Entry)); // Try again
}
if (praw == NULL)
return; // Seems that there is no memory at all.
Entry* entry = new (praw) Entry(frame, monotonic_tx_deadline, qos);
assert(entry);
queue_.insertBefore(entry, PriorityInsertionComparator(frame));
}
CanTxQueue::Entry* CanTxQueue::peek()
{
const uint64_t timestamp = sysclock_->getMonotonicMicroseconds();
Entry* p = queue_.get();
while (p)
{
if (p->isExpired(timestamp))
{
UAVCAN_TRACE("CanTxQueue", "Peek: Expired %s", p->toString().c_str());
Entry* const next = p->getNextListNode();
registerRejectedFrame();
remove(p);
p = next;
}
else
return p;
}
return NULL;
}
void CanTxQueue::remove(Entry*& entry)
{
if (entry == NULL)
{
assert(0);
return;
}
queue_.remove(entry);
Entry::destroy(entry, *allocator_);
}
bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const
{
const Entry* entry = queue_.get();
if (entry == NULL)
return false;
return !rhs_frame.priorityHigherThan(entry->frame);
}
/*
* CanIOManager
*/
int CanIOManager::sendToIface(int iface_index, const CanFrame& frame, uint64_t monotonic_tx_deadline)
{
assert(iface_index >= 0 && iface_index < MaxIfaces);
ICanIface* const iface = driver_.getIface(iface_index);
if (iface == NULL)
{
assert(0); // Nonexistent interface
return -1;
}
const uint64_t timestamp = sysclock_.getMonotonicMicroseconds();
const uint64_t timeout = (timestamp >= monotonic_tx_deadline) ? 0 : (monotonic_tx_deadline - timestamp);
const int res = iface->send(frame, timeout);
if (res != 1)
{
UAVCAN_TRACE("CanIOManager", "Send failed: code %i, iface %i, frame %s",
res, iface_index, frame.toString().c_str());
}
return res;
}
int CanIOManager::sendFromTxQueue(int iface_index)
{
assert(iface_index >= 0 && iface_index < MaxIfaces);
CanTxQueue::Entry* entry = tx_queues_[iface_index].peek();
if (entry == NULL)
return 0;
const int res = sendToIface(iface_index, entry->frame, entry->monotonic_deadline);
if (res > 0)
tx_queues_[iface_index].remove(entry);
return res;
}
int CanIOManager::makePendingTxMask() const
{
int write_mask = 0;
for (int i = 0; i < getNumIfaces(); i++)
{
if (!tx_queues_[i].isEmpty())
write_mask |= 1 << i;
}
return write_mask;
}
uint64_t CanIOManager::getTimeUntilMonotonicDeadline(uint64_t monotonic_deadline) const
{
const uint64_t timestamp = sysclock_.getMonotonicMicroseconds();
return (timestamp >= monotonic_deadline) ? 0 : (monotonic_deadline - timestamp);
}
int CanIOManager::getNumIfaces() const
{
const int num = driver_.getNumIfaces();
assert(num > 0 && num <= MaxIfaces);
return std::min(std::max(num, 0), (int)MaxIfaces);
}
uint64_t CanIOManager::getNumErrors(int iface_index) const
{
ICanIface* const iface = driver_.getIface(iface_index);
if (iface == NULL || iface_index >= MaxIfaces || iface_index < 0)
{
assert(0);
return std::numeric_limits<uint64_t>::max();
}
return iface->getNumErrors() + tx_queues_[iface_index].getNumRejectedFrames();
}
int CanIOManager::send(const CanFrame& frame, uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
int iface_mask, CanTxQueue::Qos qos)
{
const int num_ifaces = getNumIfaces();
const int all_ifaces_mask = (1 << num_ifaces) - 1;
assert((iface_mask & ~all_ifaces_mask) == 0);
iface_mask &= all_ifaces_mask;
if (monotonic_blocking_deadline > monotonic_tx_deadline)
monotonic_blocking_deadline = monotonic_tx_deadline;
int retval = 0;
while (true)
{
if (iface_mask == 0)
break;
int write_mask = iface_mask | makePendingTxMask();
const uint64_t timeout = getTimeUntilMonotonicDeadline(monotonic_blocking_deadline);
{
int read_mask = 0;
const int select_res = driver_.select(write_mask, read_mask, timeout);
if (select_res < 0)
return select_res;
}
// Transmission
for (int i = 0; i < num_ifaces; i++)
{
if (write_mask & (1 << i))
{
int res = 0;
if (iface_mask & (1 << i))
{
if (tx_queues_[i].topPriorityHigherOrEqual(frame))
{
res = sendFromTxQueue(i); // May return 0 if nothing to transmit (e.g. expired)
}
if (res <= 0)
{
res = sendToIface(i, frame, monotonic_tx_deadline);
if (res > 0)
iface_mask &= ~(1 << i); // Mark transmitted
}
}
else
{
res = sendFromTxQueue(i);
}
if (res > 0)
retval++;
}
}
// Timeout. Enqueue the frame if wasn't transmitted and leave.
if (write_mask == 0 || timeout == 0)
{
if ((timeout > 0) && (sysclock_.getMonotonicMicroseconds() < monotonic_blocking_deadline))
{
UAVCAN_TRACE("CanIOManager", "Send: Premature timeout in select(), will try again");
continue;
}
for (int i = 0; i < num_ifaces; i++)
{
if (iface_mask & (1 << i))
tx_queues_[i].push(frame, monotonic_tx_deadline, qos);
}
break;
}
}
return retval;
}
int CanIOManager::receive(CanRxFrame& frame, uint64_t monotonic_deadline)
{
const int num_ifaces = getNumIfaces();
while (true)
{
int write_mask = makePendingTxMask();
int read_mask = (1 << num_ifaces) - 1;
const uint64_t timeout = getTimeUntilMonotonicDeadline(monotonic_deadline);
{
const int select_res = driver_.select(write_mask, read_mask, timeout);
if (select_res < 0)
return select_res;
}
// Write - if buffers are not empty, one frame will be sent for each iface per one receive() call
for (int i = 0; i < num_ifaces; i++)
{
if (write_mask & (1 << i))
sendFromTxQueue(i);
}
// Read
for (int i = 0; i < num_ifaces; i++)
{
if (read_mask & (1 << i))
{
ICanIface* const iface = driver_.getIface(i);
if (iface == NULL)
{
assert(0); // Nonexistent interface
continue;
}
const int res = iface->receive(frame, frame.ts_monotonic, frame.ts_utc);
if (res == 0)
{
assert(0); // select() reported that iface has pending RX frames, but receive() returned none
continue;
}
frame.iface_index = i;
return res;
}
}
if (timeout == 0) // Timeout checked in the last order - this way we can operate with expired deadline
break;
}
return 0;
}
}
libuavcan/src/transport/crc.cpp
-48
View File
@@ -1,48 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <cassert>
#include <uavcan/internal/transport/crc.hpp>
namespace uavcan
{
// print ', '.join(map(lambda x: '%04x' % x, map(lambda x: int(x, 0), c.crc_ccitt_tab)))
const uint16_t TransferCRC::Table[256] =
{
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7,
0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef,
0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6,
0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de,
0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485,
0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d,
0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4,
0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc,
0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b,
0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12,
0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a,
0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41,
0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49,
0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70,
0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78,
0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f,
0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e,
0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256,
0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d,
0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c,
0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634,
0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab,
0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3,
0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92,
0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9,
0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1,
0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8,
0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0
};
}
libuavcan/src/transport/dispatcher.cpp
-191
View File
@@ -1,191 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <cassert>
#include <uavcan/internal/transport/dispatcher.hpp>
#include <uavcan/internal/debug.hpp>
namespace uavcan
{
/*
* Dispatcher::ListenerRegister
*/
bool Dispatcher::ListenerRegister::add(TransferListenerBase* listener, Mode mode)
{
if (mode == UniqueListener)
{
TransferListenerBase* p = list_.get();
while (p)
{
if (p->getDataTypeDescriptor().getID() == listener->getDataTypeDescriptor().getID())
return false;
p = p->getNextListNode();
}
}
// Objective is to arrange entries by Data Type ID in ascending order from root.
list_.insertBefore(listener, DataTypeIDInsertionComparator(listener->getDataTypeDescriptor().getID()));
return true;
}
void Dispatcher::ListenerRegister::remove(TransferListenerBase* listener)
{
list_.remove(listener);
}
void Dispatcher::ListenerRegister::cleanup(uint64_t ts_monotonic)
{
TransferListenerBase* p = list_.get();
while (p)
{
p->cleanup(ts_monotonic);
p = p->getNextListNode();
}
}
void Dispatcher::ListenerRegister::handleFrame(const RxFrame& frame)
{
TransferListenerBase* p = list_.get();
while (p)
{
if (p->getDataTypeDescriptor().getID() == frame.getDataTypeID())
p->handleFrame(frame);
else if (p->getDataTypeDescriptor().getID() < frame.getDataTypeID()) // Listeners are ordered by data type id!
break;
p = p->getNextListNode();
}
}
/*
* Dispatcher
*/
void Dispatcher::handleFrame(const CanRxFrame& can_frame)
{
RxFrame frame;
if (!frame.parse(can_frame))
{
UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str());
return;
}
if ((frame.getDstNodeID() != NodeID::Broadcast) &&
(frame.getDstNodeID() != getSelfNodeID()))
{
return;
}
switch (frame.getTransferType())
{
case TransferTypeMessageBroadcast:
case TransferTypeMessageUnicast:
lmsg_.handleFrame(frame);
break;
case TransferTypeServiceRequest:
lsrv_req_.handleFrame(frame);
break;
case TransferTypeServiceResponse:
lsrv_resp_.handleFrame(frame);
break;
default:
assert(0);
}
}
int Dispatcher::spin(uint64_t monotonic_deadline)
{
int num_frames_processed = 0;
do
{
CanRxFrame frame;
const int res = canio_.receive(frame, monotonic_deadline);
if (res < 0)
return res;
if (res > 0)
{
num_frames_processed++;
handleFrame(frame);
}
}
while (sysclock_.getMonotonicMicroseconds() < monotonic_deadline);
return num_frames_processed;
}
int Dispatcher::send(const Frame& frame, uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
CanTxQueue::Qos qos)
{
if (frame.getSrcNodeID() != getSelfNodeID())
{
assert(0);
return -1;
}
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);
}
void Dispatcher::cleanup(uint64_t ts_monotonic)
{
outgoing_transfer_reg_.cleanup(ts_monotonic);
lmsg_.cleanup(ts_monotonic);
lsrv_req_.cleanup(ts_monotonic);
lsrv_resp_.cleanup(ts_monotonic);
}
bool Dispatcher::registerMessageListener(TransferListenerBase* listener)
{
if (listener->getDataTypeDescriptor().getKind() != DataTypeKindMessage)
{
assert(0);
return false;
}
return lmsg_.add(listener, ListenerRegister::ManyListeners); // Multiple subscribers are OK
}
bool Dispatcher::registerServiceRequestListener(TransferListenerBase* listener)
{
if (listener->getDataTypeDescriptor().getKind() != DataTypeKindService)
{
assert(0);
return false;
}
return lsrv_req_.add(listener, ListenerRegister::UniqueListener); // Only one server per data type
}
bool Dispatcher::registerServiceResponseListener(TransferListenerBase* listener)
{
if (listener->getDataTypeDescriptor().getKind() != DataTypeKindService)
{
assert(0);
return false;
}
return lsrv_resp_.add(listener, ListenerRegister::ManyListeners); // Multiple callers may call same srv
}
void Dispatcher::unregisterMessageListener(TransferListenerBase* listener)
{
lmsg_.remove(listener);
}
void Dispatcher::unregisterServiceRequestListener(TransferListenerBase* listener)
{
lsrv_req_.remove(listener);
}
void Dispatcher::unregisterServiceResponseListener(TransferListenerBase* listener)
{
lsrv_resp_.remove(listener);
}
}
libuavcan/src/transport/transfer.cpp
-235
View File
@@ -1,235 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <cassert>
#include <cstdio>
#include <sstream>
#include <uavcan/internal/transport/transfer.hpp>
namespace uavcan
{
/**
* NodeID
*/
const NodeID NodeID::Broadcast(ValueBroadcast);
/**
* TransferID
*/
int TransferID::forwardDistance(TransferID rhs) const
{
int d = int(rhs.get()) - int(get());
if (d < 0)
d += 1 << BitLen;
assert(((get() + d) & Max) == rhs.get());
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
*/
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_);
return os.str();
}
}
libuavcan/src/transport/transfer_buffer.cpp
-196
View File
@@ -1,196 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <cassert>
#include <cstdlib>
#include <cstdio>
#include <string>
#include <uavcan/internal/transport/transfer_buffer.hpp>
namespace uavcan
{
/*
* TransferBufferManagerKey
*/
std::string TransferBufferManagerKey::toString() const
{
char buf[24];
std::snprintf(buf, sizeof(buf), "nid=%i tt=%i", int(node_id_.get()), int(transfer_type_));
return std::string(buf);
}
/*
* DynamicTransferBuffer::Block
*/
DynamicTransferBufferManagerEntry::Block* DynamicTransferBufferManagerEntry::Block::instantiate(IAllocator& allocator)
{
void* const praw = allocator.allocate(sizeof(Block));
if (praw == NULL)
return NULL;
return new (praw) Block;
}
void DynamicTransferBufferManagerEntry::Block::destroy(Block*& obj, IAllocator& allocator)
{
if (obj != NULL)
{
obj->~Block();
allocator.deallocate(obj);
obj = NULL;
}
}
void DynamicTransferBufferManagerEntry::Block::read(uint8_t*& outptr, unsigned int target_offset,
unsigned int& total_offset, unsigned int& left_to_read)
{
assert(outptr);
for (int i = 0; (i < Block::Size) && (left_to_read > 0); i++, total_offset++)
{
if (total_offset >= target_offset)
{
*outptr++ = data[i];
left_to_read--;
}
}
}
void DynamicTransferBufferManagerEntry::Block::write(const uint8_t*& inptr, unsigned int target_offset,
unsigned int& total_offset, unsigned int& left_to_write)
{
assert(inptr);
for (int i = 0; (i < Block::Size) && (left_to_write > 0); i++, total_offset++)
{
if (total_offset >= target_offset)
{
data[i] = *inptr++;
left_to_write--;
}
}
}
/*
* DynamicTransferBuffer
*/
DynamicTransferBufferManagerEntry* DynamicTransferBufferManagerEntry::instantiate(IAllocator& allocator, unsigned int max_size)
{
void* const praw = allocator.allocate(sizeof(DynamicTransferBufferManagerEntry));
if (praw == NULL)
return NULL;
return new (praw) DynamicTransferBufferManagerEntry(allocator, max_size);
}
void DynamicTransferBufferManagerEntry::destroy(DynamicTransferBufferManagerEntry*& obj, IAllocator& allocator)
{
if (obj != NULL)
{
obj->~DynamicTransferBufferManagerEntry();
allocator.deallocate(obj);
obj = NULL;
}
}
void DynamicTransferBufferManagerEntry::resetImpl()
{
max_write_pos_ = 0;
Block* p = blocks_.get();
while (p)
{
Block* const next = p->getNextListNode();
blocks_.remove(p);
Block::destroy(p, allocator_);
p = next;
}
}
int DynamicTransferBufferManagerEntry::read(unsigned int offset, uint8_t* data, unsigned int len) const
{
if (!data)
{
assert(0);
return -1;
}
if (offset >= max_write_pos_)
return 0;
if ((offset + len) > max_write_pos_)
len = max_write_pos_ - offset;
assert((offset + len) <= max_write_pos_);
// This shall be optimized.
unsigned int total_offset = 0, left_to_read = len;
uint8_t* outptr = data;
Block* p = blocks_.get();
while (p)
{
p->read(outptr, offset, total_offset, left_to_read);
if (left_to_read == 0)
break;
p = p->getNextListNode();
}
assert(left_to_read == 0);
return len;
}
int DynamicTransferBufferManagerEntry::write(unsigned int offset, const uint8_t* data, unsigned int len)
{
if (!data)
{
assert(0);
return -1;
}
if (offset >= max_size_)
return 0;
if ((offset + len) > max_size_)
len = max_size_ - offset;
assert((offset + len) <= max_size_);
unsigned int total_offset = 0, left_to_write = len;
const uint8_t* inptr = data;
Block* p = blocks_.get(), *last_written_block = NULL;
// First we need to write the part that is already allocated
while (p)
{
last_written_block = p;
p->write(inptr, offset, total_offset, left_to_write);
if (left_to_write == 0)
break;
p = p->getNextListNode();
}
// Then we need to append new chunks until all data is written
while (left_to_write > 0)
{
// cppcheck-suppress nullPointer
assert(p == NULL);
// Allocating the chunk
Block* new_block = Block::instantiate(allocator_);
if (new_block == NULL)
break; // We're in deep shit.
// Appending the chain with the new block
if (last_written_block != NULL)
{
assert(last_written_block->getNextListNode() == NULL); // Because it is last in the chain
last_written_block->setNextListNode(new_block);
new_block->setNextListNode(NULL);
}
else
{
blocks_.insert(new_block);
}
last_written_block = new_block;
// Writing the data
new_block->write(inptr, offset, total_offset, left_to_write);
}
const int actually_written = len - left_to_write;
max_write_pos_ = std::max(offset + actually_written, max_write_pos_);
return actually_written;
}
}
libuavcan/src/transport/transfer_listener.cpp
-146
View File
@@ -1,146 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <cstdlib>
#include <cassert>
#include <uavcan/internal/transport/transfer_listener.hpp>
#include <uavcan/internal/debug.hpp>
namespace uavcan
{
/*
* IncomingTransfer
*/
int IncomingTransfer::write(unsigned int, const uint8_t*, unsigned int)
{
assert(0); // Incoming transfer container is read-only
return -1;
}
/*
* SingleFrameIncomingTransfer
*/
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
{
if (data == NULL)
{
assert(0);
return -1;
}
if (offset >= payload_len_)
return 0;
if ((offset + len) > payload_len_)
len = payload_len_ - offset;
assert((offset + len) <= payload_len_);
std::copy(payload_ + offset, payload_ + offset + len, data);
return len;
}
/*
* MultiFrameIncomingTransfer
*/
MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(uint64_t ts_monotonic, uint64_t ts_utc,
const RxFrame& last_frame, TransferBufferAccessor& tba)
: IncomingTransfer(ts_monotonic, ts_utc, last_frame.getTransferType(), last_frame.getTransferID(),
last_frame.getSrcNodeID())
, buf_acc_(tba)
{
assert(last_frame.isValid());
assert(last_frame.isLast());
}
int MultiFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsigned int len) const
{
const ITransferBuffer* const tbb = const_cast<TransferBufferAccessor&>(buf_acc_).access();
if (tbb == NULL)
{
UAVCAN_TRACE("MultiFrameIncomingTransfer", "Read failed: no such buffer");
return -1;
}
return tbb->read(offset, data, len);
}
/*
* TransferListenerBase
*/
bool TransferListenerBase::checkPayloadCrc(const uint16_t compare_with, const ITransferBuffer& tbb) const
{
TransferCRC crc = crc_base_;
unsigned int offset = 0;
while (true)
{
uint8_t buf[16];
const int res = tbb.read(offset, buf, sizeof(buf));
if (res < 0)
{
UAVCAN_TRACE("TransferListenerBase", "Failed to check CRC: Buffer read failure %i", res);
return false;
}
if (res == 0)
break;
offset += res;
crc.add(buf, res);
}
if (crc.get() != compare_with)
{
UAVCAN_TRACE("TransferListenerBase", "CRC mismatch, expected=0x%04x, got=0x%04x",
int(compare_with), int(crc.get()));
return false;
}
return true;
}
void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxFrame& frame,
TransferBufferAccessor& tba)
{
switch (receiver.addFrame(frame, tba))
{
case TransferReceiver::ResultNotComplete:
return;
case TransferReceiver::ResultSingleFrame:
{
SingleFrameIncomingTransfer it(frame);
handleIncomingTransfer(it);
return;
}
case TransferReceiver::ResultComplete:
{
const ITransferBuffer* tbb = tba.access();
if (tbb == NULL)
{
UAVCAN_TRACE("TransferListenerBase", "Buffer access failure, last frame: %s", frame.toString().c_str());
return;
}
if (!checkPayloadCrc(receiver.getLastTransferCrc(), *tbb))
{
UAVCAN_TRACE("TransferListenerBase", "CRC error, last frame: %s", frame.toString().c_str());
return;
}
MultiFrameIncomingTransfer it(receiver.getLastTransferTimestampMonotonic(),
receiver.getLastTransferTimestampUtc(), frame, tba);
handleIncomingTransfer(it);
it.release();
return;
}
default:
assert(0);
break;
}
}
}
libuavcan/src/transport/transfer_receiver.cpp
-218
View File
@@ -1,218 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <cstdlib>
#include <cstdio>
#include <cassert>
#include <algorithm>
#include <uavcan/internal/debug.hpp>
#include <uavcan/internal/transport/transfer_receiver.hpp>
#include <uavcan/internal/transport/crc.hpp>
namespace uavcan
{
const uint32_t TransferReceiver::DefaultTransferInterval;
const uint32_t TransferReceiver::MinTransferInterval;
const uint32_t TransferReceiver::MaxTransferInterval;
TransferReceiver::TidRelation TransferReceiver::getTidRelation(const RxFrame& frame) const
{
const int distance = tid_.forwardDistance(frame.getTransferID());
if (distance == 0)
return TidSame;
if (distance < ((1 << TransferID::BitLen) / 2))
return TidFuture;
return TidRepeat;
}
void TransferReceiver::updateTransferTimings()
{
assert(this_transfer_ts_monotonic_ > 0);
const uint64_t prev_prev_ts = prev_transfer_ts_monotonic_;
prev_transfer_ts_monotonic_ = this_transfer_ts_monotonic_;
if ((prev_prev_ts != 0) &&
(prev_transfer_ts_monotonic_ != 0) &&
(prev_transfer_ts_monotonic_ >= prev_prev_ts))
{
uint64_t interval = prev_transfer_ts_monotonic_ - prev_prev_ts;
interval = std::max(std::min(interval, uint64_t(MaxTransferInterval)), uint64_t(MinTransferInterval));
transfer_interval_ = static_cast<uint32_t>((uint64_t(transfer_interval_) * 7 + interval) / 8);
}
}
void TransferReceiver::prepareForNextTransfer()
{
tid_.increment();
next_frame_index_ = 0;
buffer_write_pos_ = 0;
}
bool TransferReceiver::validate(const RxFrame& frame) const
{
if (iface_index_ != frame.getIfaceIndex())
return false;
if (frame.isFirst() && !frame.isLast() && (frame.getPayloadLen() < TransferCRC::NumBytes))
{
UAVCAN_TRACE("TransferReceiver", "CRC expected, %s", frame.toString().c_str());
return false;
}
if ((frame.getIndex() == Frame::MaxIndex) && !frame.isLast())
{
UAVCAN_TRACE("TransferReceiver", "Unterminated transfer, %s", frame.toString().c_str());
return false;
}
if (frame.getIndex() != next_frame_index_)
{
UAVCAN_TRACE("TransferReceiver", "Unexpected frame index (not %i), %s",
int(next_frame_index_), frame.toString().c_str());
return false;
}
if (getTidRelation(frame) != TidSame)
{
UAVCAN_TRACE("TransferReceiver", "Unexpected TID (current %i), %s", tid_.get(), frame.toString().c_str());
return false;
}
return true;
}
bool TransferReceiver::writePayload(const RxFrame& frame, ITransferBuffer& buf)
{
const uint8_t* const payload = frame.getPayloadPtr();
const int payload_len = frame.getPayloadLen();
if (frame.isFirst()) // First frame contains CRC, we need to extract it now
{
if (frame.getPayloadLen() < TransferCRC::NumBytes) // Must have been validated earlier though. I think I'm paranoid.
return false;
this_transfer_crc_ = (payload[0] & 0xFF) | (uint16_t(payload[1] & 0xFF) << 8); // Little endian.
const int effective_payload_len = payload_len - TransferCRC::NumBytes;
const int res = buf.write(buffer_write_pos_, payload + TransferCRC::NumBytes, effective_payload_len);
const bool success = res == effective_payload_len;
if (success)
buffer_write_pos_ += effective_payload_len;
return success;
}
else
{
const int res = buf.write(buffer_write_pos_, payload, payload_len);
const bool success = res == payload_len;
if (success)
buffer_write_pos_ += payload_len;
return success;
}
}
TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, TransferBufferAccessor& tba)
{
// Transfer timestamps are derived from the first frame
if (frame.isFirst())
{
this_transfer_ts_monotonic_ = frame.getMonotonicTimestamp();
first_frame_ts_utc_ = frame.getUtcTimestamp();
}
if (frame.isFirst() && frame.isLast())
{
tba.remove();
updateTransferTimings();
prepareForNextTransfer();
this_transfer_crc_ = 0; // SFT has no CRC
return ResultSingleFrame;
}
// Payload write
ITransferBuffer* buf = tba.access();
if (buf == NULL)
buf = tba.create();
if (buf == NULL)
{
UAVCAN_TRACE("TransferReceiver", "Failed to access the buffer, %s", frame.toString().c_str());
prepareForNextTransfer();
return ResultNotComplete;
}
if (!writePayload(frame, *buf))
{
UAVCAN_TRACE("TransferReceiver", "Payload write failed, %s", frame.toString().c_str());
tba.remove();
prepareForNextTransfer();
return ResultNotComplete;
}
next_frame_index_++;
if (frame.isLast())
{
updateTransferTimings();
prepareForNextTransfer();
return ResultComplete;
}
return ResultNotComplete;
}
bool TransferReceiver::isTimedOut(uint64_t ts_monotonic) const
{
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;
return (ts_monotonic - ts) > (uint64_t(transfer_interval_) * INTERVAL_MULT);
}
TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba)
{
if ((frame.getMonotonicTimestamp() == 0) ||
(frame.getMonotonicTimestamp() < prev_transfer_ts_monotonic_) ||
(frame.getMonotonicTimestamp() < this_transfer_ts_monotonic_))
{
return ResultNotComplete;
}
const bool not_initialized = !isInitialized();
const bool receiver_timed_out = isTimedOut(frame.getMonotonicTimestamp());
const bool same_iface = frame.getIfaceIndex() == iface_index_;
const bool first_fame = frame.isFirst();
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) ||
(receiver_timed_out) ||
(same_iface && first_fame && (tid_rel == TidFuture)) ||
(iface_timed_out && first_fame && (tid_rel == TidFuture));
if (need_restart)
{
UAVCAN_TRACE("TransferReceiver",
"Restart [not_inited=%i, iface_timeout=%i, recv_timeout=%i, same_iface=%i, first_frame=%i, tid_rel=%i], %s",
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.getIfaceIndex();
tid_ = frame.getTransferID();
next_frame_index_ = 0;
buffer_write_pos_ = 0;
this_transfer_crc_ = 0;
if (!first_fame)
{
tid_.increment();
return ResultNotComplete;
}
}
if (!validate(frame))
return ResultNotComplete;
return receive(frame, tba);
}
}
libuavcan/src/transport/transfer_sender.cpp
-110
View File
@@ -1,110 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <cassert>
#include <uavcan/internal/debug.hpp>
#include <uavcan/internal/transport/transfer_sender.hpp>
namespace uavcan
{
int TransferSender::send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline,
uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
TransferID tid)
{
Frame frame(data_type_.getID(), transfer_type, dispatcher_.getSelfNodeID(), dst_node_id, 0, tid);
if (frame.getMaxPayloadLen() >= payload_len) // Single Frame Transfer
{
const int res = frame.setPayload(payload, payload_len);
if (res != payload_len)
{
assert(0);
UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", res);
return (res < 0) ? res : -1;
}
frame.makeLast();
assert(frame.isLast() && frame.isFirst());
return dispatcher_.send(frame, monotonic_tx_deadline, monotonic_blocking_deadline, qos_);
}
else // Multi Frame Transfer
{
int offset = 0;
{
TransferCRC crc = crc_base_;
crc.add(payload, payload_len);
static const int BUFLEN = sizeof(CanFrame::data);
uint8_t buf[BUFLEN];
buf[0] = crc.get() & 0xFF; // Transfer CRC, little endian
buf[1] = (crc.get() >> 8) & 0xFF;
std::copy(payload, payload + BUFLEN - 2, buf + 2);
const int write_res = frame.setPayload(buf, BUFLEN);
if (write_res < 2)
{
UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res);
return write_res;
}
offset = write_res - 2;
assert(payload_len > offset);
}
int next_frame_index = 1;
while (true)
{
const int send_res = dispatcher_.send(frame, monotonic_tx_deadline, monotonic_blocking_deadline, qos_);
if (send_res < 0)
return send_res;
if (frame.isLast())
return next_frame_index; // Number of frames transmitted
frame.setIndex(next_frame_index++);
const int write_res = frame.setPayload(payload + offset, payload_len - offset);
if (write_res < 0)
{
UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res);
return write_res;
}
offset += write_res;
assert(offset <= payload_len);
if (offset >= payload_len)
frame.makeLast();
}
}
assert(0);
return -1; // Return path analysis is apparently broken. There should be no warning, this 'return' is unreachable.
}
int TransferSender::send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline,
uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id)
{
const OutgoingTransferRegistryKey otr_key(data_type_.getID(), transfer_type, dst_node_id);
assert(monotonic_tx_deadline > 0);
const uint64_t otr_deadline = monotonic_tx_deadline + max_transfer_interval_;
TransferID* const tid = dispatcher_.getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline);
if (tid == NULL)
{
UAVCAN_TRACE("TransferSender", "OTR access failure, dtd=%s tt=%i",
data_type_.toString().c_str(), int(transfer_type));
return -1;
}
const TransferID this_tid = tid->get();
tid->increment();
return send(payload, payload_len, monotonic_tx_deadline, monotonic_blocking_deadline, transfer_type,
dst_node_id, this_tid);
}
}