From c58ef55956edaf36d19a5dfa7ab0c0544fd345e8 Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 09:32:47 +0100 Subject: [PATCH 01/16] returning metadata --- .../OpenCyphal-Heartbeat-Subscriber.ino | 40 ++++++++++++---- src/Node.hpp | 18 +++++-- src/Node.ipp | 47 ++++++++++++++++--- src/Subscription.hpp | 31 ++++++++++-- src/Subscription.ipp | 17 +++++-- 5 files changed, 126 insertions(+), 27 deletions(-) diff --git a/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino b/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino index 1685e8d8..183de157 100644 --- a/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino +++ b/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino @@ -26,15 +26,20 @@ using namespace uavcan::node; * CONSTANTS **************************************************************************************/ -static int const MKRCAN_MCP2515_CS_PIN = 3; -static int const MKRCAN_MCP2515_INT_PIN = 7; - +static int const MKRCAN_MCP2515_CS_PIN = 21; +static int const MKRCAN_MCP2515_INT_PIN = 22; +#define SPI_ROBOT_INT 22 +#define SPI_ROBOT_CS 21 +#define SPI_ROBOT_MISO 16 +#define SPI_ROBOT_MOSI 19 +#define SPI_ROBOT_SCK 18 /************************************************************************************** * FUNCTION DECLARATION **************************************************************************************/ void onReceiveBufferFull (CanardFrame const &); void onHeartbeat_1_0_Received(Heartbeat_1_0 const & msg); +void onHeartbeat_1_0_Received_meta(Heartbeat_1_0 const & msg, TransferMetadata const & metadata); /************************************************************************************** * GLOBAL VARIABLES @@ -48,10 +53,10 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, nullptr); Node::Heap node_heap; -Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }); - -Subscription heartbeat_subscription = node_hdl.create_subscription(onHeartbeat_1_0_Received); +Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, 22); +// Subscription heartbeat_subscription = node_hdl.create_subscription(onHeartbeat_1_0_Received); +Subscription heartbeat_subscription = node_hdl.create_subscription(onHeartbeat_1_0_Received_meta); /************************************************************************************** * SETUP/LOOP **************************************************************************************/ @@ -60,20 +65,28 @@ void setup() { Serial.begin(9600); while(!Serial) { } + delay(1000); + Serial.println("|---- OpenCyphal Heartbeat Subscription Example ----|"); /* Setup SPI access */ + SPI.setRX(SPI_ROBOT_MISO); + SPI.setTX(SPI_ROBOT_MOSI); + SPI.setSCK(SPI_ROBOT_SCK); + SPI.setCS(SPI_ROBOT_CS); SPI.begin(); pinMode(MKRCAN_MCP2515_CS_PIN, OUTPUT); digitalWrite(MKRCAN_MCP2515_CS_PIN, HIGH); /* Attach interrupt handler to register MCP2515 signaled by taking INT low */ pinMode(MKRCAN_MCP2515_INT_PIN, INPUT_PULLUP); - attachInterrupt(digitalPinToInterrupt(MKRCAN_MCP2515_INT_PIN), []() { mcp2515.onExternalEventHandler(); }, LOW); + attachInterrupt(digitalPinToInterrupt(MKRCAN_MCP2515_INT_PIN), []() { mcp2515.onExternalEventHandler(); }, FALLING); /* Initialize MCP2515 */ mcp2515.begin(); - mcp2515.setBitRate(CanBitRate::BR_250kBPS_16MHZ); + mcp2515.setBitRate(CanBitRate::BR_1000kBPS_16MHZ); mcp2515.setNormalMode(); + + Serial.println("setup finished"); } void loop() @@ -104,3 +117,14 @@ void onHeartbeat_1_0_Received(Heartbeat_1_0 const & msg) Serial.println(msg_buf); } + +void onHeartbeat_1_0_Received_meta(Heartbeat_1_0 const & msg, TransferMetadata const & metadata) +{ + char msg_buf[64]; + snprintf(msg_buf, sizeof(msg_buf), + "Uptime = %d, Health = %d, Mode = %d, VSSC = %d", + msg.uptime, msg.health.value, msg.mode.value, msg.vendor_specific_status_code); + + Serial.println(msg_buf); + Serial.println(metadata.node_id, HEX); +} diff --git a/src/Node.hpp b/src/Node.hpp index d8fdde2d..8196ce61 100644 --- a/src/Node.hpp +++ b/src/Node.hpp @@ -37,6 +37,11 @@ /************************************************************************************** * CLASS DECLARATION **************************************************************************************/ +struct TransferMetadata final +{ + CanardNodeID node_id; + // More stuff may appear here in the future! +}; class Node { @@ -81,10 +86,15 @@ class Node template Publisher create_publisher(CanardPortID const port_id, CanardMicrosecond const tx_timeout_usec); - template - Subscription create_subscription(OnReceiveCb&& on_receive_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); - template - Subscription create_subscription(CanardPortID const port_id, OnReceiveCb&& on_receive_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); + template + Subscription create_subscription(std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); + template + Subscription create_subscription(CanardPortID const port_id, std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); + + template + Subscription create_subscription(std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); + template + Subscription create_subscription(CanardPortID const port_id, std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); template ServiceServer create_service_server(CanardMicrosecond const tx_timeout_usec, OnRequestCb&& on_request_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); diff --git a/src/Node.ipp b/src/Node.ipp index da5a60a2..a4068ab6 100644 --- a/src/Node.ipp +++ b/src/Node.ipp @@ -40,25 +40,58 @@ Publisher Node::create_publisher(CanardPortID const port_id, CanardMicrosecon ); } -template -Subscription Node::create_subscription(OnReceiveCb&& on_receive_cb, CanardMicrosecond const tid_timeout_usec) +template +Subscription Node::create_subscription(std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec) +{ + static_assert(T::_traits_::HasFixedPortID, "T does not have a fixed port id."); + return create_subscription(T::_traits_::FixedPortId, std::forward>(on_receive_cb), tid_timeout_usec); +} + +template +Subscription Node::create_subscription(CanardPortID const port_id, std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec) +{ + static_assert(!T::_traits_::IsServiceType, "T is not message type"); + + if (_opt_port_list_pub.has_value()) + _opt_port_list_pub.value()->add_subscriber(port_id); + + auto sub = std::make_shared>( + *this, + port_id, + std::forward>(on_receive_cb) + ); + + int8_t const rc = canardRxSubscribe(&_canard_hdl, + CanardTransferKindMessage, + port_id, + T::_traits_::ExtentBytes, + tid_timeout_usec, + &(sub->canard_rx_subscription())); + if (rc < 0) + return nullptr; + + return sub; +} + +template +Subscription Node::create_subscription(std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec) { static_assert(T::_traits_::HasFixedPortID, "T does not have a fixed port id."); - return create_subscription(T::_traits_::FixedPortId, on_receive_cb, tid_timeout_usec); + return create_subscription(T::_traits_::FixedPortId, std::forward>(on_receive_cb), tid_timeout_usec); } -template -Subscription Node::create_subscription(CanardPortID const port_id, OnReceiveCb&& on_receive_cb, CanardMicrosecond const tid_timeout_usec) +template +Subscription Node::create_subscription(CanardPortID const port_id, std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec) { static_assert(!T::_traits_::IsServiceType, "T is not message type"); if (_opt_port_list_pub.has_value()) _opt_port_list_pub.value()->add_subscriber(port_id); - auto sub = std::make_shared>( + auto sub = std::make_shared>( *this, port_id, - std::forward(on_receive_cb) + std::forward>(on_receive_cb) ); int8_t const rc = canardRxSubscribe(&_canard_hdl, diff --git a/src/Subscription.hpp b/src/Subscription.hpp index 93f6584f..1ef70d63 100644 --- a/src/Subscription.hpp +++ b/src/Subscription.hpp @@ -16,6 +16,7 @@ #include "Node.hpp" + /************************************************************************************** * NAMESPACE **************************************************************************************/ @@ -23,20 +24,42 @@ namespace impl { +// #if LIBCANARD +// #define CanardNodeID CyphalNodeID; +// #elif LIBUDPARD +// #define UdpardNodeID CyphalNodeID; +// #else +// # error "We only support CAN or UDP as Cyphal transport layer" +// #endif + + /************************************************************************************** * CLASS DECLARATION **************************************************************************************/ -template +template + class Subscription final : public SubscriptionBase { + public: - Subscription(Node & node_hdl, CanardPortID const port_id, OnReceiveCb const & on_receive_cb) + using OnReceiveCallback_Default= std::function; + using OnReceiveCallback_Ext = std::function; + Subscription(Node & node_hdl, CanardPortID const port_id, OnReceiveCallback_Default const & on_receive_cb) : SubscriptionBase{CanardTransferKindMessage} , _node_hdl{node_hdl} , _port_id{port_id} , _on_receive_cb{on_receive_cb} { } + + Subscription(Node & node_hdl, CanardPortID const port_id, OnReceiveCallback_Ext const & on_receive_cb_ext) + : SubscriptionBase{CanardTransferKindMessage} + , _node_hdl{node_hdl} + , _port_id{port_id} + , _on_receive_cb_ext{on_receive_cb_ext} + { + output_meta = true; + } virtual ~Subscription(); @@ -46,7 +69,9 @@ class Subscription final : public SubscriptionBase private: Node & _node_hdl; CanardPortID const _port_id; - OnReceiveCb _on_receive_cb; + OnReceiveCallback_Default _on_receive_cb; + OnReceiveCallback_Ext _on_receive_cb_ext; + bool output_meta = false; }; /************************************************************************************** diff --git a/src/Subscription.ipp b/src/Subscription.ipp index ae7dc40b..624d0097 100644 --- a/src/Subscription.ipp +++ b/src/Subscription.ipp @@ -23,8 +23,8 @@ namespace impl { * CTOR/DTOR **************************************************************************************/ -template -Subscription::~Subscription() +template +Subscription::~Subscription() { _node_hdl.unsubscribe(_port_id, SubscriptionBase::canard_transfer_kind()); } @@ -33,15 +33,22 @@ Subscription::~Subscription() * PUBLIC MEMBER FUNCTIONS **************************************************************************************/ -template -bool Subscription::onTransferReceived(CanardRxTransfer const & transfer) +template +bool Subscription::onTransferReceived(CanardRxTransfer const & transfer) { T msg; nunavut::support::const_bitspan msg_bitspan(static_cast(transfer.payload), transfer.payload_size); auto const rc = deserialize(msg, msg_bitspan); if (!rc) return false; - _on_receive_cb(msg); + if(output_meta) { + TransferMetadata meta_data; + meta_data.node_id = static_cast(transfer.metadata.remote_node_id); + _on_receive_cb_ext(msg, meta_data); + } else { + _on_receive_cb(msg); + } + return true; } From d9d381fe60429c0212e0ec0c117ebae5897ffa8b Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 10:11:32 +0100 Subject: [PATCH 02/16] move constructing transfer metadata to function --- src/Subscription.hpp | 6 ++++-- src/Subscription.ipp | 14 ++++++++++---- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/Subscription.hpp b/src/Subscription.hpp index 1ef70d63..6754ccbc 100644 --- a/src/Subscription.hpp +++ b/src/Subscription.hpp @@ -58,7 +58,7 @@ class Subscription final : public SubscriptionBase , _port_id{port_id} , _on_receive_cb_ext{on_receive_cb_ext} { - output_meta = true; + return_metadata = true; } virtual ~Subscription(); @@ -71,7 +71,9 @@ class Subscription final : public SubscriptionBase CanardPortID const _port_id; OnReceiveCallback_Default _on_receive_cb; OnReceiveCallback_Ext _on_receive_cb_ext; - bool output_meta = false; + bool return_metadata = false; + + TransferMetadata fillMetadata(CanardRxTransfer const & transfer); }; /************************************************************************************** diff --git a/src/Subscription.ipp b/src/Subscription.ipp index 624d0097..513ce4a7 100644 --- a/src/Subscription.ipp +++ b/src/Subscription.ipp @@ -41,10 +41,8 @@ bool Subscription::onTransferReceived(CanardRxTransfer const & transfer) auto const rc = deserialize(msg, msg_bitspan); if (!rc) return false; - if(output_meta) { - TransferMetadata meta_data; - meta_data.node_id = static_cast(transfer.metadata.remote_node_id); - _on_receive_cb_ext(msg, meta_data); + if(return_metadata) { + _on_receive_cb_ext(msg, fillMetadata(transfer)); } else { _on_receive_cb(msg); } @@ -53,6 +51,14 @@ bool Subscription::onTransferReceived(CanardRxTransfer const & transfer) return true; } +template +TransferMetadata Subscription::fillMetadata(CanardRxTransfer const & transfer) +{ + TransferMetadata transfer_metadata; + transfer_metadata.node_id = static_cast(transfer.metadata.remote_node_id); + + return transfer_metadata; +} /************************************************************************************** * NAMESPACE **************************************************************************************/ From 0fdc32f7e06c24735a5814b481e00d38f58fc088 Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 10:12:09 +0100 Subject: [PATCH 03/16] move TransferMetadata struct --- src/Node.hpp | 6 +----- src/Subscription.hpp | 1 + src/util/transfer_metadata.hpp | 21 +++++++++++++++++++++ 3 files changed, 23 insertions(+), 5 deletions(-) create mode 100644 src/util/transfer_metadata.hpp diff --git a/src/Node.hpp b/src/Node.hpp index 8196ce61..4f5c6368 100644 --- a/src/Node.hpp +++ b/src/Node.hpp @@ -30,6 +30,7 @@ #include "util/nodeinfo/NodeInfoBase.hpp" #include "util/registry/registry_impl.hpp" #include "util/port/PortListPublisherBase.hpp" +#include "util/transfer_metadata.hpp" #include "libo1heap/o1heap.h" #include "libcanard/canard.h" @@ -37,11 +38,6 @@ /************************************************************************************** * CLASS DECLARATION **************************************************************************************/ -struct TransferMetadata final -{ - CanardNodeID node_id; - // More stuff may appear here in the future! -}; class Node { diff --git a/src/Subscription.hpp b/src/Subscription.hpp index 6754ccbc..0e5fc334 100644 --- a/src/Subscription.hpp +++ b/src/Subscription.hpp @@ -15,6 +15,7 @@ #include "SubscriptionBase.h" #include "Node.hpp" +#include "util/transfer_metadata.hpp" /************************************************************************************** diff --git a/src/util/transfer_metadata.hpp b/src/util/transfer_metadata.hpp new file mode 100644 index 00000000..aeb983a9 --- /dev/null +++ b/src/util/transfer_metadata.hpp @@ -0,0 +1,21 @@ + +#ifndef ARDUINO_CYPHAL_TRANSFER_METADATA_HPP_ +#define ARDUINO_CYPHAL_TRANSFER_METADATA_HPP_ + +#if defined __has_include +# if __has_include () + using CyphalNodeID = CanardNodeID; +# elif __has_include () + using CyphalNodeID = UdpardNodeID; +# endif +#else +# error "We only support CAN or UDP as Cyphal transport layer" +#endif + +struct TransferMetadata final +{ + CyphalNodeID node_id; + // More stuff may appear here in the future! +}; + +#endif // ARDUINO_CYPHAL_TRANSFER_METADATA_HPP_ From 77781f84160f59956cd0242e60e2d6972919e0cb Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 10:14:18 +0100 Subject: [PATCH 04/16] add in copyright --- src/Subscription.hpp | 2 +- src/util/transfer_metadata.hpp | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/Subscription.hpp b/src/Subscription.hpp index 0e5fc334..87ff6d91 100644 --- a/src/Subscription.hpp +++ b/src/Subscription.hpp @@ -60,7 +60,7 @@ class Subscription final : public SubscriptionBase , _on_receive_cb_ext{on_receive_cb_ext} { return_metadata = true; - } + } virtual ~Subscription(); diff --git a/src/util/transfer_metadata.hpp b/src/util/transfer_metadata.hpp index aeb983a9..6a824761 100644 --- a/src/util/transfer_metadata.hpp +++ b/src/util/transfer_metadata.hpp @@ -1,3 +1,9 @@ +/** + * This software is distributed under the terms of the MIT License. + * Copyright (c) 2020-2023 LXRobotics. + * Author: Alexander Entinger + * Contributors: https://github.com/107-systems/107-Arduino-Cyphal/graphs/contributors. + */ #ifndef ARDUINO_CYPHAL_TRANSFER_METADATA_HPP_ #define ARDUINO_CYPHAL_TRANSFER_METADATA_HPP_ From 7f4654fe04632016decc82a36aaabb5c16208e77 Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 10:51:41 +0100 Subject: [PATCH 05/16] reset default subscriber --- .../OpenCyphal-Heartbeat-Subscriber.ino | 15 +-------------- 1 file changed, 1 insertion(+), 14 deletions(-) diff --git a/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino b/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino index 183de157..8768ad93 100644 --- a/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino +++ b/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino @@ -39,7 +39,6 @@ static int const MKRCAN_MCP2515_INT_PIN = 22; void onReceiveBufferFull (CanardFrame const &); void onHeartbeat_1_0_Received(Heartbeat_1_0 const & msg); -void onHeartbeat_1_0_Received_meta(Heartbeat_1_0 const & msg, TransferMetadata const & metadata); /************************************************************************************** * GLOBAL VARIABLES @@ -55,8 +54,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, Node::Heap node_heap; Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, 22); -// Subscription heartbeat_subscription = node_hdl.create_subscription(onHeartbeat_1_0_Received); -Subscription heartbeat_subscription = node_hdl.create_subscription(onHeartbeat_1_0_Received_meta); +Subscription heartbeat_subscription = node_hdl.create_subscription(onHeartbeat_1_0_Received); /************************************************************************************** * SETUP/LOOP **************************************************************************************/ @@ -117,14 +115,3 @@ void onHeartbeat_1_0_Received(Heartbeat_1_0 const & msg) Serial.println(msg_buf); } - -void onHeartbeat_1_0_Received_meta(Heartbeat_1_0 const & msg, TransferMetadata const & metadata) -{ - char msg_buf[64]; - snprintf(msg_buf, sizeof(msg_buf), - "Uptime = %d, Health = %d, Mode = %d, VSSC = %d", - msg.uptime, msg.health.value, msg.mode.value, msg.vendor_specific_status_code); - - Serial.println(msg_buf); - Serial.println(metadata.node_id, HEX); -} From da2d3e89f2506912b976ce13f35c467a8ec94412 Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 10:52:18 +0100 Subject: [PATCH 06/16] add example to get the metadata from subscription --- ...hal-Heartbeat-Subscriber-With-Metadata.ino | 123 ++++++++++++++++++ 1 file changed, 123 insertions(+) create mode 100644 examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino diff --git a/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino b/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino new file mode 100644 index 00000000..6ad653ff --- /dev/null +++ b/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino @@ -0,0 +1,123 @@ +/* + * This example shows reception of a OpenCyphal heartbeat message via CAN. + * + * Hardware: + * - Arduino MKR family board, e.g. MKR VIDOR 4000 + * - Arduino MKR CAN shield + */ + +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include + +#include <107-Arduino-Cyphal.h> +#include <107-Arduino-MCP2515.h> +#include <107-Arduino-CriticalSection.h> + +/************************************************************************************** + * NAMESPACE + **************************************************************************************/ + +using namespace uavcan::node; + +/************************************************************************************** + * CONSTANTS + **************************************************************************************/ + +static int const MKRCAN_MCP2515_CS_PIN = 21; +static int const MKRCAN_MCP2515_INT_PIN = 22; +#define SPI_ROBOT_INT 22 +#define SPI_ROBOT_CS 21 +#define SPI_ROBOT_MISO 16 +#define SPI_ROBOT_MOSI 19 +#define SPI_ROBOT_SCK 18 +/************************************************************************************** + * FUNCTION DECLARATION + **************************************************************************************/ + +void onReceiveBufferFull(CanardFrame const &); +void onHeartbeat_1_0_Received(Heartbeat_1_0 const & msg, TransferMetadata const & metadata); + +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ + +ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, + []() { digitalWrite(MKRCAN_MCP2515_CS_PIN, HIGH); }, + [](uint8_t const data) { return SPI.transfer(data); }, + micros, + onReceiveBufferFull, + nullptr); + +Node::Heap node_heap; +Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, 22); + +Subscription heartbeat_subscription = node_hdl.create_subscription(onHeartbeat_1_0_Received); +/************************************************************************************** + * SETUP/LOOP + **************************************************************************************/ + +void setup() +{ + Serial.begin(9600); + while(!Serial) { } + delay(1000); + Serial.println("|---- OpenCyphal Heartbeat Subscription Example ----|"); + + /* Setup SPI access */ + SPI.setRX(SPI_ROBOT_MISO); + SPI.setTX(SPI_ROBOT_MOSI); + SPI.setSCK(SPI_ROBOT_SCK); + SPI.setCS(SPI_ROBOT_CS); + SPI.begin(); + pinMode(MKRCAN_MCP2515_CS_PIN, OUTPUT); + digitalWrite(MKRCAN_MCP2515_CS_PIN, HIGH); + + /* Attach interrupt handler to register MCP2515 signaled by taking INT low */ + pinMode(MKRCAN_MCP2515_INT_PIN, INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(MKRCAN_MCP2515_INT_PIN), []() { mcp2515.onExternalEventHandler(); }, FALLING); + + /* Initialize MCP2515 */ + mcp2515.begin(); + mcp2515.setBitRate(CanBitRate::BR_1000kBPS_16MHZ); + mcp2515.setNormalMode(); + + Serial.println("setup finished"); +} + +void loop() +{ + /* Process all pending OpenCyphal actions. + */ + { + CriticalSection crit_sec; + node_hdl.spinSome(); + } +} + +/************************************************************************************** + * FUNCTION DEFINITION + **************************************************************************************/ + +void onReceiveBufferFull(CanardFrame const & frame) +{ + node_hdl.onCanFrameReceived(frame); +} + +void onHeartbeat_1_0_Received(Heartbeat_1_0 const & msg, TransferMetadata const & metadata) +{ + char msg_buf[70]; + snprintf( + msg_buf, + sizeof(msg_buf), + "Node ID= %d, Uptime = %d, Health = %d, Mode = %d, VSSC = %d", + metadata.node_id, + msg.uptime, + msg.health.value, + msg.mode.value, + msg.vendor_specific_status_code); + + Serial.println(msg_buf); +} From a03514eb257871c153ae8374a1725c679354cf4c Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 10:52:32 +0100 Subject: [PATCH 07/16] update serial print --- .../OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino b/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino index 6ad653ff..673f14f4 100644 --- a/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino +++ b/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino @@ -64,7 +64,7 @@ void setup() Serial.begin(9600); while(!Serial) { } delay(1000); - Serial.println("|---- OpenCyphal Heartbeat Subscription Example ----|"); + Serial.println("|---- OpenCyphal Heartbeat Subscription With Metadata Example ----|"); /* Setup SPI access */ SPI.setRX(SPI_ROBOT_MISO); From d6d724d8d4a96dc51090d572f754b5a9318ec65a Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 10:53:22 +0100 Subject: [PATCH 08/16] remove empty line --- src/Subscription.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/Subscription.hpp b/src/Subscription.hpp index 87ff6d91..8294960a 100644 --- a/src/Subscription.hpp +++ b/src/Subscription.hpp @@ -39,7 +39,6 @@ namespace impl **************************************************************************************/ template - class Subscription final : public SubscriptionBase { From dd13e90bfe508fdaa17d7d96a1190906abe44439 Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 10:58:10 +0100 Subject: [PATCH 09/16] reset spi pins to defaults --- ...hal-Heartbeat-Subscriber-With-Metadata.ino | 20 ++++++------------- .../OpenCyphal-Heartbeat-Subscriber.ino | 18 +++++------------ 2 files changed, 11 insertions(+), 27 deletions(-) diff --git a/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino b/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino index 673f14f4..dc8cb58b 100644 --- a/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino +++ b/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino @@ -26,13 +26,9 @@ using namespace uavcan::node; * CONSTANTS **************************************************************************************/ -static int const MKRCAN_MCP2515_CS_PIN = 21; -static int const MKRCAN_MCP2515_INT_PIN = 22; -#define SPI_ROBOT_INT 22 -#define SPI_ROBOT_CS 21 -#define SPI_ROBOT_MISO 16 -#define SPI_ROBOT_MOSI 19 -#define SPI_ROBOT_SCK 18 +static int const MKRCAN_MCP2515_CS_PIN = 3; +static int const MKRCAN_MCP2515_INT_PIN = 7; + /************************************************************************************** * FUNCTION DECLARATION **************************************************************************************/ @@ -52,7 +48,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, nullptr); Node::Heap node_heap; -Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, 22); +Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }); Subscription heartbeat_subscription = node_hdl.create_subscription(onHeartbeat_1_0_Received); /************************************************************************************** @@ -67,21 +63,17 @@ void setup() Serial.println("|---- OpenCyphal Heartbeat Subscription With Metadata Example ----|"); /* Setup SPI access */ - SPI.setRX(SPI_ROBOT_MISO); - SPI.setTX(SPI_ROBOT_MOSI); - SPI.setSCK(SPI_ROBOT_SCK); - SPI.setCS(SPI_ROBOT_CS); SPI.begin(); pinMode(MKRCAN_MCP2515_CS_PIN, OUTPUT); digitalWrite(MKRCAN_MCP2515_CS_PIN, HIGH); /* Attach interrupt handler to register MCP2515 signaled by taking INT low */ pinMode(MKRCAN_MCP2515_INT_PIN, INPUT_PULLUP); - attachInterrupt(digitalPinToInterrupt(MKRCAN_MCP2515_INT_PIN), []() { mcp2515.onExternalEventHandler(); }, FALLING); + attachInterrupt(digitalPinToInterrupt(MKRCAN_MCP2515_INT_PIN), []() { mcp2515.onExternalEventHandler(); }, LOW); /* Initialize MCP2515 */ mcp2515.begin(); - mcp2515.setBitRate(CanBitRate::BR_1000kBPS_16MHZ); + mcp2515.setBitRate(CanBitRate::BR_250kBPS_16MHZ); mcp2515.setNormalMode(); Serial.println("setup finished"); diff --git a/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino b/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino index 8768ad93..a2def572 100644 --- a/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino +++ b/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino @@ -26,13 +26,9 @@ using namespace uavcan::node; * CONSTANTS **************************************************************************************/ -static int const MKRCAN_MCP2515_CS_PIN = 21; -static int const MKRCAN_MCP2515_INT_PIN = 22; -#define SPI_ROBOT_INT 22 -#define SPI_ROBOT_CS 21 -#define SPI_ROBOT_MISO 16 -#define SPI_ROBOT_MOSI 19 -#define SPI_ROBOT_SCK 18 +static int const MKRCAN_MCP2515_CS_PIN = 3; +static int const MKRCAN_MCP2515_INT_PIN = 7; + /************************************************************************************** * FUNCTION DECLARATION **************************************************************************************/ @@ -52,7 +48,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, nullptr); Node::Heap node_heap; -Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, 22); +Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }); Subscription heartbeat_subscription = node_hdl.create_subscription(onHeartbeat_1_0_Received); /************************************************************************************** @@ -67,10 +63,6 @@ void setup() Serial.println("|---- OpenCyphal Heartbeat Subscription Example ----|"); /* Setup SPI access */ - SPI.setRX(SPI_ROBOT_MISO); - SPI.setTX(SPI_ROBOT_MOSI); - SPI.setSCK(SPI_ROBOT_SCK); - SPI.setCS(SPI_ROBOT_CS); SPI.begin(); pinMode(MKRCAN_MCP2515_CS_PIN, OUTPUT); digitalWrite(MKRCAN_MCP2515_CS_PIN, HIGH); @@ -81,7 +73,7 @@ void setup() /* Initialize MCP2515 */ mcp2515.begin(); - mcp2515.setBitRate(CanBitRate::BR_1000kBPS_16MHZ); + mcp2515.setBitRate(CanBitRate::BR_250kBPS_16MHZ); mcp2515.setNormalMode(); Serial.println("setup finished"); From 9fe0853a780f6f57db225edac9cd9e97719e217a Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 10:59:48 +0100 Subject: [PATCH 10/16] revert example mods --- .../OpenCyphal-Heartbeat-Subscriber.ino | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino b/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino index a2def572..5eccb6a0 100644 --- a/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino +++ b/examples/OpenCyphal-Heartbeat-Subscriber/OpenCyphal-Heartbeat-Subscriber.ino @@ -51,6 +51,7 @@ Node::Heap node_heap; Node node_hdl(node_heap.data(), node_heap.size(), micros, [] (CanardFrame const & frame) { return mcp2515.transmit(frame); }); Subscription heartbeat_subscription = node_hdl.create_subscription(onHeartbeat_1_0_Received); + /************************************************************************************** * SETUP/LOOP **************************************************************************************/ @@ -69,7 +70,7 @@ void setup() /* Attach interrupt handler to register MCP2515 signaled by taking INT low */ pinMode(MKRCAN_MCP2515_INT_PIN, INPUT_PULLUP); - attachInterrupt(digitalPinToInterrupt(MKRCAN_MCP2515_INT_PIN), []() { mcp2515.onExternalEventHandler(); }, FALLING); + attachInterrupt(digitalPinToInterrupt(MKRCAN_MCP2515_INT_PIN), []() { mcp2515.onExternalEventHandler(); }, LOW); /* Initialize MCP2515 */ mcp2515.begin(); From af41eb031309b4bd23e9285badfd327b4a4614b0 Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 10:59:57 +0100 Subject: [PATCH 11/16] remove commented code --- src/Subscription.hpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/Subscription.hpp b/src/Subscription.hpp index 8294960a..0cddcb24 100644 --- a/src/Subscription.hpp +++ b/src/Subscription.hpp @@ -25,15 +25,6 @@ namespace impl { -// #if LIBCANARD -// #define CanardNodeID CyphalNodeID; -// #elif LIBUDPARD -// #define UdpardNodeID CyphalNodeID; -// #else -// # error "We only support CAN or UDP as Cyphal transport layer" -// #endif - - /************************************************************************************** * CLASS DECLARATION **************************************************************************************/ From 1bef48769500a803cdff41d4d2152980cf1338bb Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 15:56:25 +0100 Subject: [PATCH 12/16] Working with code from Pavel --- src/Node.hpp | 13 ++++-------- src/Node.ipp | 47 +++++++------------------------------------- src/Subscription.hpp | 19 +++--------------- src/Subscription.ipp | 21 +++++++++++--------- 4 files changed, 26 insertions(+), 74 deletions(-) diff --git a/src/Node.hpp b/src/Node.hpp index 4f5c6368..f647bc2a 100644 --- a/src/Node.hpp +++ b/src/Node.hpp @@ -82,15 +82,10 @@ class Node template Publisher create_publisher(CanardPortID const port_id, CanardMicrosecond const tx_timeout_usec); - template - Subscription create_subscription(std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); - template - Subscription create_subscription(CanardPortID const port_id, std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); - - template - Subscription create_subscription(std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); - template - Subscription create_subscription(CanardPortID const port_id, std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); + template + Subscription create_subscription(OnReceiveCb&& on_receive_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); + template + Subscription create_subscription(CanardPortID const port_id, OnReceiveCb&& on_receive_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); template ServiceServer create_service_server(CanardMicrosecond const tx_timeout_usec, OnRequestCb&& on_request_cb, CanardMicrosecond const tid_timeout_usec = CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); diff --git a/src/Node.ipp b/src/Node.ipp index a4068ab6..da5a60a2 100644 --- a/src/Node.ipp +++ b/src/Node.ipp @@ -40,58 +40,25 @@ Publisher Node::create_publisher(CanardPortID const port_id, CanardMicrosecon ); } -template -Subscription Node::create_subscription(std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec) -{ - static_assert(T::_traits_::HasFixedPortID, "T does not have a fixed port id."); - return create_subscription(T::_traits_::FixedPortId, std::forward>(on_receive_cb), tid_timeout_usec); -} - -template -Subscription Node::create_subscription(CanardPortID const port_id, std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec) -{ - static_assert(!T::_traits_::IsServiceType, "T is not message type"); - - if (_opt_port_list_pub.has_value()) - _opt_port_list_pub.value()->add_subscriber(port_id); - - auto sub = std::make_shared>( - *this, - port_id, - std::forward>(on_receive_cb) - ); - - int8_t const rc = canardRxSubscribe(&_canard_hdl, - CanardTransferKindMessage, - port_id, - T::_traits_::ExtentBytes, - tid_timeout_usec, - &(sub->canard_rx_subscription())); - if (rc < 0) - return nullptr; - - return sub; -} - -template -Subscription Node::create_subscription(std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec) +template +Subscription Node::create_subscription(OnReceiveCb&& on_receive_cb, CanardMicrosecond const tid_timeout_usec) { static_assert(T::_traits_::HasFixedPortID, "T does not have a fixed port id."); - return create_subscription(T::_traits_::FixedPortId, std::forward>(on_receive_cb), tid_timeout_usec); + return create_subscription(T::_traits_::FixedPortId, on_receive_cb, tid_timeout_usec); } -template -Subscription Node::create_subscription(CanardPortID const port_id, std::function&& on_receive_cb, CanardMicrosecond const tid_timeout_usec) +template +Subscription Node::create_subscription(CanardPortID const port_id, OnReceiveCb&& on_receive_cb, CanardMicrosecond const tid_timeout_usec) { static_assert(!T::_traits_::IsServiceType, "T is not message type"); if (_opt_port_list_pub.has_value()) _opt_port_list_pub.value()->add_subscriber(port_id); - auto sub = std::make_shared>( + auto sub = std::make_shared>( *this, port_id, - std::forward>(on_receive_cb) + std::forward(on_receive_cb) ); int8_t const rc = canardRxSubscribe(&_canard_hdl, diff --git a/src/Subscription.hpp b/src/Subscription.hpp index 0cddcb24..9da0c2b5 100644 --- a/src/Subscription.hpp +++ b/src/Subscription.hpp @@ -29,28 +29,17 @@ namespace impl * CLASS DECLARATION **************************************************************************************/ -template +template class Subscription final : public SubscriptionBase { public: - using OnReceiveCallback_Default= std::function; - using OnReceiveCallback_Ext = std::function; - Subscription(Node & node_hdl, CanardPortID const port_id, OnReceiveCallback_Default const & on_receive_cb) + Subscription(Node & node_hdl, CanardPortID const port_id, OnReceiveCb const & on_receive_cb) : SubscriptionBase{CanardTransferKindMessage} , _node_hdl{node_hdl} , _port_id{port_id} , _on_receive_cb{on_receive_cb} { } - - Subscription(Node & node_hdl, CanardPortID const port_id, OnReceiveCallback_Ext const & on_receive_cb_ext) - : SubscriptionBase{CanardTransferKindMessage} - , _node_hdl{node_hdl} - , _port_id{port_id} - , _on_receive_cb_ext{on_receive_cb_ext} - { - return_metadata = true; - } virtual ~Subscription(); @@ -60,9 +49,7 @@ class Subscription final : public SubscriptionBase private: Node & _node_hdl; CanardPortID const _port_id; - OnReceiveCallback_Default _on_receive_cb; - OnReceiveCallback_Ext _on_receive_cb_ext; - bool return_metadata = false; + OnReceiveCb _on_receive_cb; TransferMetadata fillMetadata(CanardRxTransfer const & transfer); }; diff --git a/src/Subscription.ipp b/src/Subscription.ipp index 513ce4a7..191fb031 100644 --- a/src/Subscription.ipp +++ b/src/Subscription.ipp @@ -23,8 +23,8 @@ namespace impl { * CTOR/DTOR **************************************************************************************/ -template -Subscription::~Subscription() +template +Subscription::~Subscription() { _node_hdl.unsubscribe(_port_id, SubscriptionBase::canard_transfer_kind()); } @@ -33,17 +33,20 @@ Subscription::~Subscription() * PUBLIC MEMBER FUNCTIONS **************************************************************************************/ -template -bool Subscription::onTransferReceived(CanardRxTransfer const & transfer) +template +bool Subscription::onTransferReceived(CanardRxTransfer const & transfer) { T msg; nunavut::support::const_bitspan msg_bitspan(static_cast(transfer.payload), transfer.payload_size); auto const rc = deserialize(msg, msg_bitspan); if (!rc) return false; - if(return_metadata) { - _on_receive_cb_ext(msg, fillMetadata(transfer)); - } else { + if constexpr (std::is_invocable_v) + { + _on_receive_cb(msg, fillMetadata(transfer)); + } + else + { _on_receive_cb(msg); } @@ -51,8 +54,8 @@ bool Subscription::onTransferReceived(CanardRxTransfer const & transfer) return true; } -template -TransferMetadata Subscription::fillMetadata(CanardRxTransfer const & transfer) +template +TransferMetadata Subscription::fillMetadata(CanardRxTransfer const & transfer) { TransferMetadata transfer_metadata; transfer_metadata.node_id = static_cast(transfer.metadata.remote_node_id); From ac26bec8b387b51fac2d9054a8036d1ed10b77ab Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 15:57:12 +0100 Subject: [PATCH 13/16] styling updates --- src/Subscription.hpp | 1 - src/Subscription.ipp | 7 ++----- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/Subscription.hpp b/src/Subscription.hpp index 9da0c2b5..3d354276 100644 --- a/src/Subscription.hpp +++ b/src/Subscription.hpp @@ -32,7 +32,6 @@ namespace impl template class Subscription final : public SubscriptionBase { - public: Subscription(Node & node_hdl, CanardPortID const port_id, OnReceiveCb const & on_receive_cb) : SubscriptionBase{CanardTransferKindMessage} diff --git a/src/Subscription.ipp b/src/Subscription.ipp index 191fb031..98f5c8b4 100644 --- a/src/Subscription.ipp +++ b/src/Subscription.ipp @@ -41,12 +41,9 @@ bool Subscription::onTransferReceived(CanardRxTransfer const & t auto const rc = deserialize(msg, msg_bitspan); if (!rc) return false; - if constexpr (std::is_invocable_v) - { + if constexpr (std::is_invocable_v) { _on_receive_cb(msg, fillMetadata(transfer)); - } - else - { + } else { _on_receive_cb(msg); } From b355e85abd471e88af4f02beb01a0d80a6080be7 Mon Sep 17 00:00:00 2001 From: Rebecca Date: Thu, 1 Jun 2023 16:17:37 +0100 Subject: [PATCH 14/16] change to uint16_t --- src/util/transfer_metadata.hpp | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/src/util/transfer_metadata.hpp b/src/util/transfer_metadata.hpp index 6a824761..7305075f 100644 --- a/src/util/transfer_metadata.hpp +++ b/src/util/transfer_metadata.hpp @@ -8,19 +8,9 @@ #ifndef ARDUINO_CYPHAL_TRANSFER_METADATA_HPP_ #define ARDUINO_CYPHAL_TRANSFER_METADATA_HPP_ -#if defined __has_include -# if __has_include () - using CyphalNodeID = CanardNodeID; -# elif __has_include () - using CyphalNodeID = UdpardNodeID; -# endif -#else -# error "We only support CAN or UDP as Cyphal transport layer" -#endif - struct TransferMetadata final { - CyphalNodeID node_id; + uint16_t node_id; // More stuff may appear here in the future! }; From a51e18ebd53fcece77937410c24d92f94e6c3766 Mon Sep 17 00:00:00 2001 From: Rebecca Date: Mon, 5 Jun 2023 08:49:01 +0100 Subject: [PATCH 15/16] Move fillMetadata to subscription_base --- .../OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino | 2 +- src/Node.hpp | 1 - src/Subscription.hpp | 2 -- src/Subscription.ipp | 8 -------- src/SubscriptionBase.h | 9 +++++++++ src/util/transfer_metadata.hpp | 2 +- 6 files changed, 11 insertions(+), 13 deletions(-) diff --git a/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino b/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino index dc8cb58b..7726a3c8 100644 --- a/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino +++ b/examples/OpenCyphal-Heartbeat-Subscriber-With-Metadata/OpenCyphal-Heartbeat-Subscriber-With-Metadata.ino @@ -105,7 +105,7 @@ void onHeartbeat_1_0_Received(Heartbeat_1_0 const & msg, TransferMetadata const msg_buf, sizeof(msg_buf), "Node ID= %d, Uptime = %d, Health = %d, Mode = %d, VSSC = %d", - metadata.node_id, + metadata.remote_node_id, msg.uptime, msg.health.value, msg.mode.value, diff --git a/src/Node.hpp b/src/Node.hpp index f647bc2a..d8fdde2d 100644 --- a/src/Node.hpp +++ b/src/Node.hpp @@ -30,7 +30,6 @@ #include "util/nodeinfo/NodeInfoBase.hpp" #include "util/registry/registry_impl.hpp" #include "util/port/PortListPublisherBase.hpp" -#include "util/transfer_metadata.hpp" #include "libo1heap/o1heap.h" #include "libcanard/canard.h" diff --git a/src/Subscription.hpp b/src/Subscription.hpp index 3d354276..3aadbdd9 100644 --- a/src/Subscription.hpp +++ b/src/Subscription.hpp @@ -49,8 +49,6 @@ class Subscription final : public SubscriptionBase Node & _node_hdl; CanardPortID const _port_id; OnReceiveCb _on_receive_cb; - - TransferMetadata fillMetadata(CanardRxTransfer const & transfer); }; /************************************************************************************** diff --git a/src/Subscription.ipp b/src/Subscription.ipp index 98f5c8b4..8f432544 100644 --- a/src/Subscription.ipp +++ b/src/Subscription.ipp @@ -51,14 +51,6 @@ bool Subscription::onTransferReceived(CanardRxTransfer const & t return true; } -template -TransferMetadata Subscription::fillMetadata(CanardRxTransfer const & transfer) -{ - TransferMetadata transfer_metadata; - transfer_metadata.node_id = static_cast(transfer.metadata.remote_node_id); - - return transfer_metadata; -} /************************************************************************************** * NAMESPACE **************************************************************************************/ diff --git a/src/SubscriptionBase.h b/src/SubscriptionBase.h index 38f807d9..1f9af522 100644 --- a/src/SubscriptionBase.h +++ b/src/SubscriptionBase.h @@ -15,6 +15,7 @@ #include #include "libcanard/canard.h" +#include "util/transfer_metadata.hpp" /************************************************************************************** * NAMESPACE @@ -48,9 +49,17 @@ class SubscriptionBase [[nodiscard]] CanardRxSubscription &canard_rx_subscription() { return _canard_rx_sub; } + protected: [[nodiscard]] CanardTransferKind canard_transfer_kind() const { return _transfer_kind; } + [[nodiscard]] TransferMetadata fillMetadata(CanardRxTransfer const & transfer) + { + TransferMetadata transfer_metadata; + transfer_metadata.remote_node_id = static_cast(transfer.metadata.remote_node_id); + + return transfer_metadata; + } private: CanardTransferKind const _transfer_kind; diff --git a/src/util/transfer_metadata.hpp b/src/util/transfer_metadata.hpp index 7305075f..8d9aca4f 100644 --- a/src/util/transfer_metadata.hpp +++ b/src/util/transfer_metadata.hpp @@ -10,7 +10,7 @@ struct TransferMetadata final { - uint16_t node_id; + uint16_t remote_node_id; // More stuff may appear here in the future! }; From 98346cac812b9e234cc0e78639859dfcfd1cac6c Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Mon, 5 Jun 2023 10:24:03 +0200 Subject: [PATCH 16/16] Delete superfluous include for "transfer_metadata.hpp". --- src/Subscription.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/Subscription.hpp b/src/Subscription.hpp index 3aadbdd9..93f6584f 100644 --- a/src/Subscription.hpp +++ b/src/Subscription.hpp @@ -15,8 +15,6 @@ #include "SubscriptionBase.h" #include "Node.hpp" -#include "util/transfer_metadata.hpp" - /************************************************************************************** * NAMESPACE