Руслан Урядинский / libuavcan

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   UAVCAN UAVCAN_Subscriber

libuavcan_drivers/linux/apps/test_multithreading.cpp@0:dfe6edabb8ec, 2018-04-14 (annotated)

Committer:
RuslanUrya
Date:
Sat Apr 14 10:25:32 2018 +0000
Revision:
0:dfe6edabb8ec
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RuslanUrya 0:dfe6edabb8ec 1 /*
RuslanUrya 0:dfe6edabb8ec 2 * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
RuslanUrya 0:dfe6edabb8ec 3 */
RuslanUrya 0:dfe6edabb8ec 4
RuslanUrya 0:dfe6edabb8ec 5 #ifndef NDEBUG
RuslanUrya 0:dfe6edabb8ec 6 # define UAVCAN_DEBUG 1
RuslanUrya 0:dfe6edabb8ec 7 #endif
RuslanUrya 0:dfe6edabb8ec 8
RuslanUrya 0:dfe6edabb8ec 9 #include <iostream>
RuslanUrya 0:dfe6edabb8ec 10 #include <thread>
RuslanUrya 0:dfe6edabb8ec 11 #include <condition_variable>
RuslanUrya 0:dfe6edabb8ec 12 #include <uavcan_linux/uavcan_linux.hpp>
RuslanUrya 0:dfe6edabb8ec 13 #include <uavcan/node/sub_node.hpp>
RuslanUrya 0:dfe6edabb8ec 14 #include <uavcan/protocol/node_status_monitor.hpp>
RuslanUrya 0:dfe6edabb8ec 15 #include <uavcan/protocol/debug/KeyValue.hpp>
RuslanUrya 0:dfe6edabb8ec 16 #include "debug.hpp"
RuslanUrya 0:dfe6edabb8ec 17
RuslanUrya 0:dfe6edabb8ec 18 /**
RuslanUrya 0:dfe6edabb8ec 19 * Generic queue based on the linked list class defined in libuavcan.
RuslanUrya 0:dfe6edabb8ec 20 * This class does not use heap memory.
RuslanUrya 0:dfe6edabb8ec 21 */
RuslanUrya 0:dfe6edabb8ec 22 template <typename T>
RuslanUrya 0:dfe6edabb8ec 23 class Queue
RuslanUrya 0:dfe6edabb8ec 24 {
RuslanUrya 0:dfe6edabb8ec 25 struct Item : public uavcan::LinkedListNode<Item>
RuslanUrya 0:dfe6edabb8ec 26 {
RuslanUrya 0:dfe6edabb8ec 27 T payload;
RuslanUrya 0:dfe6edabb8ec 28
RuslanUrya 0:dfe6edabb8ec 29 template <typename... Args>
RuslanUrya 0:dfe6edabb8ec 30 Item(Args... args) : payload(args...) { }
RuslanUrya 0:dfe6edabb8ec 31 };
RuslanUrya 0:dfe6edabb8ec 32
RuslanUrya 0:dfe6edabb8ec 33 uavcan::LimitedPoolAllocator allocator_;
RuslanUrya 0:dfe6edabb8ec 34 uavcan::LinkedListRoot<Item> list_;
RuslanUrya 0:dfe6edabb8ec 35
RuslanUrya 0:dfe6edabb8ec 36 public:
RuslanUrya 0:dfe6edabb8ec 37 Queue(uavcan::IPoolAllocator& arg_allocator, std::size_t block_allocation_quota) :
RuslanUrya 0:dfe6edabb8ec 38 allocator_(arg_allocator, block_allocation_quota)
RuslanUrya 0:dfe6edabb8ec 39 {
RuslanUrya 0:dfe6edabb8ec 40 uavcan::IsDynamicallyAllocatable<Item>::check();
RuslanUrya 0:dfe6edabb8ec 41 }
RuslanUrya 0:dfe6edabb8ec 42
RuslanUrya 0:dfe6edabb8ec 43 bool isEmpty() const { return list_.isEmpty(); }
RuslanUrya 0:dfe6edabb8ec 44
RuslanUrya 0:dfe6edabb8ec 45 /**
RuslanUrya 0:dfe6edabb8ec 46 * Creates one item in-place at the end of the list.
RuslanUrya 0:dfe6edabb8ec 47 * Returns true if the item was appended successfully, false if there's not enough memory.
RuslanUrya 0:dfe6edabb8ec 48 * Complexity is O(N) where N is queue length.
RuslanUrya 0:dfe6edabb8ec 49 */
RuslanUrya 0:dfe6edabb8ec 50 template <typename... Args>
RuslanUrya 0:dfe6edabb8ec 51 bool tryEmplace(Args... args)
RuslanUrya 0:dfe6edabb8ec 52 {
RuslanUrya 0:dfe6edabb8ec 53 // Allocating memory
RuslanUrya 0:dfe6edabb8ec 54 void* const ptr = allocator_.allocate(sizeof(Item));
RuslanUrya 0:dfe6edabb8ec 55 if (ptr == nullptr)
RuslanUrya 0:dfe6edabb8ec 56 {
RuslanUrya 0:dfe6edabb8ec 57 return false;
RuslanUrya 0:dfe6edabb8ec 58 }
RuslanUrya 0:dfe6edabb8ec 59
RuslanUrya 0:dfe6edabb8ec 60 // Constructing the new item
RuslanUrya 0:dfe6edabb8ec 61 Item* const item = new (ptr) Item(args...);
RuslanUrya 0:dfe6edabb8ec 62 assert(item != nullptr);
RuslanUrya 0:dfe6edabb8ec 63
RuslanUrya 0:dfe6edabb8ec 64 // Inserting the new item at the end of the list
RuslanUrya 0:dfe6edabb8ec 65 Item* p = list_.get();
RuslanUrya 0:dfe6edabb8ec 66 if (p == nullptr)
RuslanUrya 0:dfe6edabb8ec 67 {
RuslanUrya 0:dfe6edabb8ec 68 list_.insert(item);
RuslanUrya 0:dfe6edabb8ec 69 }
RuslanUrya 0:dfe6edabb8ec 70 else
RuslanUrya 0:dfe6edabb8ec 71 {
RuslanUrya 0:dfe6edabb8ec 72 while (p->getNextListNode() != nullptr)
RuslanUrya 0:dfe6edabb8ec 73 {
RuslanUrya 0:dfe6edabb8ec 74 p = p->getNextListNode();
RuslanUrya 0:dfe6edabb8ec 75 }
RuslanUrya 0:dfe6edabb8ec 76 assert(p->getNextListNode() == nullptr);
RuslanUrya 0:dfe6edabb8ec 77 p->setNextListNode(item);
RuslanUrya 0:dfe6edabb8ec 78 assert(p->getNextListNode()->getNextListNode() == nullptr);
RuslanUrya 0:dfe6edabb8ec 79 }
RuslanUrya 0:dfe6edabb8ec 80
RuslanUrya 0:dfe6edabb8ec 81 return true;
RuslanUrya 0:dfe6edabb8ec 82 }
RuslanUrya 0:dfe6edabb8ec 83
RuslanUrya 0:dfe6edabb8ec 84 /**
RuslanUrya 0:dfe6edabb8ec 85 * Accesses the first element.
RuslanUrya 0:dfe6edabb8ec 86 * Nullptr will be returned if the queue is empty.
RuslanUrya 0:dfe6edabb8ec 87 * Complexity is O(1).
RuslanUrya 0:dfe6edabb8ec 88 */
RuslanUrya 0:dfe6edabb8ec 89 T* peek() { return isEmpty() ? nullptr : &list_.get()->payload; }
RuslanUrya 0:dfe6edabb8ec 90 const T* peek() const { return isEmpty() ? nullptr : &list_.get()->payload; }
RuslanUrya 0:dfe6edabb8ec 91
RuslanUrya 0:dfe6edabb8ec 92 /**
RuslanUrya 0:dfe6edabb8ec 93 * Removes the first element.
RuslanUrya 0:dfe6edabb8ec 94 * If the queue is empty, nothing will be done and assertion failure will be triggered.
RuslanUrya 0:dfe6edabb8ec 95 * Complexity is O(1).
RuslanUrya 0:dfe6edabb8ec 96 */
RuslanUrya 0:dfe6edabb8ec 97 void pop()
RuslanUrya 0:dfe6edabb8ec 98 {
RuslanUrya 0:dfe6edabb8ec 99 Item* const item = list_.get();
RuslanUrya 0:dfe6edabb8ec 100 assert(item != nullptr);
RuslanUrya 0:dfe6edabb8ec 101 if (item != nullptr)
RuslanUrya 0:dfe6edabb8ec 102 {
RuslanUrya 0:dfe6edabb8ec 103 list_.remove(item);
RuslanUrya 0:dfe6edabb8ec 104 item->~Item();
RuslanUrya 0:dfe6edabb8ec 105 allocator_.deallocate(item);
RuslanUrya 0:dfe6edabb8ec 106 }
RuslanUrya 0:dfe6edabb8ec 107 }
RuslanUrya 0:dfe6edabb8ec 108 };
RuslanUrya 0:dfe6edabb8ec 109
RuslanUrya 0:dfe6edabb8ec 110 /**
RuslanUrya 0:dfe6edabb8ec 111 * Feel free to remove.
RuslanUrya 0:dfe6edabb8ec 112 */
RuslanUrya 0:dfe6edabb8ec 113 static void testQueue()
RuslanUrya 0:dfe6edabb8ec 114 {
RuslanUrya 0:dfe6edabb8ec 115 uavcan::PoolAllocator<1024, uavcan::MemPoolBlockSize> allocator;
RuslanUrya 0:dfe6edabb8ec 116 Queue<typename uavcan::MakeString<50>::Type> q(allocator, 4);
RuslanUrya 0:dfe6edabb8ec 117 ENFORCE(q.isEmpty());
RuslanUrya 0:dfe6edabb8ec 118 ENFORCE(q.peek() == nullptr);
RuslanUrya 0:dfe6edabb8ec 119 ENFORCE(q.tryEmplace("One"));
RuslanUrya 0:dfe6edabb8ec 120 ENFORCE(q.tryEmplace("Two"));
RuslanUrya 0:dfe6edabb8ec 121 ENFORCE(q.tryEmplace("Three"));
RuslanUrya 0:dfe6edabb8ec 122 ENFORCE(q.tryEmplace("Four"));
RuslanUrya 0:dfe6edabb8ec 123 ENFORCE(!q.tryEmplace("Five"));
RuslanUrya 0:dfe6edabb8ec 124 ENFORCE(*q.peek() == "One");
RuslanUrya 0:dfe6edabb8ec 125 q.pop();
RuslanUrya 0:dfe6edabb8ec 126 ENFORCE(*q.peek() == "Two");
RuslanUrya 0:dfe6edabb8ec 127 q.pop();
RuslanUrya 0:dfe6edabb8ec 128 ENFORCE(*q.peek() == "Three");
RuslanUrya 0:dfe6edabb8ec 129 q.pop();
RuslanUrya 0:dfe6edabb8ec 130 ENFORCE(*q.peek() == "Four");
RuslanUrya 0:dfe6edabb8ec 131 q.pop();
RuslanUrya 0:dfe6edabb8ec 132 ENFORCE(q.isEmpty());
RuslanUrya 0:dfe6edabb8ec 133 ENFORCE(q.peek() == nullptr);
RuslanUrya 0:dfe6edabb8ec 134 }
RuslanUrya 0:dfe6edabb8ec 135
RuslanUrya 0:dfe6edabb8ec 136 /**
RuslanUrya 0:dfe6edabb8ec 137 * Objects of this class are owned by the sub-node thread.
RuslanUrya 0:dfe6edabb8ec 138 * This class does not use heap memory.
RuslanUrya 0:dfe6edabb8ec 139 */
RuslanUrya 0:dfe6edabb8ec 140 class VirtualCanIface : public uavcan::ICanIface,
RuslanUrya 0:dfe6edabb8ec 141 uavcan::Noncopyable
RuslanUrya 0:dfe6edabb8ec 142 {
RuslanUrya 0:dfe6edabb8ec 143 struct RxItem
RuslanUrya 0:dfe6edabb8ec 144 {
RuslanUrya 0:dfe6edabb8ec 145 const uavcan::CanRxFrame frame;
RuslanUrya 0:dfe6edabb8ec 146 const uavcan::CanIOFlags flags;
RuslanUrya 0:dfe6edabb8ec 147
RuslanUrya 0:dfe6edabb8ec 148 RxItem(const uavcan::CanRxFrame& arg_frame, uavcan::CanIOFlags arg_flags) :
RuslanUrya 0:dfe6edabb8ec 149 frame(arg_frame),
RuslanUrya 0:dfe6edabb8ec 150 flags(arg_flags)
RuslanUrya 0:dfe6edabb8ec 151 { }
RuslanUrya 0:dfe6edabb8ec 152 };
RuslanUrya 0:dfe6edabb8ec 153
RuslanUrya 0:dfe6edabb8ec 154 std::mutex& mutex_;
RuslanUrya 0:dfe6edabb8ec 155 uavcan::CanTxQueue prioritized_tx_queue_;
RuslanUrya 0:dfe6edabb8ec 156 Queue<RxItem> rx_queue_;
RuslanUrya 0:dfe6edabb8ec 157
RuslanUrya 0:dfe6edabb8ec 158 int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override
RuslanUrya 0:dfe6edabb8ec 159 {
RuslanUrya 0:dfe6edabb8ec 160 std::lock_guard<std::mutex> lock(mutex_);
RuslanUrya 0:dfe6edabb8ec 161 prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags);
RuslanUrya 0:dfe6edabb8ec 162 return 1;
RuslanUrya 0:dfe6edabb8ec 163 }
RuslanUrya 0:dfe6edabb8ec 164
RuslanUrya 0:dfe6edabb8ec 165 int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
RuslanUrya 0:dfe6edabb8ec 166 uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) override
RuslanUrya 0:dfe6edabb8ec 167 {
RuslanUrya 0:dfe6edabb8ec 168 std::lock_guard<std::mutex> lock(mutex_);
RuslanUrya 0:dfe6edabb8ec 169
RuslanUrya 0:dfe6edabb8ec 170 if (rx_queue_.isEmpty())
RuslanUrya 0:dfe6edabb8ec 171 {
RuslanUrya 0:dfe6edabb8ec 172 return 0;
RuslanUrya 0:dfe6edabb8ec 173 }
RuslanUrya 0:dfe6edabb8ec 174
RuslanUrya 0:dfe6edabb8ec 175 const auto item = *rx_queue_.peek();
RuslanUrya 0:dfe6edabb8ec 176 rx_queue_.pop();
RuslanUrya 0:dfe6edabb8ec 177
RuslanUrya 0:dfe6edabb8ec 178 out_frame = item.frame;
RuslanUrya 0:dfe6edabb8ec 179 out_ts_monotonic = item.frame.ts_mono;
RuslanUrya 0:dfe6edabb8ec 180 out_ts_utc = item.frame.ts_utc;
RuslanUrya 0:dfe6edabb8ec 181 out_flags = item.flags;
RuslanUrya 0:dfe6edabb8ec 182
RuslanUrya 0:dfe6edabb8ec 183 return 1;
RuslanUrya 0:dfe6edabb8ec 184 }
RuslanUrya 0:dfe6edabb8ec 185
RuslanUrya 0:dfe6edabb8ec 186 int16_t configureFilters(const uavcan::CanFilterConfig*, std::uint16_t) override { return -uavcan::ErrDriver; }
RuslanUrya 0:dfe6edabb8ec 187 uint16_t getNumFilters() const override { return 0; }
RuslanUrya 0:dfe6edabb8ec 188 uint64_t getErrorCount() const override { return 0; }
RuslanUrya 0:dfe6edabb8ec 189
RuslanUrya 0:dfe6edabb8ec 190 public:
RuslanUrya 0:dfe6edabb8ec 191 VirtualCanIface(uavcan::IPoolAllocator& allocator, uavcan::ISystemClock& clock,
RuslanUrya 0:dfe6edabb8ec 192 std::mutex& arg_mutex, unsigned quota_per_queue) :
RuslanUrya 0:dfe6edabb8ec 193 mutex_(arg_mutex),
RuslanUrya 0:dfe6edabb8ec 194 prioritized_tx_queue_(allocator, clock, quota_per_queue),
RuslanUrya 0:dfe6edabb8ec 195 rx_queue_(allocator, quota_per_queue)
RuslanUrya 0:dfe6edabb8ec 196 { }
RuslanUrya 0:dfe6edabb8ec 197
RuslanUrya 0:dfe6edabb8ec 198 /**
RuslanUrya 0:dfe6edabb8ec 199 * Note that RX queue overwrites oldest items when overflowed.
RuslanUrya 0:dfe6edabb8ec 200 * Call this from the main thread only.
RuslanUrya 0:dfe6edabb8ec 201 * No additional locking is required.
RuslanUrya 0:dfe6edabb8ec 202 */
RuslanUrya 0:dfe6edabb8ec 203 void addRxFrame(const uavcan::CanRxFrame& frame, uavcan::CanIOFlags flags)
RuslanUrya 0:dfe6edabb8ec 204 {
RuslanUrya 0:dfe6edabb8ec 205 std::lock_guard<std::mutex> lock(mutex_);
RuslanUrya 0:dfe6edabb8ec 206 if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty())
RuslanUrya 0:dfe6edabb8ec 207 {
RuslanUrya 0:dfe6edabb8ec 208 rx_queue_.pop();
RuslanUrya 0:dfe6edabb8ec 209 (void)rx_queue_.tryEmplace(frame, flags);
RuslanUrya 0:dfe6edabb8ec 210 }
RuslanUrya 0:dfe6edabb8ec 211 }
RuslanUrya 0:dfe6edabb8ec 212
RuslanUrya 0:dfe6edabb8ec 213 /**
RuslanUrya 0:dfe6edabb8ec 214 * Call this from the main thread only.
RuslanUrya 0:dfe6edabb8ec 215 * No additional locking is required.
RuslanUrya 0:dfe6edabb8ec 216 */
RuslanUrya 0:dfe6edabb8ec 217 void flushTxQueueTo(uavcan::INode& main_node, std::uint8_t iface_index)
RuslanUrya 0:dfe6edabb8ec 218 {
RuslanUrya 0:dfe6edabb8ec 219 std::lock_guard<std::mutex> lock(mutex_);
RuslanUrya 0:dfe6edabb8ec 220
RuslanUrya 0:dfe6edabb8ec 221 const std::uint8_t iface_mask = static_cast<std::uint8_t>(1U << iface_index);
RuslanUrya 0:dfe6edabb8ec 222
RuslanUrya 0:dfe6edabb8ec 223 while (auto e = prioritized_tx_queue_.peek())
RuslanUrya 0:dfe6edabb8ec 224 {
RuslanUrya 0:dfe6edabb8ec 225 UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s",
RuslanUrya 0:dfe6edabb8ec 226 unsigned(iface_mask), e->toString().c_str());
RuslanUrya 0:dfe6edabb8ec 227
RuslanUrya 0:dfe6edabb8ec 228 const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask,
RuslanUrya 0:dfe6edabb8ec 229 uavcan::CanTxQueue::Qos(e->qos), e->flags);
RuslanUrya 0:dfe6edabb8ec 230 prioritized_tx_queue_.remove(e);
RuslanUrya 0:dfe6edabb8ec 231 if (res <= 0)
RuslanUrya 0:dfe6edabb8ec 232 {
RuslanUrya 0:dfe6edabb8ec 233 break;
RuslanUrya 0:dfe6edabb8ec 234 }
RuslanUrya 0:dfe6edabb8ec 235 }
RuslanUrya 0:dfe6edabb8ec 236 }
RuslanUrya 0:dfe6edabb8ec 237
RuslanUrya 0:dfe6edabb8ec 238 /**
RuslanUrya 0:dfe6edabb8ec 239 * Call this from the sub-node thread only.
RuslanUrya 0:dfe6edabb8ec 240 * No additional locking is required.
RuslanUrya 0:dfe6edabb8ec 241 */
RuslanUrya 0:dfe6edabb8ec 242 bool hasDataInRxQueue() const
RuslanUrya 0:dfe6edabb8ec 243 {
RuslanUrya 0:dfe6edabb8ec 244 std::lock_guard<std::mutex> lock(mutex_);
RuslanUrya 0:dfe6edabb8ec 245 return !rx_queue_.isEmpty();
RuslanUrya 0:dfe6edabb8ec 246 }
RuslanUrya 0:dfe6edabb8ec 247 };
RuslanUrya 0:dfe6edabb8ec 248
RuslanUrya 0:dfe6edabb8ec 249 /**
RuslanUrya 0:dfe6edabb8ec 250 * This interface defines one method that will be called by the main node thread periodically in order to
RuslanUrya 0:dfe6edabb8ec 251 * transfer contents of TX queue of the sub-node into the TX queue of the main node.
RuslanUrya 0:dfe6edabb8ec 252 */
RuslanUrya 0:dfe6edabb8ec 253 class ITxQueueInjector
RuslanUrya 0:dfe6edabb8ec 254 {
RuslanUrya 0:dfe6edabb8ec 255 public:
RuslanUrya 0:dfe6edabb8ec 256 virtual ~ITxQueueInjector() { }
RuslanUrya 0:dfe6edabb8ec 257
RuslanUrya 0:dfe6edabb8ec 258 /**
RuslanUrya 0:dfe6edabb8ec 259 * Flush contents of TX queues into the main node.
RuslanUrya 0:dfe6edabb8ec 260 * @param main_node Reference to the main node.
RuslanUrya 0:dfe6edabb8ec 261 */
RuslanUrya 0:dfe6edabb8ec 262 virtual void injectTxFramesInto(uavcan::INode& main_node) = 0;
RuslanUrya 0:dfe6edabb8ec 263 };
RuslanUrya 0:dfe6edabb8ec 264
RuslanUrya 0:dfe6edabb8ec 265 /**
RuslanUrya 0:dfe6edabb8ec 266 * Objects of this class are owned by the sub-node thread.
RuslanUrya 0:dfe6edabb8ec 267 * This class does not use heap memory.
RuslanUrya 0:dfe6edabb8ec 268 * @tparam SharedMemoryPoolSize Amount of memory, in bytes, that will be statically allocated for the
RuslanUrya 0:dfe6edabb8ec 269 * memory pool that will be shared across all interfaces for RX/TX queues.
RuslanUrya 0:dfe6edabb8ec 270 * Typically this value should be no less than 4K per interface.
RuslanUrya 0:dfe6edabb8ec 271 */
RuslanUrya 0:dfe6edabb8ec 272 template <unsigned SharedMemoryPoolSize>
RuslanUrya 0:dfe6edabb8ec 273 class VirtualCanDriver : public uavcan::ICanDriver,
RuslanUrya 0:dfe6edabb8ec 274 public uavcan::IRxFrameListener,
RuslanUrya 0:dfe6edabb8ec 275 public ITxQueueInjector,
RuslanUrya 0:dfe6edabb8ec 276 uavcan::Noncopyable
RuslanUrya 0:dfe6edabb8ec 277 {
RuslanUrya 0:dfe6edabb8ec 278 class Event
RuslanUrya 0:dfe6edabb8ec 279 {
RuslanUrya 0:dfe6edabb8ec 280 std::mutex m_;
RuslanUrya 0:dfe6edabb8ec 281 std::condition_variable cv_;
RuslanUrya 0:dfe6edabb8ec 282
RuslanUrya 0:dfe6edabb8ec 283 public:
RuslanUrya 0:dfe6edabb8ec 284 /**
RuslanUrya 0:dfe6edabb8ec 285 * Note that this method may return spuriously.
RuslanUrya 0:dfe6edabb8ec 286 */
RuslanUrya 0:dfe6edabb8ec 287 void waitFor(uavcan::MonotonicDuration duration)
RuslanUrya 0:dfe6edabb8ec 288 {
RuslanUrya 0:dfe6edabb8ec 289 std::unique_lock<std::mutex> lk(m_);
RuslanUrya 0:dfe6edabb8ec 290 (void)cv_.wait_for(lk, std::chrono::microseconds(duration.toUSec()));
RuslanUrya 0:dfe6edabb8ec 291 }
RuslanUrya 0:dfe6edabb8ec 292
RuslanUrya 0:dfe6edabb8ec 293 void signal() { cv_.notify_all(); }
RuslanUrya 0:dfe6edabb8ec 294 };
RuslanUrya 0:dfe6edabb8ec 295
RuslanUrya 0:dfe6edabb8ec 296 Event event_; ///< Used to unblock the select() call when IO happens.
RuslanUrya 0:dfe6edabb8ec 297 std::mutex mutex_; ///< Shared across all ifaces
RuslanUrya 0:dfe6edabb8ec 298 uavcan::PoolAllocator<SharedMemoryPoolSize, uavcan::MemPoolBlockSize> allocator_; ///< Shared across all ifaces
RuslanUrya 0:dfe6edabb8ec 299 uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
RuslanUrya 0:dfe6edabb8ec 300 const unsigned num_ifaces_;
RuslanUrya 0:dfe6edabb8ec 301 uavcan_linux::SystemClock clock_;
RuslanUrya 0:dfe6edabb8ec 302
RuslanUrya 0:dfe6edabb8ec 303 uavcan::ICanIface* getIface(uint8_t iface_index) override
RuslanUrya 0:dfe6edabb8ec 304 {
RuslanUrya 0:dfe6edabb8ec 305 return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface*() : nullptr;
RuslanUrya 0:dfe6edabb8ec 306 }
RuslanUrya 0:dfe6edabb8ec 307
RuslanUrya 0:dfe6edabb8ec 308 uint8_t getNumIfaces() const override { return num_ifaces_; }
RuslanUrya 0:dfe6edabb8ec 309
RuslanUrya 0:dfe6edabb8ec 310 /**
RuslanUrya 0:dfe6edabb8ec 311 * This and other methods of ICanDriver will be invoked by the sub-node thread.
RuslanUrya 0:dfe6edabb8ec 312 */
RuslanUrya 0:dfe6edabb8ec 313 int16_t select(uavcan::CanSelectMasks& inout_masks,
RuslanUrya 0:dfe6edabb8ec 314 const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
RuslanUrya 0:dfe6edabb8ec 315 uavcan::MonotonicTime blocking_deadline) override
RuslanUrya 0:dfe6edabb8ec 316 {
RuslanUrya 0:dfe6edabb8ec 317 bool need_block = (inout_masks.write == 0); // Write queue is infinite
RuslanUrya 0:dfe6edabb8ec 318 for (unsigned i = 0; need_block && (i < num_ifaces_); i++)
RuslanUrya 0:dfe6edabb8ec 319 {
RuslanUrya 0:dfe6edabb8ec 320 const bool need_read = inout_masks.read & (1U << i);
RuslanUrya 0:dfe6edabb8ec 321 if (need_read && ifaces_[i]->hasDataInRxQueue())
RuslanUrya 0:dfe6edabb8ec 322 {
RuslanUrya 0:dfe6edabb8ec 323 need_block = false;
RuslanUrya 0:dfe6edabb8ec 324 }
RuslanUrya 0:dfe6edabb8ec 325 }
RuslanUrya 0:dfe6edabb8ec 326
RuslanUrya 0:dfe6edabb8ec 327 if (need_block)
RuslanUrya 0:dfe6edabb8ec 328 {
RuslanUrya 0:dfe6edabb8ec 329 event_.waitFor(blocking_deadline - clock_.getMonotonic());
RuslanUrya 0:dfe6edabb8ec 330 }
RuslanUrya 0:dfe6edabb8ec 331
RuslanUrya 0:dfe6edabb8ec 332 inout_masks = uavcan::CanSelectMasks();
RuslanUrya 0:dfe6edabb8ec 333 for (unsigned i = 0; i < num_ifaces_; i++)
RuslanUrya 0:dfe6edabb8ec 334 {
RuslanUrya 0:dfe6edabb8ec 335 const std::uint8_t iface_mask = 1U << i;
RuslanUrya 0:dfe6edabb8ec 336 inout_masks.write |= iface_mask; // Always ready to write
RuslanUrya 0:dfe6edabb8ec 337 if (ifaces_[i]->hasDataInRxQueue())
RuslanUrya 0:dfe6edabb8ec 338 {
RuslanUrya 0:dfe6edabb8ec 339 inout_masks.read |= iface_mask;
RuslanUrya 0:dfe6edabb8ec 340 }
RuslanUrya 0:dfe6edabb8ec 341 }
RuslanUrya 0:dfe6edabb8ec 342
RuslanUrya 0:dfe6edabb8ec 343 return num_ifaces_; // We're always ready to write, hence > 0.
RuslanUrya 0:dfe6edabb8ec 344 }
RuslanUrya 0:dfe6edabb8ec 345
RuslanUrya 0:dfe6edabb8ec 346 /**
RuslanUrya 0:dfe6edabb8ec 347 * This handler will be invoked by the main node thread.
RuslanUrya 0:dfe6edabb8ec 348 */
RuslanUrya 0:dfe6edabb8ec 349 void handleRxFrame(const uavcan::CanRxFrame& frame, uavcan::CanIOFlags flags) override
RuslanUrya 0:dfe6edabb8ec 350 {
RuslanUrya 0:dfe6edabb8ec 351 UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str());
RuslanUrya 0:dfe6edabb8ec 352 if (frame.iface_index < num_ifaces_)
RuslanUrya 0:dfe6edabb8ec 353 {
RuslanUrya 0:dfe6edabb8ec 354 ifaces_[frame.iface_index]->addRxFrame(frame, flags);
RuslanUrya 0:dfe6edabb8ec 355 event_.signal();
RuslanUrya 0:dfe6edabb8ec 356 }
RuslanUrya 0:dfe6edabb8ec 357 }
RuslanUrya 0:dfe6edabb8ec 358
RuslanUrya 0:dfe6edabb8ec 359 /**
RuslanUrya 0:dfe6edabb8ec 360 * This method will be invoked by the main node thread.
RuslanUrya 0:dfe6edabb8ec 361 */
RuslanUrya 0:dfe6edabb8ec 362 void injectTxFramesInto(uavcan::INode& main_node) override
RuslanUrya 0:dfe6edabb8ec 363 {
RuslanUrya 0:dfe6edabb8ec 364 for (unsigned i = 0; i < num_ifaces_; i++)
RuslanUrya 0:dfe6edabb8ec 365 {
RuslanUrya 0:dfe6edabb8ec 366 ifaces_[i]->flushTxQueueTo(main_node, i);
RuslanUrya 0:dfe6edabb8ec 367 }
RuslanUrya 0:dfe6edabb8ec 368 event_.signal();
RuslanUrya 0:dfe6edabb8ec 369 }
RuslanUrya 0:dfe6edabb8ec 370
RuslanUrya 0:dfe6edabb8ec 371 public:
RuslanUrya 0:dfe6edabb8ec 372 VirtualCanDriver(unsigned arg_num_ifaces) : num_ifaces_(arg_num_ifaces)
RuslanUrya 0:dfe6edabb8ec 373 {
RuslanUrya 0:dfe6edabb8ec 374 assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
RuslanUrya 0:dfe6edabb8ec 375
RuslanUrya 0:dfe6edabb8ec 376 const unsigned quota_per_iface = allocator_.getBlockCapacity() / num_ifaces_;
RuslanUrya 0:dfe6edabb8ec 377 const unsigned quota_per_queue = quota_per_iface; // 2x overcommit
RuslanUrya 0:dfe6edabb8ec 378
RuslanUrya 0:dfe6edabb8ec 379 UAVCAN_TRACE("VirtualCanDriver", "Total blocks: %u, quota per queue: %u",
RuslanUrya 0:dfe6edabb8ec 380 unsigned(allocator_.getBlockCapacity()), unsigned(quota_per_queue));
RuslanUrya 0:dfe6edabb8ec 381
RuslanUrya 0:dfe6edabb8ec 382 for (unsigned i = 0; i < num_ifaces_; i++)
RuslanUrya 0:dfe6edabb8ec 383 {
RuslanUrya 0:dfe6edabb8ec 384 ifaces_[i].template construct<uavcan::IPoolAllocator&, uavcan::ISystemClock&,
RuslanUrya 0:dfe6edabb8ec 385 std::mutex&, unsigned>(allocator_, clock_, mutex_, quota_per_queue);
RuslanUrya 0:dfe6edabb8ec 386 }
RuslanUrya 0:dfe6edabb8ec 387 }
RuslanUrya 0:dfe6edabb8ec 388 };
RuslanUrya 0:dfe6edabb8ec 389
RuslanUrya 0:dfe6edabb8ec 390 static uavcan_linux::NodePtr initMainNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid,
RuslanUrya 0:dfe6edabb8ec 391 const std::string& name)
RuslanUrya 0:dfe6edabb8ec 392 {
RuslanUrya 0:dfe6edabb8ec 393 std::cout << "Initializing main node" << std::endl;
RuslanUrya 0:dfe6edabb8ec 394
RuslanUrya 0:dfe6edabb8ec 395 auto node = uavcan_linux::makeNode(ifaces, name.c_str(),
RuslanUrya 0:dfe6edabb8ec 396 uavcan::protocol::SoftwareVersion(), uavcan::protocol::HardwareVersion(), nid);
RuslanUrya 0:dfe6edabb8ec 397 node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
RuslanUrya 0:dfe6edabb8ec 398 node->setModeOperational();
RuslanUrya 0:dfe6edabb8ec 399 return node;
RuslanUrya 0:dfe6edabb8ec 400 }
RuslanUrya 0:dfe6edabb8ec 401
RuslanUrya 0:dfe6edabb8ec 402 template <unsigned QueuePoolSize>
RuslanUrya 0:dfe6edabb8ec 403 static uavcan_linux::SubNodePtr initSubNode(unsigned num_ifaces, uavcan::INode& main_node)
RuslanUrya 0:dfe6edabb8ec 404 {
RuslanUrya 0:dfe6edabb8ec 405 std::cout << "Initializing sub node" << std::endl;
RuslanUrya 0:dfe6edabb8ec 406
RuslanUrya 0:dfe6edabb8ec 407 typedef VirtualCanDriver<QueuePoolSize> Driver;
RuslanUrya 0:dfe6edabb8ec 408
RuslanUrya 0:dfe6edabb8ec 409 std::shared_ptr<Driver> driver(new Driver(num_ifaces));
RuslanUrya 0:dfe6edabb8ec 410
RuslanUrya 0:dfe6edabb8ec 411 auto node = uavcan_linux::makeSubNode(driver, main_node.getNodeID());
RuslanUrya 0:dfe6edabb8ec 412
RuslanUrya 0:dfe6edabb8ec 413 main_node.getDispatcher().installRxFrameListener(driver.get());
RuslanUrya 0:dfe6edabb8ec 414
RuslanUrya 0:dfe6edabb8ec 415 return node;
RuslanUrya 0:dfe6edabb8ec 416 }
RuslanUrya 0:dfe6edabb8ec 417
RuslanUrya 0:dfe6edabb8ec 418 static void runMainNode(const uavcan_linux::NodePtr& node)
RuslanUrya 0:dfe6edabb8ec 419 {
RuslanUrya 0:dfe6edabb8ec 420 std::cout << "Running main node" << std::endl;
RuslanUrya 0:dfe6edabb8ec 421
RuslanUrya 0:dfe6edabb8ec 422 auto timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(10000), [&node](const uavcan::TimerEvent&)
RuslanUrya 0:dfe6edabb8ec 423 {
RuslanUrya 0:dfe6edabb8ec 424 node->logInfo("timer", "Your time is running out.");
RuslanUrya 0:dfe6edabb8ec 425 // coverity[dont_call]
RuslanUrya 0:dfe6edabb8ec 426 node->setVendorSpecificStatusCode(static_cast<std::uint16_t>(std::rand()));
RuslanUrya 0:dfe6edabb8ec 427 });
RuslanUrya 0:dfe6edabb8ec 428
RuslanUrya 0:dfe6edabb8ec 429 /*
RuslanUrya 0:dfe6edabb8ec 430 * We know that in this implementation, VirtualCanDriver inherits uavcan::IRxFrameListener, so we can simply
RuslanUrya 0:dfe6edabb8ec 431 * restore the reference to ITxQueueInjector using dynamic_cast. In other implementations this may be
RuslanUrya 0:dfe6edabb8ec 432 * unacceptable, so a reference to ITxQueueInjector will have to be passed using some other means.
RuslanUrya 0:dfe6edabb8ec 433 */
RuslanUrya 0:dfe6edabb8ec 434 if (node->getDispatcher().getRxFrameListener() == nullptr)
RuslanUrya 0:dfe6edabb8ec 435 {
RuslanUrya 0:dfe6edabb8ec 436 throw std::logic_error("RX frame listener is not configured");
RuslanUrya 0:dfe6edabb8ec 437 }
RuslanUrya 0:dfe6edabb8ec 438 ITxQueueInjector& tx_injector = dynamic_cast<ITxQueueInjector&>(*node->getDispatcher().getRxFrameListener());
RuslanUrya 0:dfe6edabb8ec 439
RuslanUrya 0:dfe6edabb8ec 440 while (true)
RuslanUrya 0:dfe6edabb8ec 441 {
RuslanUrya 0:dfe6edabb8ec 442 const int res = node->spin(uavcan::MonotonicDuration::fromMSec(1));
RuslanUrya 0:dfe6edabb8ec 443 if (res < 0)
RuslanUrya 0:dfe6edabb8ec 444 {
RuslanUrya 0:dfe6edabb8ec 445 node->logError("spin", "Error %*", res);
RuslanUrya 0:dfe6edabb8ec 446 }
RuslanUrya 0:dfe6edabb8ec 447 // TX queue transfer occurs here.
RuslanUrya 0:dfe6edabb8ec 448 tx_injector.injectTxFramesInto(*node);
RuslanUrya 0:dfe6edabb8ec 449 }
RuslanUrya 0:dfe6edabb8ec 450 }
RuslanUrya 0:dfe6edabb8ec 451
RuslanUrya 0:dfe6edabb8ec 452 static void runSubNode(const uavcan_linux::SubNodePtr& node)
RuslanUrya 0:dfe6edabb8ec 453 {
RuslanUrya 0:dfe6edabb8ec 454 std::cout << "Running sub node" << std::endl;
RuslanUrya 0:dfe6edabb8ec 455
RuslanUrya 0:dfe6edabb8ec 456 /*
RuslanUrya 0:dfe6edabb8ec 457 * Log subscriber
RuslanUrya 0:dfe6edabb8ec 458 */
RuslanUrya 0:dfe6edabb8ec 459 auto log_sub = node->makeSubscriber<uavcan::protocol::debug::LogMessage>(
RuslanUrya 0:dfe6edabb8ec 460 [](const uavcan::ReceivedDataStructure<uavcan::protocol::debug::LogMessage>& msg)
RuslanUrya 0:dfe6edabb8ec 461 {
RuslanUrya 0:dfe6edabb8ec 462 std::cout << msg << std::endl;
RuslanUrya 0:dfe6edabb8ec 463 });
RuslanUrya 0:dfe6edabb8ec 464
RuslanUrya 0:dfe6edabb8ec 465 /*
RuslanUrya 0:dfe6edabb8ec 466 * Node status monitor
RuslanUrya 0:dfe6edabb8ec 467 */
RuslanUrya 0:dfe6edabb8ec 468 struct NodeStatusMonitor : public uavcan::NodeStatusMonitor
RuslanUrya 0:dfe6edabb8ec 469 {
RuslanUrya 0:dfe6edabb8ec 470 explicit NodeStatusMonitor(uavcan::INode& node) : uavcan::NodeStatusMonitor(node) { }
RuslanUrya 0:dfe6edabb8ec 471
RuslanUrya 0:dfe6edabb8ec 472 virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event) override
RuslanUrya 0:dfe6edabb8ec 473 {
RuslanUrya 0:dfe6edabb8ec 474 std::cout << "Remote node NID " << int(event.node_id.get()) << " changed status: "
RuslanUrya 0:dfe6edabb8ec 475 << event.old_status.toString() << " --> " << event.status.toString() << std::endl;
RuslanUrya 0:dfe6edabb8ec 476 }
RuslanUrya 0:dfe6edabb8ec 477 };
RuslanUrya 0:dfe6edabb8ec 478 NodeStatusMonitor nsm(*node);
RuslanUrya 0:dfe6edabb8ec 479 ENFORCE(0 == nsm.start());
RuslanUrya 0:dfe6edabb8ec 480
RuslanUrya 0:dfe6edabb8ec 481 /*
RuslanUrya 0:dfe6edabb8ec 482 * KV subscriber
RuslanUrya 0:dfe6edabb8ec 483 */
RuslanUrya 0:dfe6edabb8ec 484 auto kv_sub = node->makeSubscriber<uavcan::protocol::debug::KeyValue>(
RuslanUrya 0:dfe6edabb8ec 485 [](const uavcan::ReceivedDataStructure<uavcan::protocol::debug::KeyValue>& msg)
RuslanUrya 0:dfe6edabb8ec 486 {
RuslanUrya 0:dfe6edabb8ec 487 std::cout << msg << std::endl;
RuslanUrya 0:dfe6edabb8ec 488 });
RuslanUrya 0:dfe6edabb8ec 489
RuslanUrya 0:dfe6edabb8ec 490 /*
RuslanUrya 0:dfe6edabb8ec 491 * KV publisher
RuslanUrya 0:dfe6edabb8ec 492 */
RuslanUrya 0:dfe6edabb8ec 493 unsigned kv_value = 0;
RuslanUrya 0:dfe6edabb8ec 494 auto kv_pub = node->makePublisher<uavcan::protocol::debug::KeyValue>();
RuslanUrya 0:dfe6edabb8ec 495 auto timer = node->makeTimer(uavcan::MonotonicDuration::fromMSec(5000), [&](const uavcan::TimerEvent&)
RuslanUrya 0:dfe6edabb8ec 496 {
RuslanUrya 0:dfe6edabb8ec 497 uavcan::protocol::debug::KeyValue kv;
RuslanUrya 0:dfe6edabb8ec 498 kv.key = "five_seconds";
RuslanUrya 0:dfe6edabb8ec 499 kv.value = kv_value++;
RuslanUrya 0:dfe6edabb8ec 500 const int res = kv_pub->broadcast(kv);
RuslanUrya 0:dfe6edabb8ec 501 if (res < 0)
RuslanUrya 0:dfe6edabb8ec 502 {
RuslanUrya 0:dfe6edabb8ec 503 std::cerr << "Sub KV pub err " << res << std::endl;
RuslanUrya 0:dfe6edabb8ec 504 }
RuslanUrya 0:dfe6edabb8ec 505 });
RuslanUrya 0:dfe6edabb8ec 506
RuslanUrya 0:dfe6edabb8ec 507 while (true)
RuslanUrya 0:dfe6edabb8ec 508 {
RuslanUrya 0:dfe6edabb8ec 509 const int res = node->spin(uavcan::MonotonicDuration::fromMSec(1000));
RuslanUrya 0:dfe6edabb8ec 510 if (res < 0)
RuslanUrya 0:dfe6edabb8ec 511 {
RuslanUrya 0:dfe6edabb8ec 512 std::cerr << "SubNode spin error: " << res << std::endl;
RuslanUrya 0:dfe6edabb8ec 513 }
RuslanUrya 0:dfe6edabb8ec 514 }
RuslanUrya 0:dfe6edabb8ec 515 }
RuslanUrya 0:dfe6edabb8ec 516
RuslanUrya 0:dfe6edabb8ec 517 int main(int argc, const char** argv)
RuslanUrya 0:dfe6edabb8ec 518 {
RuslanUrya 0:dfe6edabb8ec 519 try
RuslanUrya 0:dfe6edabb8ec 520 {
RuslanUrya 0:dfe6edabb8ec 521 testQueue();
RuslanUrya 0:dfe6edabb8ec 522
RuslanUrya 0:dfe6edabb8ec 523 constexpr unsigned VirtualIfacePoolSize = 32768;
RuslanUrya 0:dfe6edabb8ec 524
RuslanUrya 0:dfe6edabb8ec 525 if (argc < 3)
RuslanUrya 0:dfe6edabb8ec 526 {
RuslanUrya 0:dfe6edabb8ec 527 std::cerr << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
RuslanUrya 0:dfe6edabb8ec 528 return 1;
RuslanUrya 0:dfe6edabb8ec 529 }
RuslanUrya 0:dfe6edabb8ec 530
RuslanUrya 0:dfe6edabb8ec 531 const int self_node_id = std::stoi(argv[1]);
RuslanUrya 0:dfe6edabb8ec 532 std::vector<std::string> iface_names(argv + 2, argv + argc);
RuslanUrya 0:dfe6edabb8ec 533
RuslanUrya 0:dfe6edabb8ec 534 auto node = initMainNode(iface_names, self_node_id, "org.uavcan.linux_test_node");
RuslanUrya 0:dfe6edabb8ec 535 auto sub_node = initSubNode<VirtualIfacePoolSize>(iface_names.size(), *node);
RuslanUrya 0:dfe6edabb8ec 536
RuslanUrya 0:dfe6edabb8ec 537 std::thread sub_thread([&sub_node](){ runSubNode(sub_node); });
RuslanUrya 0:dfe6edabb8ec 538
RuslanUrya 0:dfe6edabb8ec 539 runMainNode(node);
RuslanUrya 0:dfe6edabb8ec 540
RuslanUrya 0:dfe6edabb8ec 541 if (sub_thread.joinable())
RuslanUrya 0:dfe6edabb8ec 542 {
RuslanUrya 0:dfe6edabb8ec 543 std::cout << "Waiting for the sub thread to join" << std::endl;
RuslanUrya 0:dfe6edabb8ec 544 sub_thread.join();
RuslanUrya 0:dfe6edabb8ec 545 }
RuslanUrya 0:dfe6edabb8ec 546
RuslanUrya 0:dfe6edabb8ec 547 return 0;
RuslanUrya 0:dfe6edabb8ec 548 }
RuslanUrya 0:dfe6edabb8ec 549 catch (const std::exception& ex)
RuslanUrya 0:dfe6edabb8ec 550 {
RuslanUrya 0:dfe6edabb8ec 551 std::cerr << "Exception: " << ex.what() << std::endl;
RuslanUrya 0:dfe6edabb8ec 552 return 1;
RuslanUrya 0:dfe6edabb8ec 553 }
RuslanUrya 0:dfe6edabb8ec 554 }