diff --git a/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino b/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino index aa105789..fe7a20ea 100644 --- a/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino +++ b/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino @@ -64,8 +64,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, nullptr); Node::Heap node_heap; -CircularBuffer::Heap node_rx_queue; -Node node_hdl(node_heap.data(), node_heap.size(), node_rx_queue.data(), node_rx_queue.size(), micros); +Node node_hdl(node_heap.data(), node_heap.size(), micros); Publisher> heartbeat_pub = node_hdl.create_publisher>( Heartbeat_1_0<>::PORT_ID, 1*1000*1000UL /* = 1 sec in usecs. */); diff --git a/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino b/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino index ab1dfecf..20c5658e 100644 --- a/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino +++ b/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino @@ -101,8 +101,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, nullptr); Node::Heap node_heap; -CircularBuffer::Heap node_rx_queue; -Node node_hdl(node_heap.data(), node_heap.size(), node_rx_queue.data(), node_rx_queue.size(), micros); +Node node_hdl(node_heap.data(), node_heap.size(), micros); Publisher> heartbeat_pub = node_hdl.create_publisher>(uavcan::node::Heartbeat_1_0<>::PORT_ID, 1*1000*1000UL /* = 1 sec in usecs. */); diff --git a/examples/OpenCyphal-Heartbeat-Publisher/OpenCyphal-Heartbeat-Publisher.ino b/examples/OpenCyphal-Heartbeat-Publisher/OpenCyphal-Heartbeat-Publisher.ino index a67d32a2..cf97f271 100644 --- a/examples/OpenCyphal-Heartbeat-Publisher/OpenCyphal-Heartbeat-Publisher.ino +++ b/examples/OpenCyphal-Heartbeat-Publisher/OpenCyphal-Heartbeat-Publisher.ino @@ -40,8 +40,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, nullptr); Node::Heap node_heap; -CircularBuffer::Heap node_rx_queue; -Node node_hdl(node_heap.data(), node_heap.size(), node_rx_queue.data(), node_rx_queue.size(), micros); +Node node_hdl(node_heap.data(), node_heap.size(), micros); Publisher> heartbeat_pub = node_hdl.create_publisher>(Heartbeat_1_0<>::PORT_ID, 1*1000*1000UL /* = 1 sec in usecs. */); diff --git a/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino b/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino index 353c29ce..baaa2dc2 100644 --- a/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino +++ b/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino @@ -48,8 +48,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, nullptr); Node::Heap node_heap; -CircularBuffer::Heap node_rx_queue; -Node node_hdl(node_heap.data(), node_heap.size(), node_rx_queue.data(), node_rx_queue.size(), micros); +Node node_hdl(node_heap.data(), node_heap.size(), micros); Subscription heartbeat_subscription = node_hdl.create_subscription>(Heartbeat_1_0<>::PORT_ID, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, onHeartbeat_1_0_Received); diff --git a/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino b/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino index 6f78f3ad..d2642df2 100644 --- a/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino +++ b/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino @@ -49,8 +49,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, nullptr); Node::Heap node_heap; -CircularBuffer::Heap node_rx_queue; -Node node_hdl(node_heap.data(), node_heap.size(), node_rx_queue.data(), node_rx_queue.size(), micros); +Node node_hdl(node_heap.data(), node_heap.size(), micros); ServiceClient> srv_client = node_hdl.create_service_client, ExecuteCommand_1_0::Response<>>( ExecuteCommand_1_0::Request<>::PORT_ID, diff --git a/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino b/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino index c946a401..e1e27a76 100644 --- a/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino +++ b/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino @@ -49,8 +49,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, nullptr); Node::Heap node_heap; -CircularBuffer::Heap node_rx_queue; -Node node_hdl(node_heap.data(), node_heap.size(), node_rx_queue.data(), node_rx_queue.size(), micros); +Node node_hdl(node_heap.data(), node_heap.size(), micros); ServiceServer execute_command_srv = node_hdl.create_service_server, ExecuteCommand_1_0::Response<>>( ExecuteCommand_1_0::Request<>::PORT_ID, diff --git a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino index b4051fb5..039dc9b2 100644 --- a/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino +++ b/examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino @@ -123,8 +123,7 @@ ArduinoMCP2515 mcp2515([]() nullptr); Node::Heap node_heap; -CircularBuffer::Heap node_rx_queue; -Node node_hdl(node_heap.data(), node_heap.size(), node_rx_queue.data(), node_rx_queue.size(), micros, OPEN_CYPHAL_NODE_ID); +Node node_hdl(node_heap.data(), node_heap.size(), micros, OPEN_CYPHAL_NODE_ID); Publisher> heartbeat_pub = node_hdl.create_publisher>(Heartbeat_1_0<>::PORT_ID, 1*1000*1000UL /* = 1 sec in usecs. */); Publisher tof_pub = node_hdl.create_publisher(OPEN_CYPHAL_ID_DISTANCE_DATA, 1*1000*1000UL /* = 1 sec in usecs. */); diff --git a/src/CircularBuffer.hpp b/src/CircularBuffer.hpp index 9da259eb..6929d612 100644 --- a/src/CircularBuffer.hpp +++ b/src/CircularBuffer.hpp @@ -20,29 +20,40 @@ * CLASS DECLARATION **************************************************************************************/ -template -class CircularBuffer +class CircularBufferBase { public: - template - struct Heap final : public std::array {}; - + CircularBufferBase() { } + virtual ~CircularBufferBase() { } + CircularBufferBase(CircularBufferBase const &) = delete; + CircularBufferBase(CircularBufferBase &&) = delete; + CircularBufferBase &operator=(CircularBufferBase const &) = delete; + CircularBufferBase &operator=(CircularBufferBase &&) = delete; + + virtual size_t available() const = 0; + virtual bool isFull() const = 0; + virtual bool isEmpty() const = 0; +}; - CircularBuffer(T * heap_ptr, size_t const heap_size); - ~CircularBuffer(); +template +class CircularBuffer : public CircularBufferBase +{ +public: + CircularBuffer(size_t const heap_size); + virtual ~CircularBuffer(); void enqueue(T const & val); T dequeue(); - size_t available() const { return _num_elems; } - bool isFull() const { return (_num_elems == _size); } - bool isEmpty() const { return (_num_elems == 0); } + size_t available() const override { return _num_elems; } + bool isFull() const override { return (_num_elems == _size); } + bool isEmpty() const override { return (_num_elems == 0); } private: - T * _buffer; + std::unique_ptr _buffer; size_t _size, _head, _tail, _num_elems; size_t nextIndex(size_t const index) { return ((index + 1) % _size); } diff --git a/src/CircularBuffer.ipp b/src/CircularBuffer.ipp index 18c53fc7..538a4369 100644 --- a/src/CircularBuffer.ipp +++ b/src/CircularBuffer.ipp @@ -10,8 +10,8 @@ **************************************************************************************/ template -CircularBuffer::CircularBuffer(T * heap_ptr, size_t const heap_size) -: _buffer{heap_ptr} +CircularBuffer::CircularBuffer(size_t const heap_size) +: _buffer{new T[heap_size]} , _size{heap_size} , _head{0} , _tail{0} @@ -38,7 +38,7 @@ void CircularBuffer::enqueue(T const & val) { if (isFull()) return; - _buffer[_head] = val; + _buffer.get()[_head] = val; _head = nextIndex(_head); _num_elems++; } @@ -48,7 +48,7 @@ T CircularBuffer::dequeue() { if (isEmpty()) return T{}; - T const val = _buffer[_tail]; + T const val = _buffer.get()[_tail]; _tail = nextIndex(_tail); _num_elems--; return val; diff --git a/src/Node.cpp b/src/Node.cpp index f4896f05..7f7051e4 100644 --- a/src/Node.cpp +++ b/src/Node.cpp @@ -19,17 +19,17 @@ Node::Node(uint8_t * heap_ptr, size_t const heap_size, - TReceiveCircularBuffer * rx_queue_heap_ptr, - size_t const rx_queue_heap_size, MicrosFunc const micros_func, CanardNodeID const node_id, size_t const tx_queue_capacity, + size_t const rx_queue_capacity, size_t const mtu_bytes) : _o1heap_ins{o1heapInit(heap_ptr, heap_size)} , _canard_hdl{canardInit(Node::o1heap_allocate, Node::o1heap_free)} , _micros_func{micros_func} , _canard_tx_queue{canardTxInit(tx_queue_capacity, mtu_bytes)} -, _canard_rx_queue{rx_queue_heap_ptr, rx_queue_heap_size} +, _canard_rx_queue{(_mtu_bytes == CANARD_MTU_CAN_CLASSIC) ? static_cast(new CircularBufferCan(rx_queue_capacity)) : static_cast(new CircularBufferCanFd(rx_queue_capacity))} +, _mtu_bytes{mtu_bytes} { _canard_hdl.node_id = node_id; _canard_hdl.user_reference = static_cast(_o1heap_ins); @@ -47,14 +47,22 @@ void Node::spinSome(CanFrameTxFunc const tx_func) void Node::onCanFrameReceived(CanardFrame const & frame) { - CanardMicrosecond const rx_timestamp_us = _micros_func(); + size_t const payload_size = frame.payload_size; uint32_t const extended_can_id = frame.extended_can_id; - size_t const payload_size = frame.payload_size; - - std::array payload{}; - memcpy(payload.data(), frame.payload, std::min(payload_size, payload.size())); + CanardMicrosecond const rx_timestamp_us = _micros_func(); - _canard_rx_queue.enqueue(std::make_tuple(extended_can_id, payload_size, payload, rx_timestamp_us)); + if (_mtu_bytes == CANARD_MTU_CAN_CLASSIC) + { + std::array payload{}; + memcpy(payload.data(), frame.payload, std::min(payload_size, payload.size())); + static_cast(_canard_rx_queue.get())->enqueue(std::make_tuple(extended_can_id, payload_size, payload, rx_timestamp_us)); + } + else + { + std::array payload{}; + memcpy(payload.data(), frame.payload, std::min(payload_size, payload.size())); + static_cast(_canard_rx_queue.get())->enqueue(std::make_tuple(extended_can_id, payload_size, payload, rx_timestamp_us)); + } } bool Node::enqueue_transfer(CanardMicrosecond const tx_timeout_usec, @@ -98,32 +106,21 @@ void Node::o1heap_free(CanardInstance * const ins, void * const pointer) void Node::processRxQueue() { - while (!_canard_rx_queue.isEmpty()) + while (!_canard_rx_queue->isEmpty()) { - auto [extended_can_id, payload_size, payload, rx_timestamp_us] = _canard_rx_queue.dequeue(); - - CanardFrame frame; - frame.extended_can_id = extended_can_id; - frame.payload_size = payload_size; - frame.payload = reinterpret_cast(payload.data()); - - CanardRxTransfer transfer; - CanardRxSubscription * rx_subscription; - int8_t const result = canardRxAccept(&_canard_hdl, - rx_timestamp_us, - &frame, - 0, /* redundant_transport_index */ - &transfer, - &rx_subscription); - - if(result == 1) + if (_mtu_bytes == CANARD_MTU_CAN_CLASSIC) + { + const auto [extended_can_id, payload_size, payload, rx_timestamp_us] = + static_cast(_canard_rx_queue.get())->dequeue(); + + processRxFrame(extended_can_id, payload_size, payload, rx_timestamp_us); + } + else { - /* Obtain the pointer to the subscribed object and in invoke its reception callback. */ - impl::SubscriptionBase * sub_ptr = static_cast(rx_subscription->user_reference); - sub_ptr->onTransferReceived(transfer); + const auto [extended_can_id, payload_size, payload, rx_timestamp_us] = + static_cast(_canard_rx_queue.get())->dequeue(); - /* Free dynamically allocated memory after processing. */ - _canard_hdl.memory_free(&_canard_hdl, transfer.payload); + processRxFrame(extended_can_id, payload_size, payload, rx_timestamp_us); } } } diff --git a/src/Node.hpp b/src/Node.hpp index 963d7c90..75ad005b 100644 --- a/src/Node.hpp +++ b/src/Node.hpp @@ -44,7 +44,6 @@ class Node template struct alignas(O1HEAP_ALIGNMENT) Heap final : public std::array {}; - typedef std::tuple, CanardMicrosecond> TReceiveCircularBuffer; static size_t constexpr DEFAULT_O1HEAP_SIZE = 4096; static CanardNodeID constexpr DEFAULT_NODE_ID = 42; @@ -55,18 +54,17 @@ class Node Node(uint8_t * heap_ptr, size_t const heap_size, - TReceiveCircularBuffer * rx_queue_heap_ptr, - size_t const rx_queue_heap_size, MicrosFunc const micros_func, CanardNodeID const node_id, size_t const tx_queue_capacity, + size_t const rx_queue_capacity, size_t const mtu_bytes); - Node(uint8_t * heap_ptr, size_t const heap_size, TReceiveCircularBuffer * rx_queue_heap_ptr, size_t const rx_queue_heap_size, MicrosFunc const micros_func) - : Node(heap_ptr, heap_size, rx_queue_heap_ptr, rx_queue_heap_size, micros_func, DEFAULT_NODE_ID, DEFAULT_TX_QUEUE_SIZE, DEFAULT_MTU_SIZE) { } + Node(uint8_t * heap_ptr, size_t const heap_size, MicrosFunc const micros_func) + : Node(heap_ptr, heap_size, micros_func, DEFAULT_NODE_ID, DEFAULT_TX_QUEUE_SIZE, DEFAULT_RX_QUEUE_SIZE, DEFAULT_MTU_SIZE) { } - Node(uint8_t * heap_ptr, size_t const heap_size, TReceiveCircularBuffer * rx_queue_heap_ptr, size_t const rx_queue_heap_size, MicrosFunc const micros_func, CanardNodeID const node_id) - : Node(heap_ptr, heap_size, rx_queue_heap_ptr, rx_queue_heap_size, micros_func, node_id, DEFAULT_TX_QUEUE_SIZE, DEFAULT_MTU_SIZE) { } + Node(uint8_t * heap_ptr, size_t const heap_size, MicrosFunc const micros_func, CanardNodeID const node_id) + : Node(heap_ptr, heap_size, micros_func, node_id, DEFAULT_TX_QUEUE_SIZE, DEFAULT_RX_QUEUE_SIZE, DEFAULT_MTU_SIZE) { } inline void setNodeId(CanardNodeID const node_id) { _canard_hdl.node_id = node_id; } @@ -109,17 +107,29 @@ class Node private: + typedef std::tuple, CanardMicrosecond> TCircularBufferCan; + typedef std::tuple, CanardMicrosecond> TCircularBufferCanFd; + + typedef CircularBuffer CircularBufferCan; + typedef CircularBuffer CircularBufferCanFd; + O1HeapInstance * _o1heap_ins; CanardInstance _canard_hdl; MicrosFunc const _micros_func; CanardTxQueue _canard_tx_queue; - CircularBuffer _canard_rx_queue; + std::shared_ptr _canard_rx_queue; + size_t const _mtu_bytes; static void * o1heap_allocate(CanardInstance * const ins, size_t const amount); static void o1heap_free (CanardInstance * const ins, void * const pointer); void processRxQueue(); void processTxQueue(CanFrameTxFunc const tx_func); + template + void processRxFrame(uint32_t const extended_can_id, + size_t const payload_size, + std::array const & payload, + CanardMicrosecond const rx_timestamp_us); }; /************************************************************************************** diff --git a/src/Node.ipp b/src/Node.ipp index 9aa17844..b0003516 100644 --- a/src/Node.ipp +++ b/src/Node.ipp @@ -90,3 +90,34 @@ ServiceClient Node::create_service_client(CanardPortID const port_id, return clt; } + +template +void Node::processRxFrame(uint32_t const extended_can_id, + size_t const payload_size, + std::array const & payload, + CanardMicrosecond const rx_timestamp_us) +{ + CanardFrame rx_frame; + rx_frame.extended_can_id = extended_can_id; + rx_frame.payload_size = payload_size; + rx_frame.payload = reinterpret_cast(payload.data()); + + CanardRxTransfer rx_transfer; + CanardRxSubscription * rx_subscription; + int8_t const result = canardRxAccept(&_canard_hdl, + rx_timestamp_us, + &rx_frame, + 0, /* redundant_transport_index */ + &rx_transfer, + &rx_subscription); + + if(result == 1) + { + /* Obtain the pointer to the subscribed object and in invoke its reception callback. */ + impl::SubscriptionBase * sub_ptr = static_cast(rx_subscription->user_reference); + sub_ptr->onTransferReceived(rx_transfer); + + /* Free dynamically allocated memory after processing. */ + _canard_hdl.memory_free(&_canard_hdl, rx_transfer.payload); + } +}