-
Notifications
You must be signed in to change notification settings - Fork 503
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
16 changed files
with
830 additions
and
532 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,6 +1,9 @@ | ||
| #include <utility> | ||
|
|
||
| /* | ||
| * CAN bus IO logic. | ||
| * Copyright (C) 2014 Pavel Kirienko <[email protected]> | ||
| * Copyright (C) 2019 Theodoros Ntakouris <[email protected]> | ||
| */ | ||
|
|
||
| #ifndef UAVCAN_TRANSPORT_CAN_IO_HPP_INCLUDED | ||
|
|
@@ -10,6 +13,7 @@ | |
| #include <uavcan/error.hpp> | ||
| #include <uavcan/std.hpp> | ||
| #include <uavcan/util/linked_list.hpp> | ||
| #include <uavcan/util/avl_tree.hpp> | ||
| #include <uavcan/dynamic_memory.hpp> | ||
| #include <uavcan/build_config.hpp> | ||
| #include <uavcan/util/templates.hpp> | ||
|
|
@@ -32,91 +36,87 @@ struct UAVCAN_EXPORT CanRxFrame : public CanFrame | |
| { } | ||
|
|
||
| #if UAVCAN_TOSTRING | ||
| std::string toString(StringRepresentation mode = StrTight) const; | ||
| std::string toString(StringRepresentation mode = StrTight) const; | ||
| #endif | ||
| }; | ||
|
|
||
| enum Qos { Volatile, Persistent }; | ||
|
|
||
| class UAVCAN_EXPORT CanTxQueue : Noncopyable | ||
| struct CanTxQueueEntry // Not required to be packed - fits the block in any case | ||
| { | ||
| public: | ||
| enum Qos { Volatile, Persistent }; | ||
|
|
||
| struct Entry : public LinkedListNode<Entry> // Not required to be packed - fits the block in any case | ||
| { | ||
| MonotonicTime deadline; | ||
| CanFrame frame; | ||
| uint8_t qos; | ||
| CanIOFlags flags; | ||
| MonotonicTime deadline; | ||
| const CanFrame frame; | ||
| uint8_t qos; | ||
| CanIOFlags flags; | ||
|
|
||
| Entry(const CanFrame& arg_frame, MonotonicTime arg_deadline, Qos arg_qos, CanIOFlags arg_flags) | ||
| : deadline(arg_deadline) | ||
| CanTxQueueEntry(const CanFrame& arg_frame, const MonotonicTime arg_deadline, Qos arg_qos, CanIOFlags arg_flags) | ||
| : deadline(std::move(arg_deadline)) | ||
| , frame(arg_frame) | ||
| , qos(uint8_t(arg_qos)) | ||
| , flags(arg_flags) | ||
| { | ||
| UAVCAN_ASSERT((qos == Volatile) || (qos == Persistent)); | ||
| IsDynamicallyAllocatable<Entry>::check(); | ||
| } | ||
| { | ||
| UAVCAN_ASSERT((qos == Volatile) || (qos == Persistent)); | ||
| IsDynamicallyAllocatable<CanTxQueueEntry>::check(); | ||
| } | ||
|
|
||
| static void destroy(Entry*& obj, IPoolAllocator& allocator); | ||
| static void destroy(CanTxQueueEntry*& obj, IPoolAllocator& allocator); | ||
|
|
||
| bool isExpired(MonotonicTime timestamp) const { return timestamp > deadline; } | ||
| bool isExpired(const MonotonicTime timestamp) const { return timestamp > deadline; } | ||
|
|
||
| bool qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const; | ||
| bool qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const; | ||
| bool qosHigherThan(const Entry& rhs) const { return qosHigherThan(rhs.frame, Qos(rhs.qos)); } | ||
| bool qosLowerThan(const Entry& rhs) const { return qosLowerThan(rhs.frame, Qos(rhs.qos)); } | ||
| bool operator <(const CanTxQueueEntry & other) const | ||
| { | ||
| return this->frame.priorityLowerThan(other.frame); | ||
| } | ||
|
|
||
| #if UAVCAN_TOSTRING | ||
| std::string toString() const; | ||
| #endif | ||
| }; | ||
| bool operator >(const CanTxQueueEntry & other) const | ||
| { | ||
| return this->frame.priorityHigherThan(other.frame); | ||
| } | ||
|
|
||
| private: | ||
| class PriorityInsertionComparator | ||
| bool operator =(const CanTxQueueEntry & other) const | ||
| { | ||
| const CanFrame& frm_; | ||
| public: | ||
| explicit PriorityInsertionComparator(const CanFrame& frm) : frm_(frm) { } | ||
| bool operator()(const Entry* entry) | ||
| { | ||
| UAVCAN_ASSERT(entry); | ||
| return frm_.priorityHigherThan(entry->frame); | ||
| } | ||
| }; | ||
| return this->frame == other.frame; | ||
| } | ||
|
|
||
| LinkedListRoot<Entry> queue_; | ||
| LimitedPoolAllocator allocator_; | ||
| #if UAVCAN_TOSTRING | ||
| std::string toString() const; | ||
| #endif | ||
| }; | ||
|
|
||
| class UAVCAN_EXPORT CanTxQueue : public AvlTree<CanTxQueueEntry> | ||
| { | ||
| protected: | ||
| ISystemClock& sysclock_; | ||
| uint32_t rejected_frames_cnt_; | ||
|
|
||
| void registerRejectedFrame(); | ||
| void safeIncrementRejectedFrames(); | ||
|
|
||
| AvlTree::Node* searchForNonExpiredMax(Node* n); | ||
|
|
||
| public: | ||
| CanTxQueue(IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t allocator_quota) | ||
| : allocator_(allocator, allocator_quota) | ||
| : AvlTree(allocator, allocator_quota) | ||
| , sysclock_(sysclock) | ||
| , rejected_frames_cnt_(0) | ||
| { } | ||
| {} | ||
|
|
||
| ~CanTxQueue(); | ||
| ~CanTxQueue() override; | ||
|
|
||
| void push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags); | ||
| /* Avl Tree allocates the AvlTree::Node, while this(CanTxQueue) allocates the CanTxQueueEntry | ||
| * Same logic for removal. */ | ||
| void push(const CanFrame &frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags); | ||
| void remove(CanTxQueueEntry* entry) override; | ||
|
|
||
| Entry* peek(); // Modifier | ||
| void remove(Entry*& entry); | ||
| const CanFrame* getTopPriorityPendingFrame() const; | ||
| uint32_t getRejectedFrameCount() const { return rejected_frames_cnt_; } | ||
|
|
||
| /// The 'or equal' condition is necessary to avoid frame reordering. | ||
| bool topPriorityHigherOrEqual(const CanFrame& rhs_frame) const; | ||
| bool contains(const CanFrame &frame) const; | ||
|
|
||
| uint32_t getRejectedFrameCount() const { return rejected_frames_cnt_; } | ||
| /* Tries to look up rightmost Node. If the frame is expired, removes it and continues traversing */ | ||
| CanTxQueueEntry* peek(); | ||
|
|
||
| bool isEmpty() const { return queue_.isEmpty(); } | ||
| bool topPriorityHigherOrEqual(const CanFrame &rhs_frame); | ||
| }; | ||
|
|
||
|
|
||
| struct UAVCAN_EXPORT CanIfacePerfCounters | ||
| { | ||
| uint64_t frames_tx; | ||
|
|
@@ -130,7 +130,6 @@ struct UAVCAN_EXPORT CanIfacePerfCounters | |
| { } | ||
| }; | ||
|
|
||
|
|
||
| class UAVCAN_EXPORT CanIOManager : Noncopyable | ||
| { | ||
| struct IfaceFrameCounters | ||
|
|
@@ -177,7 +176,7 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable | |
| * negative - failure | ||
| */ | ||
| int send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, | ||
| uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags); | ||
| uint8_t iface_mask, Qos qos, CanIOFlags flags); | ||
| int receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags); | ||
| }; | ||
|
|
||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.