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

CAN TX queue quota - limiting max number of memory blocks per TX queue to avoid memory congestion if one iface is down

Browse Source
This commit is contained in:
Pavel Kirienko
2014-04-12 18:11:30 +04:00
parent 3e4234e049
commit d801f4a7f3
4 changed files with 47 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/include/uavcan/transport/can_io.hpp
+11 -24
View File
@@ -12,6 +12,7 @@
#include <uavcan/dynamic_memory.hpp>
#include <uavcan/impl_constants.hpp>
#include <uavcan/util/compile_time.hpp>
#include <uavcan/util/lazy_constructor.hpp>
#include <uavcan/driver/can.hpp>
#include <uavcan/driver/system_clock.hpp>
#include <uavcan/time.hpp>
@@ -87,21 +88,15 @@ private:
};
LinkedListRoot<Entry> queue_;
IPoolAllocator* const allocator_;
ISystemClock* const sysclock_;
LimitedPoolAllocator allocator_;
ISystemClock& sysclock_;
uint32_t rejected_frames_cnt_;
void registerRejectedFrame();
public:
CanTxQueue()
: allocator_(NULL)
, sysclock_(NULL)
, rejected_frames_cnt_(0)
{ }
CanTxQueue(IPoolAllocator* allocator, ISystemClock* sysclock)
: allocator_(allocator)
CanTxQueue(IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t allocator_quota)
: allocator_(allocator, allocator_quota)
, sysclock_(sysclock)
, rejected_frames_cnt_(0)
{ }
@@ -151,29 +146,21 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable
ICanDriver& driver_;
ISystemClock& sysclock_;
CanTxQueue tx_queues_[MaxCanIfaces];
LazyConstructor<CanTxQueue> tx_queues_[MaxCanIfaces];
IfaceFrameCounters counters_[MaxCanIfaces];
const uint8_t num_ifaces_;
int sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags);
int sendFromTxQueue(uint8_t iface_index);
uint8_t makePendingTxMask() const;
int callSelect(CanSelectMasks& inout_masks, MonotonicTime blocking_deadline);
public:
CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock)
: driver_(driver)
, sysclock_(sysclock)
{
assert(driver.getNumIfaces() <= MaxCanIfaces);
// We can't initialize member array with non-default constructors in C++03
for (int i = 0; i < MaxCanIfaces; i++)
{
tx_queues_[i].~CanTxQueue();
new (tx_queues_ + i) CanTxQueue(&allocator, &sysclock);
}
}
CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock,
std::size_t mem_blocks_per_iface = 0);
uint8_t getNumIfaces() const;
uint8_t getNumIfaces() const { return num_ifaces_; }
CanIfacePerfCounters getIfacePerfCounters(uint8_t iface_index) const;
libuavcan/src/transport/uc_can_io.cpp
+34 -16
View File
@@ -106,7 +106,7 @@ void CanTxQueue::registerRejectedFrame()
void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags)
{
const MonotonicTime timestamp = sysclock_->getMonotonic();
const MonotonicTime timestamp = sysclock_.getMonotonic();
if (timestamp >= tx_deadline)
{
@@ -115,7 +115,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos,
return;
}
void* praw = allocator_->allocate(sizeof(Entry));
void* praw = allocator_.allocate(sizeof(Entry));
if (praw == NULL)
{
UAVCAN_TRACE("CanTxQueue", "Push OOM #1, cleanup");
@@ -132,7 +132,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos,
}
p = next;
}
praw = allocator_->allocate(sizeof(Entry)); // Try again
praw = allocator_.allocate(sizeof(Entry)); // Try again
}
if (praw == NULL)
@@ -164,7 +164,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos,
}
UAVCAN_TRACE("CanTxQueue", "Push: Replacing %s", lowestqos->toString().c_str());
remove(lowestqos);
praw = allocator_->allocate(sizeof(Entry)); // Try again
praw = allocator_.allocate(sizeof(Entry)); // Try again
}
if (praw == NULL)
@@ -178,7 +178,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos,
CanTxQueue::Entry* CanTxQueue::peek()
{
const MonotonicTime timestamp = sysclock_->getMonotonic();
const MonotonicTime timestamp = sysclock_.getMonotonic();
Entry* p = queue_.get();
while (p)
{
@@ -206,7 +206,7 @@ void CanTxQueue::remove(Entry*& entry)
return;
}
queue_.remove(entry);
Entry::destroy(entry, *allocator_);
Entry::destroy(entry, allocator_);
}
bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const
@@ -247,7 +247,7 @@ int CanIOManager::sendToIface(uint8_t iface_index, const CanFrame& frame, Monoto
int CanIOManager::sendFromTxQueue(uint8_t iface_index)
{
assert(iface_index < MaxCanIfaces);
CanTxQueue::Entry* entry = tx_queues_[iface_index].peek();
CanTxQueue::Entry* entry = tx_queues_[iface_index]->peek();
if (entry == NULL)
{
return 0;
@@ -255,7 +255,7 @@ int CanIOManager::sendFromTxQueue(uint8_t iface_index)
const int res = sendToIface(iface_index, entry->frame, entry->deadline, entry->flags);
if (res > 0)
{
tx_queues_[iface_index].remove(entry);
tx_queues_[iface_index]->remove(entry);
}
return res;
}
@@ -265,7 +265,7 @@ uint8_t CanIOManager::makePendingTxMask() const
uint8_t write_mask = 0;
for (uint8_t i = 0; i < getNumIfaces(); i++)
{
if (!tx_queues_[i].isEmpty())
if (!tx_queues_[i]->isEmpty())
{
write_mask |= 1 << i;
}
@@ -286,11 +286,29 @@ int CanIOManager::callSelect(CanSelectMasks& inout_masks, MonotonicTime blocking
return res;
}
uint8_t CanIOManager::getNumIfaces() const
CanIOManager::CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock,
std::size_t mem_blocks_per_iface)
: driver_(driver)
, sysclock_(sysclock)
, num_ifaces_(driver.getNumIfaces())
{
const uint8_t num = driver_.getNumIfaces();
assert(num > 0 && num <= MaxCanIfaces);
return std::min(std::max(num, uint8_t(0)), uint8_t(MaxCanIfaces));
if (num_ifaces_ < 1 || num_ifaces_ > MaxCanIfaces)
{
handleFatalError("Wrong number of CAN ifaces");
}
if (mem_blocks_per_iface == 0)
{
mem_blocks_per_iface = allocator.getNumBlocks() / (num_ifaces_ + 1) + 1;
}
UAVCAN_TRACE("CanIOManager", "Memory blocks per iface: %u, total: %u",
unsigned(mem_blocks_per_iface), unsigned(allocator.getNumBlocks()));
for (int i = 0; i < num_ifaces_; i++)
{
tx_queues_[i].construct<IPoolAllocator&, ISystemClock&, std::size_t>
(allocator, sysclock, mem_blocks_per_iface);
}
}
CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) const
@@ -302,7 +320,7 @@ CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) con
return CanIfacePerfCounters();
}
CanIfacePerfCounters cnt;
cnt.errors = iface->getErrorCount() + tx_queues_[iface_index].getRejectedFrameCount();
cnt.errors = iface->getErrorCount() + tx_queues_[iface_index]->getRejectedFrameCount();
cnt.frames_rx = counters_[iface_index].frames_rx;
cnt.frames_tx = counters_[iface_index].frames_tx;
return cnt;
@@ -347,7 +365,7 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton
int res = 0;
if (iface_mask & (1 << i))
{
if (tx_queues_[i].topPriorityHigherOrEqual(frame))
if (tx_queues_[i]->topPriorityHigherOrEqual(frame))
{
res = sendFromTxQueue(i); // May return 0 if nothing to transmit (e.g. expired)
}
@@ -384,7 +402,7 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton
{
if (iface_mask & (1 << i))
{
tx_queues_[i].push(frame, tx_deadline, qos, flags);
tx_queues_[i]->push(frame, tx_deadline, qos, flags);
}
}
break;
libuavcan/test/transport/can/io.cpp
+1 -1
View File
@@ -130,7 +130,7 @@ TEST(CanIOManager, Transmission)
CanDriverMock driver(2, clockmock);
// IO Manager
CanIOManager iomgr(driver, poolmgr, clockmock);
CanIOManager iomgr(driver, poolmgr, clockmock, 9999);
ASSERT_EQ(2, iomgr.getNumIfaces());
const int ALL_IFACES_MASK = 3;
libuavcan/test/transport/can/tx_queue.cpp
+1 -1
View File
@@ -73,7 +73,7 @@ TEST(CanTxQueue, TxQueue)
SystemClockMock clockmock;
CanTxQueue queue(&poolmgr, &clockmock);
CanTxQueue queue(poolmgr, clockmock, 99999);
EXPECT_TRUE(queue.isEmpty());
const uavcan::CanIOFlags flags = 0;