diff --git a/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino b/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino index 4a0a5bb4..8d7ac035 100644 --- a/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino +++ b/examples/OpenCyphal-Blink/OpenCyphal-Blink.ino @@ -64,7 +64,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, CyphalHeap node_heap; Node node_hdl(node_heap.data(), node_heap.size()); -Publisher> heartbeat_pub = node_hdl.create_publisher>(Heartbeat_1_0<>::PORT_ID, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); +Publisher> heartbeat_pub = node_hdl.create_publisher>(Heartbeat_1_0<>::PORT_ID, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, micros); Heartbeat_1_0<> hb_msg; @@ -120,7 +120,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }); + node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, micros); /* Update the heartbeat object */ hb_msg.data.uptime = millis() / 1000; diff --git a/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino b/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino index b54b7a6f..e1539ec5 100644 --- a/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino +++ b/examples/OpenCyphal-GNSS-Node/OpenCyphal-GNSS-Node.ino @@ -147,7 +147,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }); + node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, micros); /* Handle actions common to all states. */ diff --git a/examples/OpenCyphal-Heartbeat-Publisher/OpenCyphal-Heartbeat-Publisher.ino b/examples/OpenCyphal-Heartbeat-Publisher/OpenCyphal-Heartbeat-Publisher.ino index c8493bd6..0511f4c5 100644 --- a/examples/OpenCyphal-Heartbeat-Publisher/OpenCyphal-Heartbeat-Publisher.ino +++ b/examples/OpenCyphal-Heartbeat-Publisher/OpenCyphal-Heartbeat-Publisher.ino @@ -41,7 +41,7 @@ ArduinoMCP2515 mcp2515([]() { digitalWrite(MKRCAN_MCP2515_CS_PIN, LOW); }, CyphalHeap node_heap; Node node_hdl(node_heap.data(), node_heap.size()); -Publisher> heartbeat_pub = node_hdl.create_publisher>(Heartbeat_1_0<>::PORT_ID, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); +Publisher> heartbeat_pub = node_hdl.create_publisher>(Heartbeat_1_0<>::PORT_ID, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, micros); Heartbeat_1_0<> hb_msg; @@ -79,7 +79,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }); + node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, micros); /* Update the heartbeat object */ hb_msg.data.uptime = millis() / 1000; diff --git a/examples/OpenCyphal-Heartbeat-Subscribe/OpenCyphal-Heartbeat-Subscribe.ino b/examples/OpenCyphal-Heartbeat-Subscribe/OpenCyphal-Heartbeat-Subscribe.ino index e0832eea..95593b54 100644 --- a/examples/OpenCyphal-Heartbeat-Subscribe/OpenCyphal-Heartbeat-Subscribe.ino +++ b/examples/OpenCyphal-Heartbeat-Subscribe/OpenCyphal-Heartbeat-Subscribe.ino @@ -80,7 +80,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }); + node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, micros); } /************************************************************************************** diff --git a/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino b/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino index be527dce..7272f68d 100644 --- a/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino +++ b/examples/OpenCyphal-Service-Client/OpenCyphal-Service-Client.ino @@ -89,7 +89,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }); + node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, micros); } /************************************************************************************** diff --git a/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino b/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino index 1e61dc59..75830167 100644 --- a/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino +++ b/examples/OpenCyphal-Service-Server/OpenCyphal-Service-Server.ino @@ -81,7 +81,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }); + node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, micros); } /************************************************************************************** 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 e7325ea5..0c371ff0 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 @@ -122,8 +122,8 @@ ArduinoMCP2515 mcp2515([]() CyphalHeap node_heap; Node node_hdl(node_heap.data(), node_heap.size(), OPEN_CYPHAL_NODE_ID); -Publisher> heartbeat_pub = node_hdl.create_publisher>(Heartbeat_1_0<>::PORT_ID, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); -Publisher tof_pub = node_hdl.create_publisher(OPEN_CYPHAL_ID_DISTANCE_DATA, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC); +Publisher> heartbeat_pub = node_hdl.create_publisher>(Heartbeat_1_0<>::PORT_ID, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, micros); +Publisher tof_pub = node_hdl.create_publisher(OPEN_CYPHAL_ID_DISTANCE_DATA, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, micros); OpenCyphalNodeData node_data = OPEN_CYPHAL_NODE_INITIAL_DATA; OpenCyphalNodeConfiguration node_config = OPEN_CYPHAL_NODE_INITIAL_CONFIGURATION; @@ -243,7 +243,7 @@ void loop() { /* Process all pending OpenCyphal actions. */ - node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }); + node_hdl.spinSome([] (CanardFrame const & frame) { return mcp2515.transmit(frame); }, micros); /* Handle actions common to all states. */ diff --git a/src/Node.cpp b/src/Node.cpp index 55f8efcf..09f904ab 100644 --- a/src/Node.cpp +++ b/src/Node.cpp @@ -11,12 +11,6 @@ #include "Node.hpp" -/************************************************************************************** - * FORWARD DECLARATION - **************************************************************************************/ - -extern "C" unsigned long micros(); - /************************************************************************************** * CTOR/DTOR **************************************************************************************/ @@ -40,10 +34,10 @@ Node::Node(uint8_t * heap_ptr, * PUBLIC MEMBER FUNCTIONS **************************************************************************************/ -void Node::spinSome(CanFrameTransmitFunc const tx_func) +void Node::spinSome(CanFrameTransmitFunc const tx_func, TransferTimestampFunc const micros_func) { processRxQueue(); - processTxQueue(tx_func); + processTxQueue(tx_func, micros_func); } void Node::onCanFrameReceived(CanardFrame const & frame, CanardMicrosecond const & rx_timestamp_us) @@ -115,14 +109,14 @@ void Node::processRxQueue() } } -void Node::processTxQueue(CanFrameTransmitFunc const tx_func) +void Node::processTxQueue(CanFrameTransmitFunc const tx_func, TransferTimestampFunc const micros_func) { for(CanardTxQueueItem * tx_queue_item = const_cast(canardTxPeek(&_canard_tx_queue)); tx_queue_item != nullptr; tx_queue_item = const_cast(canardTxPeek(&_canard_tx_queue))) { /* Discard the frame if the transmit deadline has expired. */ - if (tx_queue_item->tx_deadline_usec > micros()) { + if (tx_queue_item->tx_deadline_usec > micros_func()) { _canard_hdl.memory_free(&_canard_hdl, canardTxPop(&_canard_tx_queue, tx_queue_item)); continue; } diff --git a/src/Node.hpp b/src/Node.hpp index e1f55e32..296ed70b 100644 --- a/src/Node.hpp +++ b/src/Node.hpp @@ -40,6 +40,7 @@ class Node; typedef std::function OnTransferReceivedFunc; typedef std::function CanFrameTransmitFunc; +typedef std::function TransferTimestampFunc; template struct alignas(O1HEAP_ALIGNMENT) CyphalHeap final : public std::array {}; @@ -76,12 +77,15 @@ class Node inline void setNodeId(CanardNodeID const node_id) { _canard_hdl.node_id = node_id; } inline CanardNodeID getNodeId() const { return _canard_hdl.node_id; } - template Publisher create_publisher(CanardPortID const port_id, CanardMicrosecond const tx_timeout_usec); + template + Publisher create_publisher(CanardPortID const port_id, + CanardMicrosecond const tx_timeout_usec, + std::function const micros_func); /* Must be called from the application to process * all received CAN frames. */ - void spinSome(CanFrameTransmitFunc const tx_func); + void spinSome(CanFrameTransmitFunc const tx_func, TransferTimestampFunc const micros_func); /* Must be called from the application upon the * reception of a can frame. */ @@ -115,7 +119,7 @@ class Node static void o1heap_free (CanardInstance * const ins, void * const pointer); void processRxQueue(); - void processTxQueue(CanFrameTransmitFunc const tx_func); + void processTxQueue(CanFrameTransmitFunc const tx_func, TransferTimestampFunc const micros_func); CanardTransferID getNextTransferId(CanardPortID const port_id); bool subscribe (CanardTransferKind const transfer_kind, CanardPortID const port_id, size_t const payload_size_max, OnTransferReceivedFunc func); diff --git a/src/Node.ipp b/src/Node.ipp index c06bd241..87bdca22 100644 --- a/src/Node.ipp +++ b/src/Node.ipp @@ -10,9 +10,11 @@ **************************************************************************************/ template -Publisher Node::create_publisher(CanardPortID const port_id, CanardMicrosecond const tx_timeout_usec) +Publisher Node::create_publisher(CanardPortID const port_id, + CanardMicrosecond const tx_timeout_usec, + std::function const micros_func) { - return std::make_shared>(_canard_hdl, _canard_tx_queue, port_id, tx_timeout_usec); + return std::make_shared>(_canard_hdl, _canard_tx_queue, port_id, tx_timeout_usec, micros_func); } template diff --git a/src/Publisher.hpp b/src/Publisher.hpp index 85295c7f..15e05224 100644 --- a/src/Publisher.hpp +++ b/src/Publisher.hpp @@ -29,11 +29,12 @@ template class Publisher { public: - Publisher(CanardInstance & canard_hdl, CanardTxQueue & canard_tx_queue, CanardPortID const port_id, CanardMicrosecond const tx_timeout_usec) + Publisher(CanardInstance & canard_hdl, CanardTxQueue & canard_tx_queue, CanardPortID const port_id, CanardMicrosecond const tx_timeout_usec, std::function const micros_func) : _canard_hdl{canard_hdl} , _canard_tx_queue{canard_tx_queue} , _port_id{port_id} , _tx_timeout_usec{tx_timeout_usec} + , _micros_func{micros_func} , _transfer_id{0} { } @@ -44,6 +45,7 @@ class Publisher CanardTxQueue & _canard_tx_queue; CanardPortID const _port_id; CanardMicrosecond const _tx_timeout_usec; + std::function const _micros_func; CanardTransferID _transfer_id; }; diff --git a/src/Publisher.ipp b/src/Publisher.ipp index 36a08430..b2ec799a 100644 --- a/src/Publisher.ipp +++ b/src/Publisher.ipp @@ -18,12 +18,6 @@ namespace impl { -/************************************************************************************** - * FORWARD DECLARATION - **************************************************************************************/ - -extern "C" unsigned long micros(); - /************************************************************************************** * PUBLIC MEMBER FUNCTIONS **************************************************************************************/ @@ -47,7 +41,7 @@ bool Publisher::publish(T const & msg) /* Serialize transfer into a series of CAN frames */ int32_t result = canardTxPush(&_canard_tx_queue, &_canard_hdl, - micros() + _tx_timeout_usec, + _micros_func() + _tx_timeout_usec, &transfer_metadata, payload_buf_size, payload_buf.data());