diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/dynamic_memory.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/dynamic_memory.hpp index da977d8c684..5d1f7da6fb6 100644 --- a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/dynamic_memory.hpp +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/dynamic_memory.hpp @@ -114,12 +114,14 @@ class LimitedPoolAllocator : public IPoolAllocator IPoolAllocator& allocator_; const uint16_t max_blocks_; uint16_t used_blocks_; + uint16_t peak_used_blocks_; public: LimitedPoolAllocator(IPoolAllocator& allocator, std::size_t max_blocks) : allocator_(allocator) , max_blocks_(static_cast(min(max_blocks, 0xFFFFU))) , used_blocks_(0) + , peak_used_blocks_(0) { UAVCAN_ASSERT(max_blocks_ > 0); } @@ -128,6 +130,13 @@ public: virtual void deallocate(const void* ptr) override; virtual uint16_t getBlockCapacity() const override; + + uint16_t getNumUsedBlocks() const { return used_blocks_; } + + /// High water mark. Reaching getBlockLimit() is what turns into a rejected frame. + uint16_t getPeakNumUsedBlocks() const { return peak_used_blocks_; } + + uint16_t getBlockLimit() const { return max_blocks_; } }; // ---------------------------------------------------------------------------- diff --git a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_io.hpp b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_io.hpp index 11752f0f8ab..144c246bc4e 100644 --- a/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_io.hpp +++ b/src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_io.hpp @@ -90,14 +90,20 @@ private: LimitedPoolAllocator allocator_; ISystemClock& sysclock_; uint32_t rejected_frames_cnt_; + uint32_t rejected_expired_cnt_; + uint32_t rejected_oom_cnt_; void registerRejectedFrame(); + void registerExpiredFrame(); + void registerOutOfMemoryFrame(); public: CanTxQueue(IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t allocator_quota) : allocator_(allocator, allocator_quota) , sysclock_(sysclock) , rejected_frames_cnt_(0) + , rejected_expired_cnt_(0) + , rejected_oom_cnt_(0) { } ~CanTxQueue(); @@ -113,6 +119,15 @@ public: uint32_t getRejectedFrameCount() const { return rejected_frames_cnt_; } + /// Frames dropped because their transmit deadline had already passed. Not a memory shortage. + uint32_t getExpiredFrameCount() const { return rejected_expired_cnt_; } + + /// Frames dropped with the queue out of blocks, whether this frame lost QoS arbitration or evicted one. + uint32_t getOutOfMemoryFrameCount() const { return rejected_oom_cnt_; } + + uint16_t getPeakNumUsedBlocks() const { return allocator_.getPeakNumUsedBlocks(); } + uint16_t getBlockLimit() const { return allocator_.getBlockLimit(); } + bool isEmpty() const { return queue_.isEmpty(); } }; @@ -131,6 +146,31 @@ struct UAVCAN_EXPORT CanIfacePerfCounters }; +/** + * State of one interface's TX queue. A rejected frame is one the queue could not hold: its deadline + * had already passed, or the queue was at its block limit and QoS arbitration found nothing to evict. + * Dropping a single frame loses the whole multi-frame transfer it belonged to, which the peer sees as + * a timeout. peak_used_blocks against block_limit says whether the limit is what is doing the + * rejecting - the limit is derived from the pool's soft capacity, so it is raised by raising that. + */ +struct UAVCAN_EXPORT CanTxQueuePerfCounters +{ + uint32_t rejected_frames; + uint32_t expired_frames; + uint32_t out_of_memory_frames; + uint16_t peak_used_blocks; + uint16_t block_limit; + + CanTxQueuePerfCounters() + : rejected_frames(0) + , expired_frames(0) + , out_of_memory_frames(0) + , peak_used_blocks(0) + , block_limit(0) + { } +}; + + class UAVCAN_EXPORT CanIOManager : Noncopyable { struct IfaceFrameCounters @@ -165,6 +205,8 @@ public: CanIfacePerfCounters getIfacePerfCounters(uint8_t iface_index) const; + CanTxQueuePerfCounters getTxQueuePerfCounters(uint8_t iface_index) const; + const ICanDriver& getCanDriver() const { return driver_; } ICanDriver& getCanDriver() { return driver_; } diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_io.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_io.cpp index 8d9fa3a2ae0..cb310bb6c84 100644 --- a/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_io.cpp +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_io.cpp @@ -105,6 +105,26 @@ void CanTxQueue::registerRejectedFrame() } } +void CanTxQueue::registerExpiredFrame() +{ + registerRejectedFrame(); + + if (rejected_expired_cnt_ < NumericTraits::max()) + { + rejected_expired_cnt_++; + } +} + +void CanTxQueue::registerOutOfMemoryFrame() +{ + registerRejectedFrame(); + + if (rejected_oom_cnt_ < NumericTraits::max()) + { + rejected_oom_cnt_++; + } +} + void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags) { const MonotonicTime timestamp = sysclock_.getMonotonic(); @@ -112,7 +132,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, if (timestamp >= tx_deadline) { UAVCAN_TRACE("CanTxQueue", "Push rejected: already expired"); - registerRejectedFrame(); + registerExpiredFrame(); return; } @@ -128,7 +148,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, if (p->isExpired(timestamp)) { UAVCAN_TRACE("CanTxQueue", "Push: Expired %s", p->toString().c_str()); - registerRejectedFrame(); + registerExpiredFrame(); remove(p); } p = next; @@ -139,7 +159,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, if (praw == UAVCAN_NULLPTR) { UAVCAN_TRACE("CanTxQueue", "Push OOM #2, QoS arbitration"); - registerRejectedFrame(); + registerOutOfMemoryFrame(); // Find a frame with lowest QoS Entry* p = queue_.get(); @@ -335,6 +355,22 @@ CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) con return cnt; } +CanTxQueuePerfCounters CanIOManager::getTxQueuePerfCounters(uint8_t iface_index) const +{ + if (iface_index >= num_ifaces_ || iface_index >= MaxCanIfaces || !tx_queues_[iface_index].isConstructed()) + { + UAVCAN_ASSERT(0); + return CanTxQueuePerfCounters(); + } + CanTxQueuePerfCounters cnt; + cnt.rejected_frames = tx_queues_[iface_index]->getRejectedFrameCount(); + cnt.expired_frames = tx_queues_[iface_index]->getExpiredFrameCount(); + cnt.out_of_memory_frames = tx_queues_[iface_index]->getOutOfMemoryFrameCount(); + cnt.peak_used_blocks = tx_queues_[iface_index]->getPeakNumUsedBlocks(); + cnt.block_limit = tx_queues_[iface_index]->getBlockLimit(); + return cnt; +} + int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags) { diff --git a/src/drivers/uavcan/libdronecan/libuavcan/src/uc_dynamic_memory.cpp b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_dynamic_memory.cpp index e806d5de9ea..073d5c7702b 100644 --- a/src/drivers/uavcan/libdronecan/libuavcan/src/uc_dynamic_memory.cpp +++ b/src/drivers/uavcan/libdronecan/libuavcan/src/uc_dynamic_memory.cpp @@ -11,15 +11,29 @@ namespace uavcan */ void* LimitedPoolAllocator::allocate(std::size_t size) { - if (used_blocks_ < max_blocks_) - { - used_blocks_++; - return allocator_.allocate(size); - } - else + if (used_blocks_ >= max_blocks_) { return UAVCAN_NULLPTR; } + + /* + * Counting the block only once the underlying allocator has actually produced one. Counting the + * attempt instead spends quota on a block that deallocate() will never return, so a pool that is + * momentarily empty permanently shrinks every queue that asked it for memory while it was. + */ + void* const praw = allocator_.allocate(size); + + if (praw != UAVCAN_NULLPTR) + { + used_blocks_++; + + if (used_blocks_ > peak_used_blocks_) + { + peak_used_blocks_ = used_blocks_; + } + } + + return praw; } void LimitedPoolAllocator::deallocate(const void* ptr) diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index f8dd1903216..817c3fa2b0b 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -1295,6 +1295,11 @@ UavcanNode::print_info() printf("\tIO errors: %" PRIu64 "\n", iface_perf_cnt.errors); printf("\tRX frames: %" PRIu64 "\n", iface_perf_cnt.frames_rx); printf("\tTX frames: %" PRIu64 "\n", iface_perf_cnt.frames_tx); + + auto txq = _node.getDispatcher().getCanIOManager().getTxQueuePerfCounters(i); + printf("\tTX queue peak: %" PRIu16 "/%" PRIu16 " blocks\n", txq.peak_used_blocks, txq.block_limit); + printf("\tTX rejected: %" PRIu32 " frames (%" PRIu32 " expired, %" PRIu32 " no memory)\n", + txq.rejected_frames, txq.expired_frames, txq.out_of_memory_frames); } }