Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Support both CAN and CAN FD. #198

Merged
merged 7 commits into from
Jan 10, 2023
Merged

Support both CAN and CAN FD. #198

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
ea5ed66
Support both CAN and CAN FD.
aentinger Jan 10, 2023
e48072e
Fix another if/elseif to if/else.
aentinger Jan 10, 2023
185b76e
Hide raw pointer behind std::unique_ptr.
aentinger Jan 10, 2023
bf01ad2
Fix another if/elseif to if/else.
aentinger Jan 10, 2023
f0610d2
Fix typo: should be DEFAULT_RX_QUEUE_SIZE.
aentinger Jan 10, 2023
630bc30
Incredibly fugly code but at least all members are initialized in order.
aentinger Jan 10, 2023
6454839
Simplify code by using a templated function processRxFrame.
aentinger Jan 10, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 1 addition & 2 deletions examples/OpenCyphal-Blink/OpenCyphal-Blink.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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. */);
Expand Down
3 changes: 1 addition & 2 deletions examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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. */);

Expand Down
3 changes: 1 addition & 2 deletions examples/OpenCyphal-Heartbeat-Publisher/OpenCyphal-Heartbeat-Publisher.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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. */);

Expand Down
3 changes: 1 addition & 2 deletions examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
3 changes: 1 addition & 2 deletions examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
3 changes: 1 addition & 2 deletions examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
3 changes: 1 addition & 2 deletions examples/OpenCyphal-ToF-Distance-Sensor-Node/OpenCyphal-ToF-Distance-Sensor-Node.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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. */);
Expand Down
33 changes: 22 additions & 11 deletions src/CircularBuffer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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); }
Expand Down
8 changes: 4 additions & 4 deletions src/CircularBuffer.ipp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]}
aentinger marked this conversation as resolved.
Show resolved Hide resolved
, _size{heap_size}
, _head{0}
, _tail{0}
Expand All @@ -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++;
}
Expand All @@ -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;
Expand Down
93 changes: 61 additions & 32 deletions src/Node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))}
aentinger marked this conversation as resolved.
Show resolved Hide resolved
, _mtu_bytes{mtu_bytes}
{
_canard_hdl.node_id = node_id;
_canard_hdl.user_reference = static_cast<void *>(_o1heap_ins);
Expand All @@ -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));
}
aentinger marked this conversation as resolved.
Show resolved Hide resolved
}

bool Node::enqueue_transfer(CanardMicrosecond const tx_timeout_usec,
Expand Down Expand Up @@ -98,36 +106,57 @@ 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();

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());

processRxFrame(&rx_frame, 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();

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());

/* Free dynamically allocated memory after processing. */
_canard_hdl.memory_free(&_canard_hdl, transfer.payload);
processRxFrame(&rx_frame, rx_timestamp_us);
}
}
}

void Node::processRxFrame(CanardFrame const * frame, CanardMicrosecond const rx_timestamp_us)
{
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)
{
/* 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);

/* Free dynamically allocated memory after processing. */
_canard_hdl.memory_free(&_canard_hdl, transfer.payload);
}
}

void Node::processTxQueue(CanFrameTxFunc const tx_func)
{
for(CanardTxQueueItem * tx_queue_item = const_cast<CanardTxQueueItem *>(canardTxPeek(&_canard_tx_queue));
Expand Down
22 changes: 14 additions & 8 deletions src/Node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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; }
Expand Down Expand Up @@ -109,16 +107,24 @@ 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 processRxFrame(CanardFrame const * frame, CanardMicrosecond const rx_timestamp_us);
void processTxQueue(CanFrameTxFunc const tx_func);
};

Expand Down