Merge pull request #198 from 107-systems/fix-178-rev-2

Support both CAN and CAN FD.
107-systems · Jan 10, 2023 · 6fc2819 · 6fc2819
2 parents cc29696 + 6454839
commit 6fc2819
Show file tree

Hide file tree

Showing 12 changed files with 111 additions and 69 deletions.
diff --git 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::DEFAULT_O1HEAP_SIZE> node_heap;
-CircularBuffer<Node::TReceiveCircularBuffer>::Heap<Node::DEFAULT_RX_QUEUE_SIZE> 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_1_0<>> heartbeat_pub = node_hdl.create_publisher<Heartbeat_1_0<>>(
   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
@@ -101,8 +101,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); },
                        nullptr);
 
 Node::Heap<Node::DEFAULT_O1HEAP_SIZE> node_heap;
-CircularBuffer<Node::TReceiveCircularBuffer>::Heap<Node::DEFAULT_RX_QUEUE_SIZE> 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<uavcan::node::Heartbeat_1_0<>> heartbeat_pub = node_hdl.create_publisher<uavcan::node::Heartbeat_1_0<>>(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
@@ -40,8 +40,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); },
                        nullptr);
 
 Node::Heap<Node::DEFAULT_O1HEAP_SIZE> node_heap;
-CircularBuffer<Node::TReceiveCircularBuffer>::Heap<Node::DEFAULT_RX_QUEUE_SIZE> 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_1_0<>> heartbeat_pub = node_hdl.create_publisher<Heartbeat_1_0<>>(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
@@ -48,8 +48,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); },
                        nullptr);
 
 Node::Heap<Node::DEFAULT_O1HEAP_SIZE> node_heap;
-CircularBuffer<Node::TReceiveCircularBuffer>::Heap<Node::DEFAULT_RX_QUEUE_SIZE> 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<>>(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
@@ -49,8 +49,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); },
                        nullptr);
 
 Node::Heap<Node::DEFAULT_O1HEAP_SIZE> node_heap;
-CircularBuffer<Node::TReceiveCircularBuffer>::Heap<Node::DEFAULT_RX_QUEUE_SIZE> 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<ExecuteCommand_1_0::Request<>> srv_client = node_hdl.create_service_client<ExecuteCommand_1_0::Request<>, 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
@@ -49,8 +49,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); },
                        nullptr);
 
 Node::Heap<Node::DEFAULT_O1HEAP_SIZE> node_heap;
-CircularBuffer<Node::TReceiveCircularBuffer>::Heap<Node::DEFAULT_RX_QUEUE_SIZE> 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::Request<>, 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
@@ -123,8 +123,7 @@ ArduinoMCP2515 mcp2515([]()
                        nullptr);
 
 Node::Heap<Node::DEFAULT_O1HEAP_SIZE> node_heap;
-CircularBuffer<Node::TReceiveCircularBuffer>::Heap<Node::DEFAULT_RX_QUEUE_SIZE> 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_1_0<>> heartbeat_pub = node_hdl.create_publisher<Heartbeat_1_0<>>(Heartbeat_1_0<>::PORT_ID, 1*1000*1000UL /* = 1 sec in usecs. */);
 Publisher<DistanceMessageType> tof_pub = node_hdl.create_publisher<DistanceMessageType>(OPEN_CYPHAL_ID_DISTANCE_DATA, 1*1000*1000UL /* = 1 sec in usecs. */);

diff --git a/src/CircularBuffer.hpp b/src/CircularBuffer.hpp
@@ -20,29 +20,40 @@
  * CLASS DECLARATION
  **************************************************************************************/
 
-template <typename T>
-class CircularBuffer
+class CircularBufferBase
 {
 public:
-  template <size_t SIZE>
-  struct Heap final : public std::array<T, SIZE> {};
-
+  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 <typename T>
+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<T> _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
@@ -10,8 +10,8 @@
  **************************************************************************************/
 
 template <typename T>
-CircularBuffer<T>::CircularBuffer(T * heap_ptr, size_t const heap_size)
-: _buffer{heap_ptr}
+CircularBuffer<T>::CircularBuffer(size_t const heap_size)
+: _buffer{new T[heap_size]}
 , _size{heap_size}
 , _head{0}
 , _tail{0}
@@ -38,7 +38,7 @@ void CircularBuffer<T>::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<T>::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
@@ -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<CircularBufferBase *>(new CircularBufferCan(rx_queue_capacity)) : static_cast<CircularBufferBase *>(new CircularBufferCanFd(rx_queue_capacity))}
+, _mtu_bytes{mtu_bytes}
 {
   _canard_hdl.node_id = node_id;
   _canard_hdl.user_reference = static_cast<void *>(_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<uint8_t, 8> 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<uint8_t, CANARD_MTU_CAN_CLASSIC> payload{};
+    memcpy(payload.data(), frame.payload, std::min(payload_size, payload.size()));
+    static_cast<CircularBufferCan *>(_canard_rx_queue.get())->enqueue(std::make_tuple(extended_can_id, payload_size, payload, rx_timestamp_us));
+  }
+  else
+  {
+    std::array<uint8_t, CANARD_MTU_CAN_FD> payload{};
+    memcpy(payload.data(), frame.payload, std::min(payload_size, payload.size()));
+    static_cast<CircularBufferCanFd *>(_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<const void *>(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<CircularBufferCan *>(_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<impl::SubscriptionBase *>(rx_subscription->user_reference);
-      sub_ptr->onTransferReceived(transfer);
+      const auto [extended_can_id, payload_size, payload, rx_timestamp_us] =
+        static_cast<CircularBufferCanFd *>(_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
@@ -44,7 +44,6 @@ class Node
   template <size_t SIZE>
   struct alignas(O1HEAP_ALIGNMENT) Heap final : public std::array<uint8_t, SIZE> {};
 
-  typedef std::tuple<uint32_t, size_t, std::array<uint8_t, 8>, 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<uint32_t, size_t, std::array<uint8_t, CANARD_MTU_CAN_CLASSIC>, CanardMicrosecond> TCircularBufferCan;
+  typedef std::tuple<uint32_t, size_t, std::array<uint8_t, CANARD_MTU_CAN_FD>,      CanardMicrosecond> TCircularBufferCanFd;
+
+  typedef CircularBuffer<TCircularBufferCan>   CircularBufferCan;
+  typedef CircularBuffer<TCircularBufferCanFd> CircularBufferCanFd;
+
   O1HeapInstance * _o1heap_ins;
   CanardInstance _canard_hdl;
   MicrosFunc const _micros_func;
   CanardTxQueue _canard_tx_queue;
-  CircularBuffer<TReceiveCircularBuffer> _canard_rx_queue;
+  std::shared_ptr<CircularBufferBase> _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<size_t MTU_BYTES>
+  void processRxFrame(uint32_t const extended_can_id,
+                      size_t const payload_size,
+                      std::array<uint8_t, MTU_BYTES> const & payload,
+                      CanardMicrosecond const rx_timestamp_us);
 };
 
 /**************************************************************************************

diff --git a/src/Node.ipp b/src/Node.ipp
@@ -90,3 +90,34 @@ ServiceClient<T_REQ> Node::create_service_client(CanardPortID const port_id,
 
   return clt;
 }
+
+template<size_t MTU_BYTES>
+void Node::processRxFrame(uint32_t const extended_can_id,
+                          size_t const payload_size,
+                          std::array<uint8_t, MTU_BYTES> 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<const void *>(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<impl::SubscriptionBase *>(rx_subscription->user_reference);
+    sub_ptr->onTransferReceived(rx_transfer);
+
+    /* Free dynamically allocated memory after processing. */
+    _canard_hdl.memory_free(&_canard_hdl, rx_transfer.payload);
+  }
+}