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

CAN IO flags

Browse Source
This commit is contained in:
Pavel Kirienko
2014-03-21 16:02:05 +04:00
parent 6246f24297
commit 9d35c616ef
13 changed files with 258 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/include/uavcan/driver/can.hpp
+9 -2
View File
@@ -94,6 +94,12 @@ struct CanSelectMasks
{ }
};
enum
{
CanIOFlagLoopback = 1 ///< Send the frame back to RX with true TX timestamps
};
typedef uint16_t CanIOFlags;
/**
* Single non-blocking CAN interface.
*/
@@ -107,7 +113,7 @@ public:
* If the frame wasn't transmitted upon TX deadline, the driver should discard it.
* @return 1 = one frame transmitted, 0 = TX buffer full, negative for error.
*/
virtual int send(const CanFrame& frame, MonotonicTime tx_deadline) = 0;
virtual int send(const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags) = 0;
/**
* Non-blocking reception.
@@ -121,7 +127,8 @@ public:
* @param [out] out_ts_utc UTC timestamp, optional, zero if unknown.
* @return 1 = one frame received, 0 = RX buffer empty, negative for error.
*/
virtual int receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic, UtcTime& out_ts_utc) = 0;
virtual int receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic, UtcTime& out_ts_utc,
CanIOFlags& out_flags) = 0;
/**
* Configure the hardware CAN filters. @ref CanFilterConfig.
libuavcan/include/uavcan/transport/can_io.hpp
+7 -5
View File
@@ -42,11 +42,13 @@ public:
MonotonicTime deadline;
CanFrame frame;
uint8_t qos;
CanIOFlags flags;
Entry(const CanFrame& frame, MonotonicTime deadline, Qos qos)
Entry(const CanFrame& frame, MonotonicTime deadline, Qos qos, CanIOFlags flags)
: deadline(deadline)
, frame(frame)
, qos(uint8_t(qos))
, flags(flags)
{
assert(qos == Volatile || qos == Persistent);
IsDynamicallyAllocatable<Entry>::check();
@@ -99,7 +101,7 @@ public:
~CanTxQueue();
void push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos);
void push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags);
Entry* peek(); // Modifier
void remove(Entry*& entry);
@@ -127,7 +129,7 @@ private:
CanIOManager(CanIOManager&);
CanIOManager& operator=(CanIOManager&);
int sendToIface(int iface_index, const CanFrame& frame, MonotonicTime tx_deadline);
int sendToIface(int iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags);
int sendFromTxQueue(int iface_index);
int makePendingTxMask() const;
@@ -156,8 +158,8 @@ public:
* negative - failure
*/
int send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
int iface_mask, CanTxQueue::Qos qos);
int receive(CanRxFrame& frame, MonotonicTime blocking_deadline);
int iface_mask, CanTxQueue::Qos qos, CanIOFlags flags);
int receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags);
};
}
libuavcan/include/uavcan/transport/dispatcher.hpp
+2 -1
View File
@@ -68,7 +68,8 @@ public:
/**
* Refer to CanIOManager::send() for the parameter description
*/
int send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos);
int send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos,
CanIOFlags flags);
void cleanup(MonotonicTime ts);
libuavcan/include/uavcan/transport/transfer_sender.hpp
+5
View File
@@ -20,6 +20,7 @@ class TransferSender
const DataTypeDescriptor& data_type_;
const CanTxQueue::Qos qos_;
const TransferCRC crc_base_;
CanIOFlags flags_;
Dispatcher& dispatcher_;
@@ -35,9 +36,13 @@ public:
, data_type_(data_type)
, qos_(qos)
, crc_base_(data_type.getSignature().toTransferCRC())
, flags_(CanIOFlags(0))
, dispatcher_(dispatcher)
{ }
CanIOFlags getCanIOFlags() const { return flags_; }
void setCanIOFlags(CanIOFlags flags) { flags_ = flags; }
/**
* Send with explicit Transfer ID.
* Should be used only for service responses, where response TID should match request TID.
libuavcan/src/transport/can_io.cpp
+11 -11
View File
@@ -88,7 +88,7 @@ void CanTxQueue::registerRejectedFrame()
rejected_frames_cnt_++;
}
void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos)
void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags)
{
const MonotonicTime timestamp = sysclock_->getMonotonic();
@@ -147,7 +147,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos)
if (praw == NULL)
return; // Seems that there is no memory at all.
Entry* entry = new (praw) Entry(frame, tx_deadline, qos);
Entry* entry = new (praw) Entry(frame, tx_deadline, qos, flags);
assert(entry);
queue_.insertBefore(entry, PriorityInsertionComparator(frame));
}
@@ -194,7 +194,7 @@ bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const
/*
* CanIOManager
*/
int CanIOManager::sendToIface(int iface_index, const CanFrame& frame, MonotonicTime tx_deadline)
int CanIOManager::sendToIface(int iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags)
{
assert(iface_index >= 0 && iface_index < MaxIfaces);
ICanIface* const iface = driver_.getIface(iface_index);
@@ -203,7 +203,7 @@ int CanIOManager::sendToIface(int iface_index, const CanFrame& frame, MonotonicT
assert(0); // Nonexistent interface
return -1;
}
const int res = iface->send(frame, tx_deadline);
const int res = iface->send(frame, tx_deadline, flags);
if (res != 1)
{
UAVCAN_TRACE("CanIOManager", "Send failed: code %i, iface %i, frame %s",
@@ -218,7 +218,7 @@ int CanIOManager::sendFromTxQueue(int iface_index)
CanTxQueue::Entry* entry = tx_queues_[iface_index].peek();
if (entry == NULL)
return 0;
const int res = sendToIface(iface_index, entry->frame, entry->deadline);
const int res = sendToIface(iface_index, entry->frame, entry->deadline, entry->flags);
if (res > 0)
tx_queues_[iface_index].remove(entry);
return res;
@@ -254,7 +254,7 @@ uint64_t CanIOManager::getNumErrors(int iface_index) const
}
int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
int iface_mask, CanTxQueue::Qos qos)
int iface_mask, CanTxQueue::Qos qos, CanIOFlags flags)
{
const int num_ifaces = getNumIfaces();
const int all_ifaces_mask = (1 << num_ifaces) - 1;
@@ -294,7 +294,7 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton
}
if (res <= 0)
{
res = sendToIface(i, frame, tx_deadline);
res = sendToIface(i, frame, tx_deadline, flags);
if (res > 0)
iface_mask &= ~(1 << i); // Mark transmitted
}
@@ -320,7 +320,7 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton
for (int i = 0; i < num_ifaces; i++)
{
if (iface_mask & (1 << i))
tx_queues_[i].push(frame, tx_deadline, qos);
tx_queues_[i].push(frame, tx_deadline, qos, flags);
}
break;
}
@@ -328,7 +328,7 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton
return retval;
}
int CanIOManager::receive(CanRxFrame& frame, MonotonicTime blocking_deadline)
int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags)
{
const int num_ifaces = getNumIfaces();
@@ -361,13 +361,13 @@ int CanIOManager::receive(CanRxFrame& frame, MonotonicTime blocking_deadline)
assert(0); // Nonexistent interface
continue;
}
const int res = iface->receive(frame, frame.ts_mono, frame.ts_utc);
const int res = iface->receive(out_frame, out_frame.ts_mono, out_frame.ts_utc, out_flags);
if (res == 0)
{
assert(0); // select() reported that iface has pending RX frames, but receive() returned none
continue;
}
frame.iface_index = i;
out_frame.iface_index = i;
return res;
}
}
libuavcan/src/transport/dispatcher.cpp
+14 -5
View File
@@ -111,14 +111,23 @@ int Dispatcher::spin(MonotonicTime deadline)
int num_frames_processed = 0;
do
{
CanIOFlags flags = CanIOFlags();
CanRxFrame frame;
const int res = canio_.receive(frame, deadline);
const int res = canio_.receive(frame, deadline, flags);
if (res < 0)
return res;
if (res > 0)
{
num_frames_processed++;
handleFrame(frame);
if (flags & CanIOFlagLoopback)
{
// TODO: Loopback handling!
assert(0); // Not implemented
}
else
{
num_frames_processed++;
handleFrame(frame);
}
}
}
while (sysclock_.getMonotonic() < deadline);
@@ -127,7 +136,7 @@ int Dispatcher::spin(MonotonicTime deadline)
}
int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
CanTxQueue::Qos qos)
CanTxQueue::Qos qos, CanIOFlags flags)
{
if (frame.getSrcNodeID() != getNodeID())
{
@@ -144,7 +153,7 @@ int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTim
}
const int iface_mask = (1 << canio_.getNumIfaces()) - 1;
return canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos);
return canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos, flags);
}
void Dispatcher::cleanup(MonotonicTime ts)
libuavcan/src/transport/transfer_sender.cpp
+2 -2
View File
@@ -27,7 +27,7 @@ int TransferSender::send(const uint8_t* payload, int payload_len, MonotonicTime
}
frame.makeLast();
assert(frame.isLast() && frame.isFirst());
return dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_);
return dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_);
}
else // Multi Frame Transfer
{
@@ -57,7 +57,7 @@ int TransferSender::send(const uint8_t* payload, int payload_len, MonotonicTime
while (true)
{
const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_);
const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_);
if (send_res < 0)
return send_res;
libuavcan/test/node/test_node.hpp
+24 -6
View File
@@ -42,6 +42,7 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
uavcan::ISystemClock& clock;
PairableCanDriver* other;
std::queue<uavcan::CanFrame> read_queue;
std::queue<uavcan::CanFrame> loopback_queue;
PairableCanDriver(uavcan::ISystemClock& clock)
: clock(clock)
@@ -67,7 +68,9 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
{
assert(other);
if (inout_masks.read == 1)
inout_masks.read = read_queue.size() ? 1 : 0;
{
inout_masks.read = (read_queue.size() || loopback_queue.size()) ? 1 : 0;
}
if (inout_masks.read || inout_masks.write)
return 1;
@@ -78,19 +81,34 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
return 0;
}
int send(const uavcan::CanFrame& frame, uavcan::MonotonicTime)
int send(const uavcan::CanFrame& frame, uavcan::MonotonicTime, uavcan::CanIOFlags flags)
{
assert(other);
other->read_queue.push(frame);
if (flags & uavcan::CanIOFlagLoopback)
{
loopback_queue.push(frame);
}
return 1;
}
int receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, uavcan::UtcTime& out_ts_utc)
int receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, uavcan::UtcTime& out_ts_utc,
uavcan::CanIOFlags& out_flags)
{
assert(other);
assert(read_queue.size());
out_frame = read_queue.front();
read_queue.pop();
out_flags = 0;
if (loopback_queue.empty())
{
assert(read_queue.size());
out_frame = read_queue.front();
read_queue.pop();
}
else
{
out_flags |= uavcan::CanIOFlagLoopback;
out_frame = loopback_queue.front();
loopback_queue.pop();
}
out_ts_monotonic = clock.getMonotonic();
out_ts_utc = clock.getUtc();
return 1;
libuavcan/test/transport/can/can.hpp
+28 -12
View File
@@ -36,6 +36,7 @@ public:
std::queue<FrameWithTime> tx; ///< Queue of outgoing frames (bus <-- library)
std::queue<FrameWithTime> rx; ///< Queue of incoming frames (bus --> library)
std::queue<FrameWithTime> loopback; ///< Loopback
bool writeable;
bool tx_failure;
bool rx_failure;
@@ -91,7 +92,7 @@ public:
return frame_time.frame;
}
int send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline)
int send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags)
{
assert(this);
EXPECT_TRUE(writeable); // Shall never be called when not writeable
@@ -100,21 +101,36 @@ public:
if (!writeable)
return 0;
tx.push(FrameWithTime(frame, tx_deadline));
if (flags & uavcan::CanIOFlagLoopback)
loopback.push(FrameWithTime(frame, iclock.getMonotonic()));
return 1;
}
int receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, uavcan::UtcTime& out_ts_utc)
int receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, uavcan::UtcTime& out_ts_utc,
uavcan::CanIOFlags& out_flags)
{
assert(this);
EXPECT_TRUE(rx.size()); // Shall never be called when not readable
if (rx_failure)
return -1;
if (rx.empty())
return 0;
const FrameWithTime frame = rx.front();
rx.pop();
out_frame = frame.frame;
out_ts_monotonic = frame.time;
out_flags = uavcan::CanIOFlags();
if (loopback.empty())
{
EXPECT_TRUE(rx.size()); // Shall never be called when not readable
if (rx_failure)
return -1;
if (rx.empty())
return 0;
const FrameWithTime frame = rx.front();
rx.pop();
out_frame = frame.frame;
out_ts_monotonic = frame.time;
}
else
{
out_flags |= uavcan::CanIOFlagLoopback;
const FrameWithTime frame = loopback.front();
loopback.pop();
out_frame = frame.frame;
out_ts_monotonic = frame.time;
}
out_ts_utc = uavcan::UtcTime();
return 1;
}
@@ -159,7 +175,7 @@ public:
const int mask = 1 << i;
if ((inout_masks.write & mask) && ifaces.at(i).writeable)
out_write_mask |= mask;
if ((inout_masks.read & mask) && ifaces.at(i).rx.size())
if ((inout_masks.read & mask) && (ifaces.at(i).rx.size() || ifaces.at(i).loopback.size()))
out_read_mask |= mask;
}
inout_masks.write = out_write_mask;
libuavcan/test/transport/can/iface_mock.cpp
+42 -2
View File
@@ -5,7 +5,7 @@
#include <gtest/gtest.h>
#include "can.hpp"
TEST(CanIOManager, CanDriverMock)
TEST(CanDriverMock, Basic)
{
using uavcan::CanFrame;
using uavcan::CanSelectMasks;
@@ -46,7 +46,9 @@ TEST(CanIOManager, CanDriverMock)
CanFrame fr2;
uavcan::MonotonicTime ts_monotonic;
uavcan::UtcTime ts_utc;
EXPECT_EQ(1, driver.getIface(1)->receive(fr2, ts_monotonic, ts_utc));
uavcan::CanIOFlags flags = 0;
EXPECT_EQ(1, driver.getIface(1)->receive(fr2, ts_monotonic, ts_utc, flags));
EXPECT_EQ(0, flags);
EXPECT_EQ(fr1, fr2);
EXPECT_EQ(100, ts_monotonic.toUSec());
EXPECT_EQ(0, ts_utc.toUSec());
@@ -60,3 +62,41 @@ TEST(CanIOManager, CanDriverMock)
EXPECT_EQ(1, masks.write); // Leaving masks unchanged - the library must ignore them
EXPECT_EQ(7, masks.read);
}
TEST(CanDriverMock, Loopback)
{
using uavcan::CanFrame;
using uavcan::CanSelectMasks;
SystemClockMock clockmock;
CanDriverMock driver(1, clockmock);
CanSelectMasks masks;
masks.write = 1;
masks.read = 1;
EXPECT_LT(0, driver.select(masks, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_EQ(1, masks.write);
EXPECT_EQ(0, masks.read);
clockmock.advance(200);
CanFrame fr1;
fr1.id = 123 | CanFrame::FlagEFF;
EXPECT_EQ(1, driver.getIface(0)->send(fr1, uavcan::MonotonicTime::fromUSec(10000), uavcan::CanIOFlagLoopback));
masks.write = 0;
masks.read = 1;
EXPECT_LT(0, driver.select(masks, uavcan::MonotonicTime::fromUSec(100)));
EXPECT_EQ(0, masks.write);
EXPECT_EQ(1, masks.read);
CanFrame fr2;
uavcan::MonotonicTime ts_monotonic;
uavcan::UtcTime ts_utc;
uavcan::CanIOFlags flags = 0;
EXPECT_EQ(1, driver.getIface(0)->receive(fr2, ts_monotonic, ts_utc, flags));
EXPECT_EQ(uavcan::CanIOFlagLoopback, flags);
EXPECT_EQ(fr1, fr2);
EXPECT_EQ(200, ts_monotonic.toUSec());
EXPECT_EQ(0, ts_utc.toUSec());
}
libuavcan/test/transport/can/io.cpp
+95 -26
View File
@@ -37,7 +37,9 @@ TEST(CanIOManager, Reception)
* Empty, will time out
*/
uavcan::CanRxFrame frame;
EXPECT_EQ(0, iomgr.receive(frame, tsMono(100)));
uavcan::CanIOFlags flags = uavcan::CanIOFlags();
EXPECT_EQ(0, iomgr.receive(frame, tsMono(100), flags));
EXPECT_EQ(0, flags);
EXPECT_EQ(100, clockmock.monotonic);
EXPECT_EQ(100, clockmock.utc);
@@ -60,36 +62,43 @@ TEST(CanIOManager, Reception)
driver.ifaces.at(1).pushRx(frames[1][2]);
clockmock.advance(10);
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime()));
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
EXPECT_TRUE(rxFrameEquals(frame, frames[0][0], 110, 0));
EXPECT_EQ(0, flags);
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime()));
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
EXPECT_TRUE(rxFrameEquals(frame, frames[0][1], 120, 0));
EXPECT_EQ(0, flags);
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime()));
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
EXPECT_TRUE(rxFrameEquals(frame, frames[0][2], 130, 0));
EXPECT_EQ(0, flags);
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime()));
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
EXPECT_TRUE(rxFrameEquals(frame, frames[1][0], 110, 1));
EXPECT_EQ(0, flags);
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime()));
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
EXPECT_TRUE(rxFrameEquals(frame, frames[1][1], 120, 1));
EXPECT_EQ(0, flags);
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime()));
EXPECT_EQ(1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
EXPECT_TRUE(rxFrameEquals(frame, frames[1][2], 130, 1));
EXPECT_EQ(0, flags);
EXPECT_EQ(0, iomgr.receive(frame, uavcan::MonotonicTime())); // Will time out
EXPECT_EQ(0, iomgr.receive(frame, uavcan::MonotonicTime(), flags)); // Will time out
EXPECT_EQ(0, flags);
/*
* Errors
*/
driver.select_failure = true;
EXPECT_EQ(-1, iomgr.receive(frame, uavcan::MonotonicTime()));
EXPECT_EQ(-1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
driver.select_failure = false;
driver.ifaces.at(1).pushRx(frames[0][0]);
driver.ifaces.at(1).rx_failure = true;
EXPECT_EQ(-1, iomgr.receive(frame, uavcan::MonotonicTime()));
EXPECT_EQ(-1, iomgr.receive(frame, uavcan::MonotonicTime(), flags));
driver.ifaces.at(0).num_errors = 9000;
driver.ifaces.at(1).num_errors = 100500;
@@ -123,14 +132,16 @@ TEST(CanIOManager, Transmission)
makeCanFrame(1, "a0", EXT), makeCanFrame(99, "a1", EXT), makeCanFrame(803, "a2", STD)
};
uavcan::CanIOFlags flags = uavcan::CanIOFlags();
/*
* Simple transmission
*/
EXPECT_EQ(2, iomgr.send(frames[0], tsMono(100), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Volatile)); // To both
EXPECT_EQ(2, iomgr.send(frames[0], tsMono(100), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));// To both
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 100));
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 100));
EXPECT_EQ(1, iomgr.send(frames[1], tsMono(200), tsMono(100), 2, CanTxQueue::Persistent)); // To #1
EXPECT_EQ(1, iomgr.send(frames[1], tsMono(200), tsMono(100), 2, CanTxQueue::Persistent, flags)); // To #1
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 200));
EXPECT_EQ(0, clockmock.monotonic);
@@ -147,7 +158,7 @@ TEST(CanIOManager, Transmission)
// Sending to both, #0 blocked
driver.ifaces.at(0).writeable = false;
EXPECT_LT(0, iomgr.send(frames[0], tsMono(201), tsMono(200), ALL_IFACES_MASK, CanTxQueue::Persistent));
EXPECT_LT(0, iomgr.send(frames[0], tsMono(201), tsMono(200), ALL_IFACES_MASK, CanTxQueue::Persistent, flags));
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 201));
EXPECT_EQ(200, clockmock.monotonic);
EXPECT_EQ(200, clockmock.utc);
@@ -157,11 +168,11 @@ TEST(CanIOManager, Transmission)
// Sending to both, both blocked
driver.ifaces.at(1).writeable = false;
EXPECT_EQ(0, iomgr.send(frames[1], tsMono(777), tsMono(300), ALL_IFACES_MASK, CanTxQueue::Volatile));
EXPECT_EQ(0, iomgr.send(frames[1], tsMono(777), tsMono(300), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));
EXPECT_EQ(3, pool.getNumUsedBlocks()); // Total 3 frames in TX queue now
// Sending to #0, both blocked
EXPECT_EQ(0, iomgr.send(frames[2], tsMono(888), tsMono(400), 1, CanTxQueue::Persistent));
EXPECT_EQ(0, iomgr.send(frames[2], tsMono(888), tsMono(400), 1, CanTxQueue::Persistent, flags));
EXPECT_EQ(400, clockmock.monotonic);
EXPECT_EQ(400, clockmock.utc);
EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
@@ -175,7 +186,7 @@ TEST(CanIOManager, Transmission)
// Sending to #1, both writeable
driver.ifaces.at(0).writeable = true;
driver.ifaces.at(1).writeable = true;
EXPECT_LT(0, iomgr.send(frames[0], tsMono(999), tsMono(500), 2, CanTxQueue::Persistent)); // One frame per each iface will be sent
EXPECT_LT(0, iomgr.send(frames[0], tsMono(999), tsMono(500), 2, CanTxQueue::Persistent, flags)); // One frame per each iface will be sent
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 777)); // Note that frame[0] on iface #0 has expired
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 999)); // In different order due to prioritization
EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
@@ -183,9 +194,10 @@ TEST(CanIOManager, Transmission)
// Calling receive() to flush the rest two frames
uavcan::CanRxFrame dummy_rx_frame;
EXPECT_EQ(0, iomgr.receive(dummy_rx_frame, tsMono(0)));
EXPECT_EQ(0, iomgr.receive(dummy_rx_frame, tsMono(0), flags));
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 888));
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 777));
ASSERT_EQ(0, flags);
// Final checks
EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
@@ -201,9 +213,9 @@ TEST(CanIOManager, Transmission)
driver.ifaces.at(1).writeable = false;
// Sending 5 frames, one will be rejected
EXPECT_EQ(0, iomgr.send(frames[2], tsMono(2222), tsMono(1000), ALL_IFACES_MASK, CanTxQueue::Persistent));
EXPECT_EQ(0, iomgr.send(frames[0], tsMono(3333), tsMono(1100), 2, CanTxQueue::Persistent));
EXPECT_EQ(0, iomgr.send(frames[1], tsMono(4444), tsMono(1200), ALL_IFACES_MASK, CanTxQueue::Volatile)); // One frame kicked here
EXPECT_EQ(0, iomgr.send(frames[2], tsMono(2222), tsMono(1000), ALL_IFACES_MASK, CanTxQueue::Persistent, flags));
EXPECT_EQ(0, iomgr.send(frames[0], tsMono(3333), tsMono(1100), 2, CanTxQueue::Persistent, flags));
EXPECT_EQ(0, iomgr.send(frames[1], tsMono(4444), tsMono(1200), ALL_IFACES_MASK, CanTxQueue::Volatile, flags)); // One frame kicked here
// State checks
EXPECT_EQ(4, pool.getNumUsedBlocks()); // TX queue is full
@@ -221,15 +233,17 @@ TEST(CanIOManager, Transmission)
// This shall transmit _some_ frames now, at least one per iface (exact number can be changed - it will be OK)
uavcan::CanRxFrame rx_frame;
EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0))); // Non-blocking
EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0), flags)); // Non-blocking
EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[0], 1200, 0));
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 4444));
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 3333));
ASSERT_EQ(0, flags);
EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0)));
EXPECT_EQ(1, iomgr.receive(rx_frame, tsMono(0), flags));
EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[1], 1200, 1));
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 2222));
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[2], 2222)); // Iface #1, frame[1] was rejected (VOLATILE)
ASSERT_EQ(0, flags);
// State checks
EXPECT_EQ(0, pool.getNumUsedBlocks()); // TX queue is empty
@@ -245,10 +259,11 @@ TEST(CanIOManager, Transmission)
*/
// Select failure
driver.select_failure = true;
EXPECT_EQ(-1, iomgr.receive(rx_frame, tsMono(2000)));
EXPECT_EQ(-1, iomgr.send(frames[0], tsMono(2100), tsMono(2000), ALL_IFACES_MASK, CanTxQueue::Volatile));
EXPECT_EQ(-1, iomgr.receive(rx_frame, tsMono(2000), flags));
EXPECT_EQ(-1, iomgr.send(frames[0], tsMono(2100), tsMono(2000), ALL_IFACES_MASK, CanTxQueue::Volatile, flags));
EXPECT_EQ(1200, clockmock.monotonic);
EXPECT_EQ(1200, clockmock.utc);
ASSERT_EQ(0, flags);
// Transmission failure
driver.select_failure = false;
@@ -256,15 +271,69 @@ TEST(CanIOManager, Transmission)
driver.ifaces.at(1).writeable = true;
driver.ifaces.at(0).tx_failure = true;
driver.ifaces.at(1).tx_failure = true;
EXPECT_GE(0, iomgr.send(frames[0], tsMono(2200), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Persistent)); // Non-blocking - return < 0
EXPECT_GE(0, iomgr.send(frames[0], tsMono(2200), tsMono(0), ALL_IFACES_MASK, CanTxQueue::Persistent, flags)); // Non-blocking - return < 0
ASSERT_EQ(2, pool.getNumUsedBlocks()); // Untransmitted frames will be buffered
// Failure removed - transmission shall proceed
driver.ifaces.at(0).tx_failure = false;
driver.ifaces.at(1).tx_failure = false;
EXPECT_EQ(0, iomgr.receive(rx_frame, tsMono(2500)));
EXPECT_EQ(0, iomgr.receive(rx_frame, tsMono(2500), flags));
EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 2200));
EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 2200));
EXPECT_EQ(0, pool.getNumUsedBlocks()); // All transmitted
ASSERT_EQ(0, flags);
}
TEST(CanIOManager, Loopback)
{
using uavcan::CanIOManager;
using uavcan::CanTxQueue;
using uavcan::CanFrame;
using uavcan::CanRxFrame;
// Memory
typedef uavcan::PoolAllocator<sizeof(CanTxQueue::Entry) * 4, sizeof(CanTxQueue::Entry)> Pool1;
Pool1* ppool = new Pool1();
Pool1& pool = *ppool;
uavcan::PoolManager<2> poolmgr;
poolmgr.addPool(&pool);
// Platform interface
SystemClockMock clockmock;
CanDriverMock driver(2, clockmock);
// IO Manager
CanIOManager iomgr(driver, poolmgr, clockmock);
ASSERT_EQ(2, iomgr.getNumIfaces());
CanFrame fr1;
fr1.id = 123 | CanFrame::FlagEFF;
CanFrame fr2;
fr2.id = 456 | CanFrame::FlagEFF;
CanRxFrame rfr1;
CanRxFrame rfr2;
uavcan::CanIOFlags flags = 0;
ASSERT_EQ(1, iomgr.send(fr1, tsMono(1000), tsMono(0), 1, CanTxQueue::Volatile, uavcan::CanIOFlagLoopback));
ASSERT_LE(0, iomgr.receive(rfr1, tsMono(100), flags));
ASSERT_EQ(uavcan::CanIOFlagLoopback, flags);
ASSERT_TRUE(rfr1 == fr1);
flags = 0;
ASSERT_EQ(1, iomgr.send(fr1, tsMono(1000), tsMono(0), 1, CanTxQueue::Volatile, uavcan::CanIOFlagLoopback));
ASSERT_EQ(1, iomgr.send(fr2, tsMono(1000), tsMono(0), 1, CanTxQueue::Persistent, uavcan::CanIOFlagLoopback));
ASSERT_LE(0, iomgr.receive(rfr1, tsMono(100), flags));
ASSERT_EQ(uavcan::CanIOFlagLoopback, flags);
ASSERT_LE(0, iomgr.receive(rfr2, tsMono(100), flags));
ASSERT_EQ(uavcan::CanIOFlagLoopback, flags);
ASSERT_TRUE(rfr1 == fr1);
ASSERT_TRUE(rfr2 == fr2);
}
TEST(CanIOManager, Size)
{
std::cout << sizeof(uavcan::CanIOManager) << std::endl;
}
libuavcan/test/transport/can/tx_queue.cpp
+18 -15
View File
@@ -33,8 +33,9 @@ static bool isInQueue(uavcan::CanTxQueue& queue, const uavcan::CanFrame& frame)
TEST(CanTxQueue, Qos)
{
uavcan::CanTxQueue::Entry e1(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile);
uavcan::CanTxQueue::Entry e2(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile);
const uavcan::CanIOFlags flags = 0;
uavcan::CanTxQueue::Entry e1(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile, flags);
uavcan::CanTxQueue::Entry e2(makeCanFrame(100, "", EXT), tsMono(1000), uavcan::CanTxQueue::Volatile, flags);
EXPECT_FALSE(e1.qosHigherThan(e2));
EXPECT_FALSE(e2.qosHigherThan(e1));
@@ -73,6 +74,8 @@ TEST(CanTxQueue, TxQueue)
CanTxQueue queue(&poolmgr, &clockmock);
EXPECT_TRUE(queue.isEmpty());
const uavcan::CanIOFlags flags = 0;
// Descending priority
const CanFrame f0 = makeCanFrame(0, "f0", EXT);
const CanFrame f1 = makeCanFrame(10, "f1", EXT);
@@ -86,7 +89,7 @@ TEST(CanTxQueue, TxQueue)
/*
* Priority insertion
*/
queue.push(f4, tsMono(100), CanTxQueue::Persistent);
queue.push(f4, tsMono(100), CanTxQueue::Persistent, flags);
EXPECT_FALSE(queue.isEmpty());
EXPECT_EQ(1, pool32.getNumUsedBlocks());
EXPECT_EQ(f4, queue.peek()->frame);
@@ -94,13 +97,13 @@ TEST(CanTxQueue, TxQueue)
EXPECT_TRUE(queue.topPriorityHigherOrEqual(f4)); // Equal
EXPECT_FALSE(queue.topPriorityHigherOrEqual(f3));
queue.push(f3, tsMono(200), CanTxQueue::Persistent);
queue.push(f3, tsMono(200), CanTxQueue::Persistent, flags);
EXPECT_EQ(f3, queue.peek()->frame);
queue.push(f0, tsMono(300), CanTxQueue::Volatile);
queue.push(f0, tsMono(300), CanTxQueue::Volatile, flags);
EXPECT_EQ(f0, queue.peek()->frame);
queue.push(f1, tsMono(400), CanTxQueue::Volatile);
queue.push(f1, tsMono(400), CanTxQueue::Volatile, flags);
EXPECT_EQ(f0, queue.peek()->frame); // Still f0, since it is highest
EXPECT_TRUE(queue.topPriorityHigherOrEqual(f0)); // Equal
EXPECT_TRUE(queue.topPriorityHigherOrEqual(f1));
@@ -125,28 +128,28 @@ TEST(CanTxQueue, TxQueue)
* QoS
*/
EXPECT_FALSE(isInQueue(queue, f2));
queue.push(f2, tsMono(100), CanTxQueue::Volatile); // Non preempting, will be rejected
queue.push(f2, tsMono(100), CanTxQueue::Volatile, flags); // Non preempting, will be rejected
EXPECT_FALSE(isInQueue(queue, f2));
queue.push(f2, tsMono(500), CanTxQueue::Persistent); // Will override f1 (f3 and f4 are presistent)
queue.push(f2, tsMono(500), CanTxQueue::Persistent, flags); // Will override f1 (f3 and f4 are presistent)
EXPECT_TRUE(isInQueue(queue, f2));
EXPECT_FALSE(isInQueue(queue, f1));
EXPECT_EQ(4, getQueueLength(queue));
EXPECT_EQ(2, queue.getNumRejectedFrames());
EXPECT_EQ(f0, queue.peek()->frame); // Check the priority
queue.push(f5, tsMono(600), CanTxQueue::Persistent); // Will override f0 (rest are presistent)
queue.push(f5, tsMono(600), CanTxQueue::Persistent, flags); // Will override f0 (rest are presistent)
EXPECT_TRUE(isInQueue(queue, f5));
EXPECT_FALSE(isInQueue(queue, f0));
EXPECT_EQ(f2, queue.peek()->frame); // Check the priority
// No volatile frames left now
queue.push(f5a, tsMono(700), CanTxQueue::Persistent); // Will override f5 (same frame, same QoS)
queue.push(f5a, tsMono(700), CanTxQueue::Persistent, flags); // Will override f5 (same frame, same QoS)
EXPECT_TRUE(isInQueue(queue, f5a));
EXPECT_FALSE(isInQueue(queue, f5));
queue.push(f6, tsMono(700), CanTxQueue::Persistent); // Will be rejected (lowest QoS)
queue.push(f6, tsMono(700), CanTxQueue::Persistent, flags); // Will be rejected (lowest QoS)
EXPECT_FALSE(isInQueue(queue, f6));
EXPECT_FALSE(queue.topPriorityHigherOrEqual(f0));
@@ -165,19 +168,19 @@ TEST(CanTxQueue, TxQueue)
* Expiration
*/
clockmock.monotonic = 101;
queue.push(f0, tsMono(800), CanTxQueue::Volatile); // Will replace f4 which is expired now
queue.push(f0, tsMono(800), CanTxQueue::Volatile, flags); // Will replace f4 which is expired now
EXPECT_TRUE(isInQueue(queue, f0));
EXPECT_FALSE(isInQueue(queue, f4));
EXPECT_EQ(6, queue.getNumRejectedFrames());
clockmock.monotonic = 1001;
queue.push(f5, tsMono(2000), CanTxQueue::Volatile); // Entire queue is expired
queue.push(f5, tsMono(2000), CanTxQueue::Volatile, flags); // Entire queue is expired
EXPECT_TRUE(isInQueue(queue, f5));
EXPECT_EQ(1, getQueueLength(queue)); // Just one entry left - f5
EXPECT_EQ(1, pool32.getNumUsedBlocks()); // Make sure there is no leaks
EXPECT_EQ(10, queue.getNumRejectedFrames());
queue.push(f0, tsMono(1000), CanTxQueue::Persistent); // This entry is already expired
queue.push(f0, tsMono(1000), CanTxQueue::Persistent, flags); // This entry is already expired
EXPECT_EQ(1, getQueueLength(queue));
EXPECT_EQ(1, pool32.getNumUsedBlocks());
EXPECT_EQ(11, queue.getNumRejectedFrames());
@@ -185,7 +188,7 @@ TEST(CanTxQueue, TxQueue)
/*
* Removing
*/
queue.push(f4, tsMono(5000), CanTxQueue::Volatile);
queue.push(f4, tsMono(5000), CanTxQueue::Volatile, flags);
EXPECT_EQ(2, getQueueLength(queue));
EXPECT_TRUE(isInQueue(queue, f4));
EXPECT_EQ(f4, queue.peek()->frame);
libuavcan/test/transport/dispatcher.cpp
+1 -1
View File
@@ -237,7 +237,7 @@ TEST(Dispatcher, Transmission)
ASSERT_TRUE(dispatcher.hasPublisher(123));
ASSERT_FALSE(dispatcher.hasPublisher(456));
ASSERT_EQ(2, dispatcher.send(frame, TX_DEADLINE, tsMono(0), uavcan::CanTxQueue::Volatile));
ASSERT_EQ(2, dispatcher.send(frame, TX_DEADLINE, tsMono(0), uavcan::CanTxQueue::Volatile, 0));
/*
* Validation