From 0cba7152d6dab9554061e413c61e7574fb2bcb53 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 28 Dec 2023 06:26:10 +1100 Subject: [PATCH 1/4] canard:add pragma once to callbacks.h --- canard/callbacks.h | 1 + 1 file changed, 1 insertion(+) diff --git a/canard/callbacks.h b/canard/callbacks.h index a201e95..145af8e 100644 --- a/canard/callbacks.h +++ b/canard/callbacks.h @@ -20,6 +20,7 @@ * SOFTWARE. * */ +#pragma once #include From 2ada17209da08175a43f0c2f226f4781ee9cf086 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 28 Dec 2023 06:27:48 +1100 Subject: [PATCH 2/4] canard: split common code into cpp files --- canard/handler_list.cpp | 69 +++++++++++++++++++++++++++++++++++++++ canard/handler_list.h | 67 +++---------------------------------- canard/interface.h | 4 +++ canard/publisher.cpp | 48 +++++++++++++++++++++++++++ canard/publisher.h | 60 +++++++++++++++------------------- canard/service_server.cpp | 37 +++++++++++++++++++++ canard/service_server.h | 65 +++++++++++++++++------------------- canard/subscriber.h | 11 +++++-- canard/transfer_object.h | 6 ++-- 9 files changed, 230 insertions(+), 137 deletions(-) create mode 100644 canard/handler_list.cpp create mode 100644 canard/publisher.cpp create mode 100644 canard/service_server.cpp diff --git a/canard/handler_list.cpp b/canard/handler_list.cpp new file mode 100644 index 0000000..0c63378 --- /dev/null +++ b/canard/handler_list.cpp @@ -0,0 +1,69 @@ +#include "handler_list.h" + +using namespace Canard; + +HandlerList::HandlerList(CanardTransferType _transfer_type, uint16_t _msgid, uint64_t _signature, uint8_t _index) : +index(_index) { + if (index >= CANARD_NUM_HANDLERS) { + return; + } +#ifdef CANARD_MUTEX_ENABLED + WITH_SEMAPHORE(sem[index]); +#endif + next = head[index]; + head[index] = this; + msgid = _msgid; + signature = _signature; + transfer_type = _transfer_type; +} + +HandlerList::~HandlerList() +{ +#ifdef CANARD_MUTEX_ENABLED + WITH_SEMAPHORE(sem[index]); +#endif + HandlerList* entry = head[index]; + if (entry == this) { + head[index] = next; + return; + } + while (entry != nullptr) { + if (entry->next == this) { + entry->next = next; + return; + } + entry = entry->next; + } +} + +bool HandlerList::accept_message(uint8_t index, uint16_t msgid, uint64_t &signature) +{ +#ifdef CANARD_MUTEX_ENABLED + WITH_SEMAPHORE(sem[index]); +#endif + HandlerList* entry = head[index]; + while (entry != nullptr) { + if (entry->msgid == msgid) { + signature = entry->signature; + return true; + } + entry = entry->next; + } + return false; +} + +void HandlerList::handle_message(uint8_t index, const CanardRxTransfer& transfer) +{ +#ifdef CANARD_MUTEX_ENABLED + WITH_SEMAPHORE(sem[index]); +#endif + HandlerList* entry = head[index]; + while (entry != nullptr) { + if (transfer.data_type_id == entry->msgid && + entry->transfer_type == transfer.transfer_type) { + entry->handle_message(transfer); + return; + } + entry = entry->next; + } +} diff --git a/canard/handler_list.h b/canard/handler_list.h index edf4fd6..f0a2e05 100644 --- a/canard/handler_list.h +++ b/canard/handler_list.h @@ -41,82 +41,25 @@ class HandlerList { /// @param _msgid ID of the message/service /// @param _signature Signature of the message/service /// @param _index Index of the handler list - HandlerList(CanardTransferType _transfer_type, uint16_t _msgid, uint64_t _signature, uint8_t _index) NOINLINE_FUNC : - index(_index) { - if (index >= CANARD_NUM_HANDLERS) { - return; - } -#ifdef WITH_SEMAPHORE - WITH_SEMAPHORE(sem[index]); -#endif - next = head[index]; - head[index] = this; - msgid = _msgid; - signature = _signature; - transfer_type = _transfer_type; - } + HandlerList(CanardTransferType _transfer_type, uint16_t _msgid, uint64_t _signature, uint8_t _index) NOINLINE_FUNC; /// @brief delete copy constructor and assignment operator HandlerList(const HandlerList&) = delete; // destructor, remove the entry from the singly-linked list - virtual ~HandlerList() NOINLINE_FUNC { -#ifdef WITH_SEMAPHORE - WITH_SEMAPHORE(sem[index]); -#endif - HandlerList* entry = head[index]; - if (entry == this) { - head[index] = next; - return; - } - while (entry != nullptr) { - if (entry->next == this) { - entry->next = next; - return; - } - entry = entry->next; - } - } + virtual ~HandlerList() NOINLINE_FUNC; /// @brief accept a message if it is handled by this handler list /// @param index Index of the handler list /// @param msgid ID of the message/service /// @param[out] signature Signature of the message/service /// @return true if the message is handled by this handler list - static bool accept_message(uint8_t index, uint16_t msgid, uint64_t &signature) NOINLINE_FUNC - { -#ifdef WITH_SEMAPHORE - WITH_SEMAPHORE(sem[index]); -#endif - HandlerList* entry = head[index]; - while (entry != nullptr) { - if (entry->msgid == msgid) { - signature = entry->signature; - return true; - } - entry = entry->next; - } - return false; - } + static bool accept_message(uint8_t index, uint16_t msgid, uint64_t &signature) NOINLINE_FUNC; /// @brief handle a message if it is handled by this handler list /// @param index Index of the handler list /// @param transfer transfer object of the request - static void handle_message(uint8_t index, const CanardRxTransfer& transfer) NOINLINE_FUNC - { -#ifdef WITH_SEMAPHORE - WITH_SEMAPHORE(sem[index]); -#endif - HandlerList* entry = head[index]; - while (entry != nullptr) { - if (transfer.data_type_id == entry->msgid && - entry->transfer_type == transfer.transfer_type) { - entry->handle_message(transfer); - return; - } - entry = entry->next; - } - } + static void handle_message(uint8_t index, const CanardRxTransfer& transfer) NOINLINE_FUNC; /// @brief Method to handle a message implemented by the derived class /// @param transfer transfer object of the request @@ -128,7 +71,7 @@ class HandlerList { private: static HandlerList* head[CANARD_NUM_HANDLERS]; -#ifdef WITH_SEMAPHORE +#ifdef CANARD_MUTEX_ENABLED static Canard::Semaphore sem[CANARD_NUM_HANDLERS]; #endif uint16_t msgid; diff --git a/canard/interface.h b/canard/interface.h index d5a66dc..7c99a37 100644 --- a/canard/interface.h +++ b/canard/interface.h @@ -40,8 +40,12 @@ struct Transfer { uint8_t priority; ///< Priority of the transfer const void* payload; ///< Pointer to the payload uint32_t payload_len; ///< Length of the payload +#if CANARD_MULTI_IFACE uint8_t iface_mask; ///< Bitmask of interfaces to send the transfer on +#endif +#if CANARD_ENABLE_CANFD bool canfd; ///< true if the transfer is CAN FD +#endif uint32_t timeout_ms; ///< timeout in ms }; diff --git a/canard/publisher.cpp b/canard/publisher.cpp new file mode 100644 index 0000000..38309b6 --- /dev/null +++ b/canard/publisher.cpp @@ -0,0 +1,48 @@ +#include "publisher.h" + +using namespace Canard; + +bool PublisherBase::send(uint16_t data_type_id, + uint64_t data_type_signature, + uint8_t* msg_buf, + uint32_t len +#if CANARD_ENABLE_CANFD + , bool canfd +#endif + ) { + if (len == 0) { + return false; + } + Transfer msg_transfer {}; + msg_transfer.transfer_type = CanardTransferTypeBroadcast; + msg_transfer.data_type_id = data_type_id; + msg_transfer.data_type_signature = data_type_signature; + msg_transfer.payload = msg_buf; + msg_transfer.payload_len = len; +#if CANARD_ENABLE_CANFD + msg_transfer.canfd = canfd; +#endif +#if CANARD_MULTI_IFACE + msg_transfer.iface_mask = CANARD_IFACE_ALL; +#endif + return Sender::send(msg_transfer); +} + +bool Sender::send(Transfer& transfer, uint8_t destination_node_id) { + switch (transfer.transfer_type) + { + case CanardTransferTypeBroadcast: + transfer.inout_transfer_id = TransferObject::get_tid_ptr(interface.get_index(),transfer.data_type_id, CanardTransferTypeBroadcast, interface.get_node_id(), destination_node_id); + transfer.priority = priority; + transfer.timeout_ms = timeout; + return interface.broadcast(transfer); + case CanardTransferTypeRequest: + transfer.inout_transfer_id = TransferObject::get_tid_ptr(interface.get_index(),transfer.data_type_id, CanardTransferTypeRequest, interface.get_node_id(), destination_node_id); + transfer.priority = priority; + transfer.timeout_ms = timeout; + return interface.request(destination_node_id, transfer); + case CanardTransferTypeResponse: + default: + return false; + } +} \ No newline at end of file diff --git a/canard/publisher.h b/canard/publisher.h index 7d22e5d..8b4f657 100644 --- a/canard/publisher.h +++ b/canard/publisher.h @@ -56,34 +56,34 @@ class Sender { /// @brief Send a message /// @param Transfer message to send /// @return true if the message was put into the queue successfully - bool send(Transfer& transfer, uint8_t destination_node_id = CANARD_BROADCAST_NODE_ID) NOINLINE_FUNC { - switch (transfer.transfer_type) - { - case CanardTransferTypeBroadcast: - transfer.inout_transfer_id = TransferObject::get_tid_ptr(interface.get_index(),transfer.data_type_id, CanardTransferTypeBroadcast, interface.get_node_id(), destination_node_id); - transfer.priority = priority; - transfer.timeout_ms = timeout; - return interface.broadcast(transfer); - case CanardTransferTypeRequest: - transfer.inout_transfer_id = TransferObject::get_tid_ptr(interface.get_index(),transfer.data_type_id, CanardTransferTypeRequest, interface.get_node_id(), destination_node_id); - transfer.priority = priority; - transfer.timeout_ms = timeout; - return interface.request(destination_node_id, transfer); - case CanardTransferTypeResponse: - default: - return false; - } - } + bool send(Transfer& transfer, uint8_t destination_node_id = CANARD_BROADCAST_NODE_ID) NOINLINE_FUNC; private: uint8_t priority = CANARD_TRANSFER_PRIORITY_MEDIUM; ///< Priority of the message uint32_t timeout = 1000; ///< Timeout of the message in ms }; +class PublisherBase : public Sender { +public: + PublisherBase(Interface &_interface) : + Sender(_interface) + {} + +protected: + bool send(uint16_t data_type_id, + uint64_t data_type_signature, + uint8_t* msg_buf, + uint32_t len +#if CANARD_ENABLE_CANFD + , bool canfd +#endif + ) NOINLINE_FUNC; +}; + template -class Publisher : public Sender { +class Publisher : public PublisherBase { public: Publisher(Interface &_interface) : - Sender(_interface) + PublisherBase(_interface) {} // delete copy constructor and assignment operator @@ -115,22 +115,14 @@ class Publisher : public Sender { #endif ); // send the message if encoded successfully - if (len > 0) { - Transfer msg_transfer {}; - msg_transfer.transfer_type = CanardTransferTypeBroadcast; - msg_transfer.data_type_id = msgtype::cxx_iface::ID; - msg_transfer.data_type_signature = msgtype::cxx_iface::SIGNATURE; - msg_transfer.payload = msg_buf; - msg_transfer.payload_len = len; + return PublisherBase::send(msgtype::cxx_iface::ID, + msgtype::cxx_iface::SIGNATURE, + (uint8_t*)msg_buf, + len #if CANARD_ENABLE_CANFD - msg_transfer.canfd = canfd; -#endif -#if CANARD_MULTI_IFACE - msg_transfer.iface_mask = CANARD_IFACE_ALL; + ,canfd #endif - return send(msg_transfer); - } - return false; + ); } private: uint8_t msg_buf[msgtype::cxx_iface::MAX_SIZE]; ///< Buffer to store the encoded message diff --git a/canard/service_server.cpp b/canard/service_server.cpp new file mode 100644 index 0000000..3e10b4f --- /dev/null +++ b/canard/service_server.cpp @@ -0,0 +1,37 @@ +#include "service_server.h" + +using namespace Canard; + +ServerBase::ServerBase(Interface &_interface, uint16_t _msgid, uint64_t _signature) : + HandlerList(CanardTransferTypeRequest, _msgid, _signature, _interface.get_index()), + interface(_interface) +{} + +bool ServerBase::respond(const CanardRxTransfer& transfer, + uint16_t data_type_id, + uint64_t data_type_signature, + uint8_t* rsp_buf, + uint32_t len) +{ + // send the message if encoded successfully + if (len == 0) { + return false; + } + Transfer rsp_transfer = { + .transfer_type = CanardTransferTypeResponse, + .data_type_signature = data_type_signature, + .data_type_id = data_type_id, + .inout_transfer_id = &transfer_id, + .priority = transfer.priority, + .payload = rsp_buf, + .payload_len = len, +#if CANARD_MULTI_IFACE + .iface_mask = iface_mask, +#endif +#if CANARD_ENABLE_CANFD + .canfd = transfer.canfd, +#endif + .timeout_ms = timeout, + }; + return interface.respond(transfer.source_node_id, rsp_transfer); +} diff --git a/canard/service_server.h b/canard/service_server.h index 32c905b..6c72973 100644 --- a/canard/service_server.h +++ b/canard/service_server.h @@ -24,13 +24,37 @@ #pragma once #include "handler_list.h" #include "interface.h" +#include "callbacks.h" namespace Canard { +class ServerBase : public HandlerList { +public: + ServerBase(Interface &_interface, uint16_t _msgid, uint64_t _signature); + bool respond(const CanardRxTransfer& transfer, + uint16_t data_type_id, + uint64_t data_type_signature, + uint8_t* rsp_buf, + uint32_t len); + + /// @brief Set the timeout for the response + /// @param _timeout timeout in milliseconds + void set_timeout_ms(uint32_t _timeout) { + timeout = _timeout; + } +protected: + uint8_t transfer_id = 0; + uint32_t timeout = 1000; + Interface &interface; +#if CANARD_MULTI_IFACE + uint8_t iface_mask = CANARD_IFACE_ALL; +#endif +}; + /// @brief Server class to handle service requests /// @tparam reqtype template -class Server : public HandlerList { +class Server : public ServerBase { public: /// @brief Server constructor @@ -38,8 +62,7 @@ class Server : public HandlerList { /// @param _cb Callback object /// @param _index HandlerList instance id Server(Interface &_interface, Callback &_cb) : - HandlerList(CanardTransferTypeRequest, reqtype::cxx_iface::ID, reqtype::cxx_iface::SIGNATURE, _interface.get_index()), - interface(_interface), + ServerBase(_interface, reqtype::cxx_iface::ID, reqtype::cxx_iface::SIGNATURE), cb(_cb) { // multiple servers are not allowed, so no list } @@ -74,43 +97,15 @@ class Server : public HandlerList { #endif ); // send the message if encoded successfully - if (len > 0) { - Transfer rsp_transfer; -#if CANARD_ENABLE_CANFD - rsp_transfer.canfd = transfer.canfd; -#endif -#if CANARD_MULTI_IFACE - rsp_transfer.iface_mask = iface_mask; -#endif - rsp_transfer.transfer_type = CanardTransferTypeResponse; - rsp_transfer.inout_transfer_id = &transfer_id; - rsp_transfer.data_type_id = reqtype::cxx_iface::ID; - rsp_transfer.data_type_signature = reqtype::cxx_iface::SIGNATURE; - rsp_transfer.payload = rsp_buf; - rsp_transfer.payload_len = len; - rsp_transfer.priority = transfer.priority; - rsp_transfer.timeout_ms = timeout; - return interface.respond(transfer.source_node_id, rsp_transfer); - } - return false; - } - - /// @brief Set the timeout for the response - /// @param _timeout timeout in milliseconds - void set_timeout_ms(uint32_t _timeout) { - timeout = _timeout; + return ServerBase::respond(transfer, reqtype::cxx_iface::ID, + reqtype::cxx_iface::SIGNATURE, + (uint8_t*)rsp_buf, + len); } private: uint8_t rsp_buf[reqtype::cxx_iface::RSP_MAX_SIZE]; - Interface &interface; Callback &cb; - - uint32_t timeout = 1000; - uint8_t transfer_id = 0; -#if CANARD_MULTI_IFACE - uint8_t iface_mask = CANARD_IFACE_ALL; -#endif }; } // namespace Canard diff --git a/canard/subscriber.h b/canard/subscriber.h index 9f94721..760c80f 100644 --- a/canard/subscriber.h +++ b/canard/subscriber.h @@ -41,7 +41,7 @@ class Subscriber : public HandlerList { Subscriber(Callback &_cb, uint8_t _index) : HandlerList(CanardTransferTypeBroadcast, msgtype::cxx_iface::ID, msgtype::cxx_iface::SIGNATURE, _index), cb (_cb) { -#ifdef WITH_SEMAPHORE +#ifdef CANARD_MUTEX_ENABLED WITH_SEMAPHORE(sem[index]); #endif next = branch_head[index]; @@ -86,8 +86,8 @@ class Subscriber : public HandlerList { private: Subscriber* next; static Subscriber *branch_head[CANARD_NUM_HANDLERS]; -#ifdef WITH_SEMAPHORE - Canard::Semaphore sem[CANARD_NUM_HANDLERS]; +#ifdef CANARD_MUTEX_ENABLED + static Canard::Semaphore sem[CANARD_NUM_HANDLERS]; #endif Callback &cb; }; @@ -95,6 +95,11 @@ class Subscriber : public HandlerList { template Subscriber* Subscriber::branch_head[] = {nullptr}; +#ifdef CANARD_MUTEX_ENABLED +template +Canard::Semaphore Subscriber::sem[CANARD_NUM_HANDLERS]; +#endif + template class SubscriberArgCb { public: diff --git a/canard/transfer_object.h b/canard/transfer_object.h index 7a9c250..f42e96b 100644 --- a/canard/transfer_object.h +++ b/canard/transfer_object.h @@ -41,7 +41,7 @@ class TransferObject { if (index >= CANARD_NUM_HANDLERS) { return nullptr; } -#ifdef WITH_SEMAPHORE +#ifdef CANARD_MUTEX_ENABLED WITH_SEMAPHORE(sem[index]); #endif uint32_t _transfer_desc = MAKE_TRANSFER_DESCRIPTOR(data_type_id, transfer_type, src_node_id, dst_node_id); @@ -77,7 +77,7 @@ class TransferObject { if (index >= CANARD_NUM_HANDLERS) { return; } -#ifdef WITH_SEMAPHORE +#ifdef CANARD_MUTEX_ENABLED WITH_SEMAPHORE(sem[index]); #endif TransferObject *tid_map_ptr = tid_map_head[index]; @@ -90,7 +90,7 @@ class TransferObject { } private: static TransferObject *tid_map_head[CANARD_NUM_HANDLERS]; -#ifdef WITH_SEMAPHORE +#ifdef CANARD_MUTEX_ENABLED static Canard::Semaphore sem[CANARD_NUM_HANDLERS]; #endif TransferObject *next; From 0fd7abe12cfb5e8540dd38dcbdc151f4d6802c45 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 28 Dec 2023 06:46:56 +1100 Subject: [PATCH 3/4] canard: fix tests to include canard cpp source files --- canard/tests/CMakeLists.txt | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/canard/tests/CMakeLists.txt b/canard/tests/CMakeLists.txt index d401a6e..e8ea22e 100644 --- a/canard/tests/CMakeLists.txt +++ b/canard/tests/CMakeLists.txt @@ -41,12 +41,19 @@ include_directories(${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated/include) # glob all generated dsdlc files file(GLOB DSDL_GENERATED_SOURCES ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated/src/*.c*) +# glob all canard/*cpp files +file(GLOB CANARD_CPP_SOURCES ${CMAKE_CURRENT_LIST_DIR}/../*.cpp) + +message(STATUS "CANARD_CPP_SOURCES: ${CANARD_CPP_SOURCES}") + # append DSDL_GENERATED_SOURCES_C to SRC_FILES list(APPEND SRC_FILES ${DSDL_GENERATED_SOURCES}) -set(SRC_FILES_TEST cxx_test_interface.cpp +set(SRC_FILES_TEST ${CANARD_CPP_SOURCES} + cxx_test_interface.cpp test_cxx_wrappers.cpp) -set(SRC_FILES_CANARD canard_interface.cpp +set(SRC_FILES_CANARD ${CANARD_CPP_SOURCES} + canard_interface.cpp test_canard_interface.cpp) # create test target From 9d806c66973506cbda34f6d06296bca5348720cb Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Thu, 28 Dec 2023 08:31:27 +1100 Subject: [PATCH 4/4] canard: use common semaphore from handler_list inside subscriber --- canard/handler_list.h | 4 +++- canard/subscriber.h | 16 +++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/canard/handler_list.h b/canard/handler_list.h index f0a2e05..f322063 100644 --- a/canard/handler_list.h +++ b/canard/handler_list.h @@ -68,7 +68,9 @@ class HandlerList { protected: uint8_t index; HandlerList* next; - +#ifdef CANARD_MUTEX_ENABLED + Canard::Semaphore& get_sem() { return sem[index]; } +#endif private: static HandlerList* head[CANARD_NUM_HANDLERS]; #ifdef CANARD_MUTEX_ENABLED diff --git a/canard/subscriber.h b/canard/subscriber.h index 760c80f..d051039 100644 --- a/canard/subscriber.h +++ b/canard/subscriber.h @@ -42,7 +42,7 @@ class Subscriber : public HandlerList { HandlerList(CanardTransferTypeBroadcast, msgtype::cxx_iface::ID, msgtype::cxx_iface::SIGNATURE, _index), cb (_cb) { #ifdef CANARD_MUTEX_ENABLED - WITH_SEMAPHORE(sem[index]); + WITH_SEMAPHORE(get_sem()); #endif next = branch_head[index]; branch_head[index] = this; @@ -53,6 +53,9 @@ class Subscriber : public HandlerList { // destructor, remove the entry from the singly-linked list ~Subscriber() { +#ifdef CANARD_MUTEX_ENABLED + WITH_SEMAPHORE(get_sem()); +#endif Subscriber* entry = branch_head[index]; if (entry == this) { branch_head[index] = next; @@ -70,6 +73,9 @@ class Subscriber : public HandlerList { /// @brief parse the message and call the callback /// @param transfer transfer object void handle_message(const CanardRxTransfer& transfer) override { +#ifdef CANARD_MUTEX_ENABLED + WITH_SEMAPHORE(get_sem()); +#endif msgtype msg {}; if (msgtype::cxx_iface::decode(&transfer, &msg)) { // invalid decode @@ -86,20 +92,12 @@ class Subscriber : public HandlerList { private: Subscriber* next; static Subscriber *branch_head[CANARD_NUM_HANDLERS]; -#ifdef CANARD_MUTEX_ENABLED - static Canard::Semaphore sem[CANARD_NUM_HANDLERS]; -#endif Callback &cb; }; template Subscriber* Subscriber::branch_head[] = {nullptr}; -#ifdef CANARD_MUTEX_ENABLED -template -Canard::Semaphore Subscriber::sem[CANARD_NUM_HANDLERS]; -#endif - template class SubscriberArgCb { public: