From a6091ad7bad5ed74cca1f3970059079136159914 Mon Sep 17 00:00:00 2001 From: Duarte Dias Date: Sun, 11 Jun 2023 13:09:12 +0200 Subject: [PATCH 01/12] First step on adding KDECAN protocol (or other independent protocols) together with UAVCAN protocol. Each iface is defined a protocol, allowing the sending of messages just for that specific protocol (this selection is done by the dispatcher). --- libuavcan/include/independent/kdecan.hpp | 180 ++++++++++++++++++ .../include/uavcan/transport/dispatcher.hpp | 31 ++- libuavcan/include/uavcan/transport/frame.hpp | 11 +- .../include/uavcan/transport/transfer.hpp | 6 + .../include/uavcan/util/method_binder.hpp | 7 + libuavcan/src/transport/uc_dispatcher.cpp | 105 +++++++--- 6 files changed, 314 insertions(+), 26 deletions(-) create mode 100644 libuavcan/include/independent/kdecan.hpp diff --git a/libuavcan/include/independent/kdecan.hpp b/libuavcan/include/independent/kdecan.hpp new file mode 100644 index 00000000..c7f183da --- /dev/null +++ b/libuavcan/include/independent/kdecan.hpp @@ -0,0 +1,180 @@ +#ifndef UAVCAN_INDEPENDENT_KDECAN_HPP_INCLUDED +#define UAVCAN_INDEPENDENT_KDECAN_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace kdecan +{ + +static const int escNodeIdBroadcast = 1; +static const int escNodeIdOffset = 2; + +enum kdeCanObjAddr +{ + ESCInformation = 0, + PWMThrottle = 1, + ESCInputThrottle = 6, + ESCOutputThrottle = 7, + ESCStatus = 11, + GetMCUId = 8, + UpdateNodeAddress = 9, + StartESCEnumeration = 10, + Shutdown = 32, + Restart = 33 +}; + +class KdeFrame +{ +public: + enum { PayloadCapacity = 8 }; + + uint8_t source_address_; + uint8_t destination_address_; + kdeCanObjAddr object_address_; + uint8_t data_[PayloadCapacity]; + + KdeFrame(const uint8_t source_address, + const uint8_t destination_address, + kdeCanObjAddr object_address, + const uint8_t* data) : + source_address_(source_address), + destination_address_(destination_address), + object_address_(object_address) + { + memcpy(data_, data, PayloadCapacity); + } + + bool compile(uavcan::CanFrame& out_can_frame) + { + out_can_frame.id = ((uint32_t)0x00000000) | + ((uint32_t)object_address_) | + (((uint32_t)destination_address_) << 8) | + (((uint32_t)source_address_) << 16) | + uavcan::CanFrame::FlagEFF; + + switch(object_address_) + { + case ESCInformation: { out_can_frame.dlc = 0; } break; + case PWMThrottle: { out_can_frame.dlc = 2; } break; + case ESCInputThrottle: { out_can_frame.dlc = 0; } break; + case ESCOutputThrottle: { out_can_frame.dlc = 0; } break; + case ESCStatus: { out_can_frame.dlc = 0; } break; + case GetMCUId: { out_can_frame.dlc = 0; } break; + case UpdateNodeAddress: { out_can_frame.dlc = 8; } break; + case StartESCEnumeration: { out_can_frame.dlc = 2; } break; + case Shutdown: { out_can_frame.dlc = 0; } break; + case Restart: { out_can_frame.dlc = 0; } break; + + default: { return false; } + } + + // this function will take care of little/big endian conversions + (void)uavcan::copy(data_, data_ + out_can_frame.dlc, out_can_frame.data); + + return true; + } +}; + +class KdeCanTransferListener : public uavcan::LinkedListNode +{ +public: + kdeCanObjAddr target_object_address_; + + KdeCanTransferListener(const kdeCanObjAddr target_object_address) : + target_object_address_(target_object_address) + {;} + + virtual void handleIncomingTransfer(const uint32_t id, const uint8_t dlc, const uint8_t* data) = 0; + + void handleFrame(const uavcan::CanRxFrame& can_frame) + { + uint32_t object_address = can_frame.id & 0x000000FF; + + if (object_address == target_object_address_) + { + handleIncomingTransfer(can_frame.id, can_frame.dlc, can_frame.data); + } + } +}; + +template = UAVCAN_CPP11 + typename Callback_ = std::function +#else + typename Callback_ = void (*)(const uint32_t id, const uint8_t dlc, const uint8_t* data) +#endif +> +class KdeCanSubscriber +{ +public: + typedef KdeCanSubscriber SelfType; + typedef Callback_ Callback; + + class kdeTransferForwarder : public KdeCanTransferListener + { + public: + SelfType& obj_; + + void handleIncomingTransfer(const uint32_t id, const uint8_t dlc, const uint8_t* data) override + { + obj_.handleIncomingTransfer(id, dlc, data); + } + + kdeTransferForwarder(SelfType& obj, const kdeCanObjAddr target_object_address) : + KdeCanTransferListener(target_object_address), + obj_(obj) + {;} + }; + + Callback callback_; + kdeTransferForwarder forwarder_; + + KdeCanSubscriber() : + callback_(), + forwarder_(*this, DataType_) + {;} + + void handleIncomingTransfer(const uint32_t id, const uint8_t dlc, const uint8_t* data) + { + if (callback_) + { + callback_(id, dlc, data); + } + } + + void setCallback(Callback callback) + { + callback_ = callback; + } + + KdeCanTransferListener* getKdeListener() + { + return &(this->forwarder_); + } +}; + +class KdeCanTransferSender +{ +public: + KdeCanTransferSender() {;} + + bool publish(uavcan::CanFrame& can_frame, + const uint8_t source_address, + const uint8_t destination_address, + const kdeCanObjAddr object_address, const uint8_t* data) + { + KdeFrame kde_frame(source_address, destination_address, object_address, data); + + return kde_frame.compile(can_frame); + } +}; + +}; + +#endif // UAVCAN_INDEPENDENT_KDECAN_HPP_INCLUDED diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index 50d06d78..1bd5c247 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -14,6 +14,7 @@ #include #include #include +#include namespace uavcan { @@ -45,7 +46,6 @@ class UAVCAN_EXPORT LoopbackFrameListenerBase : public LinkedListNode listeners_; @@ -86,6 +86,8 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable bool tao_disabled_ = false; bool canfd_ = false; + TransferProtocol iface_protocol_[UAVCAN_STM32H7_NUM_IFACES]; + class ListenerRegistry { LinkedListRoot list_; @@ -120,6 +122,8 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable ListenerRegistry lsrv_req_; ListenerRegistry lsrv_resp_; + LinkedListRoot kdeCanListener_list_; + #if !UAVCAN_TINY LoopbackFrameListenerRegistry loopback_listeners_; IRxFrameListener* rx_listener_; @@ -144,7 +148,12 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable #endif , self_node_id_(NodeID::Broadcast) // Default , self_node_id_is_set_(false) - { } + { + for (unsigned i = 0; i < driver.getNumIfaces(); i++) + { + iface_protocol_[i] = UAVCANProtocol; + } + } /** * This version returns strictly when the deadline is reached. @@ -159,6 +168,8 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable /** * Refer to CanIOManager::send() for the parameter description */ + int sendRaw(const CanFrame& can_frame, TransferProtocol CAN_protocol, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos, + CanIOFlags flags, uint8_t iface_mask); int send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask); @@ -167,10 +178,12 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable bool registerMessageListener(TransferListener* listener); bool registerServiceRequestListener(TransferListener* listener); bool registerServiceResponseListener(TransferListener* listener); + bool registerKdeCanListener(kdecan::KdeCanTransferListener* listener); void unregisterMessageListener(TransferListener* listener); void unregisterServiceRequestListener(TransferListener* listener); void unregisterServiceResponseListener(TransferListener* listener); + void unregisterKdeCanListener(kdecan::KdeCanTransferListener* listener); bool hasSubscriber(DataTypeID dtid) const; bool hasPublisher(DataTypeID dtid) const; @@ -183,6 +196,20 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable unsigned getNumServiceRequestListeners() const { return lsrv_req_.getNumEntries(); } unsigned getNumServiceResponseListeners() const { return lsrv_resp_.getNumEntries(); } + bool changeIfaceProtocol(unsigned ifaceId, TransferProtocol protocol) + { + if (ifaceId < canio_.getNumIfaces()) + { + iface_protocol_[ifaceId] = protocol; + + return true; + } + else + { + return false; + } + } + /** * These methods can be used to retreive lists of messages, service requests and service responses the * dispatcher is currently listening to. diff --git a/libuavcan/include/uavcan/transport/frame.hpp b/libuavcan/include/uavcan/transport/frame.hpp index 47f12e0c..26f5f917 100644 --- a/libuavcan/include/uavcan/transport/frame.hpp +++ b/libuavcan/include/uavcan/transport/frame.hpp @@ -35,6 +35,8 @@ class UAVCAN_EXPORT Frame bool toggle_; bool canfd_frame_; + TransferProtocol CAN_protocol_; + public: Frame() : transfer_type_(TransferType(NumTransferTypes)), // Invalid value @@ -42,7 +44,8 @@ class UAVCAN_EXPORT Frame start_of_transfer_(false), end_of_transfer_(false), toggle_(false), - canfd_frame_(false) + canfd_frame_(false), + CAN_protocol_(UAVCANProtocol) { } Frame(DataTypeID data_type_id, @@ -61,7 +64,8 @@ class UAVCAN_EXPORT Frame start_of_transfer_(false), end_of_transfer_(false), toggle_(false), - canfd_frame_(canfd_frame) + canfd_frame_(canfd_frame), + CAN_protocol_(UAVCANProtocol) { UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast()); UAVCAN_ASSERT(data_type_id.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type))); @@ -94,6 +98,9 @@ class UAVCAN_EXPORT Frame NodeID getDstNodeID() const { return dst_node_id_; } TransferID getTransferID() const { return transfer_id_; } + void setCANProtocol(TransferProtocol CAN_protocol) { CAN_protocol_ = CAN_protocol; } + TransferProtocol getCANProtocol() const { return CAN_protocol_; } + void setStartOfTransfer(bool x) { start_of_transfer_ = x; } void setEndOfTransfer(bool x) { end_of_transfer_ = x; } diff --git a/libuavcan/include/uavcan/transport/transfer.hpp b/libuavcan/include/uavcan/transport/transfer.hpp index 7e5d5b88..f98158ec 100644 --- a/libuavcan/include/uavcan/transport/transfer.hpp +++ b/libuavcan/include/uavcan/transport/transfer.hpp @@ -22,6 +22,12 @@ enum TransferType TransferTypeMessageBroadcast = 2 }; +enum TransferProtocol +{ + UAVCANProtocol = 0, + KDECANProtocol = 1 +}; + static const uint8_t NumTransferTypes = 3; diff --git a/libuavcan/include/uavcan/util/method_binder.hpp b/libuavcan/include/uavcan/util/method_binder.hpp index f3bbf424..7da8c9fe 100644 --- a/libuavcan/include/uavcan/util/method_binder.hpp +++ b/libuavcan/include/uavcan/util/method_binder.hpp @@ -77,6 +77,13 @@ class UAVCAN_EXPORT MethodBinder validateBeforeCall(); (obj_->*fun_)(p1, p2); } + + template + void operator()(Par1& p1, Par2& p2, Par3& p3) + { + validateBeforeCall(); + (obj_->*fun_)(p1, p2, p3); + } }; } diff --git a/libuavcan/src/transport/uc_dispatcher.cpp b/libuavcan/src/transport/uc_dispatcher.cpp index 9db653c5..7b0720c8 100644 --- a/libuavcan/src/transport/uc_dispatcher.cpp +++ b/libuavcan/src/transport/uc_dispatcher.cpp @@ -162,28 +162,50 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame) return; } - switch (frame.getTransferType()) + switch(iface_protocol_[frame.getIfaceIndex()]) { - case TransferTypeMessageBroadcast: - { - lmsg_.handleFrame(frame, tao_disabled_); - break; - } - case TransferTypeServiceRequest: - { - lsrv_req_.handleFrame(frame, tao_disabled_); - break; - } - case TransferTypeServiceResponse: - { - lsrv_resp_.handleFrame(frame, tao_disabled_); - break; - } - default: - { - UAVCAN_ASSERT(0); - break; - } + case UAVCANProtocol: + { + switch (frame.getTransferType()) + { + case TransferTypeMessageBroadcast: + { + lmsg_.handleFrame(frame); + break; + } + case TransferTypeServiceRequest: + { + lsrv_req_.handleFrame(frame); + break; + } + case TransferTypeServiceResponse: + { + lsrv_resp_.handleFrame(frame); + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } + } + break; + } + default: + { + if (iface_protocol_[frame.getIfaceIndex()] == KDECANProtocol) + { + kdecan::KdeCanTransferListener* p = kdeCanListener_list_.get(); + while (p) + { + kdecan::KdeCanTransferListener* const next = p->getNextListNode(); + + p->handleFrame(can_frame); // p may be modified + + p = next; + } + } + } } } @@ -284,6 +306,22 @@ int Dispatcher::spinOnce() return num_frames_processed; } +int Dispatcher::sendRaw(const CanFrame& can_frame, TransferProtocol CAN_protocol, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, + CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask) +{ + uint8_t protocol_filtered_iface_mask = iface_mask; + + for (unsigned i = 0; i < canio_.getNumIfaces(); i++) + { + if (iface_protocol_[i] != CAN_protocol) + { + protocol_filtered_iface_mask &= ~(1 << i); + } + } + + return canio_.send(can_frame, tx_deadline, blocking_deadline, protocol_filtered_iface_mask, qos, flags); +} + int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask) { @@ -300,7 +338,18 @@ int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTim UAVCAN_ASSERT(0); return -ErrLogic; } - return canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos, flags); + + uint8_t protocol_filtered_iface_mask = iface_mask; + + for (unsigned i = 0; i < canio_.getNumIfaces(); i++) + { + if (iface_protocol_[i] != frame.getCANProtocol()) + { + protocol_filtered_iface_mask &= ~(1 << i); + } + } + + return canio_.send(can_frame, tx_deadline, blocking_deadline, protocol_filtered_iface_mask, qos, flags); } void Dispatcher::cleanup(MonotonicTime ts) @@ -341,6 +390,13 @@ bool Dispatcher::registerServiceResponseListener(TransferListener* listener) return lsrv_resp_.add(listener, ListenerRegistry::ManyListeners); // Multiple callers may call same srv } +bool Dispatcher::registerKdeCanListener(kdecan::KdeCanTransferListener* listener) +{ + kdeCanListener_list_.insert(listener); + + return true; +} + void Dispatcher::unregisterMessageListener(TransferListener* listener) { lmsg_.remove(listener); @@ -356,6 +412,11 @@ void Dispatcher::unregisterServiceResponseListener(TransferListener* listener) lsrv_resp_.remove(listener); } +void Dispatcher::unregisterKdeCanListener(kdecan::KdeCanTransferListener* listener) +{ + kdeCanListener_list_.remove(listener); +} + bool Dispatcher::hasSubscriber(DataTypeID dtid) const { return lmsg_.exists(dtid); From 0ae42da8600c833038f095a34d1cf08fb2614a4e Mon Sep 17 00:00:00 2001 From: Duarte Dias Date: Sun, 11 Jun 2023 21:49:41 +0200 Subject: [PATCH 02/12] add kdecan min and max actuation limits --- libuavcan/include/independent/kdecan.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libuavcan/include/independent/kdecan.hpp b/libuavcan/include/independent/kdecan.hpp index c7f183da..efa627d3 100644 --- a/libuavcan/include/independent/kdecan.hpp +++ b/libuavcan/include/independent/kdecan.hpp @@ -14,6 +14,8 @@ namespace kdecan static const int escNodeIdBroadcast = 1; static const int escNodeIdOffset = 2; +static const int minPwmValue = 1100; +static const int maxPwmValue = 1940; enum kdeCanObjAddr { From 60b83b3c66c50e30e6279d6138a27fc1a4a97e2d Mon Sep 17 00:00:00 2001 From: Duarte Dias Date: Mon, 12 Jun 2023 00:23:01 +0200 Subject: [PATCH 03/12] dispatcher now operates over a virtual set of independent protocol listeners, so it is more versatile when considering different protocols. --- libuavcan/include/independent/kdecan.hpp | 97 +++++++++---------- .../include/uavcan/transport/dispatcher.hpp | 8 +- .../include/uavcan/transport/independent.hpp | 41 ++++++++ libuavcan/src/transport/uc_dispatcher.cpp | 23 ++--- 4 files changed, 104 insertions(+), 65 deletions(-) create mode 100644 libuavcan/include/uavcan/transport/independent.hpp diff --git a/libuavcan/include/independent/kdecan.hpp b/libuavcan/include/independent/kdecan.hpp index efa627d3..4f0eda9c 100644 --- a/libuavcan/include/independent/kdecan.hpp +++ b/libuavcan/include/independent/kdecan.hpp @@ -1,13 +1,8 @@ #ifndef UAVCAN_INDEPENDENT_KDECAN_HPP_INCLUDED #define UAVCAN_INDEPENDENT_KDECAN_HPP_INCLUDED -#include -#include -#include -#include -#include -#include -#include +#include +#include namespace kdecan { @@ -83,28 +78,6 @@ class KdeFrame } }; -class KdeCanTransferListener : public uavcan::LinkedListNode -{ -public: - kdeCanObjAddr target_object_address_; - - KdeCanTransferListener(const kdeCanObjAddr target_object_address) : - target_object_address_(target_object_address) - {;} - - virtual void handleIncomingTransfer(const uint32_t id, const uint8_t dlc, const uint8_t* data) = 0; - - void handleFrame(const uavcan::CanRxFrame& can_frame) - { - uint32_t object_address = can_frame.id & 0x000000FF; - - if (object_address == target_object_address_) - { - handleIncomingTransfer(can_frame.id, can_frame.dlc, can_frame.data); - } - } -}; - template = UAVCAN_CPP11 typename Callback_ = std::function @@ -112,41 +85,48 @@ template -class KdeCanSubscriber +class Subscriber { public: - typedef KdeCanSubscriber SelfType; + typedef Subscriber SelfType; typedef Callback_ Callback; - class kdeTransferForwarder : public KdeCanTransferListener + class kdeTransferForwarder : public uavcan::IndependentTransferListener { public: SelfType& obj_; - void handleIncomingTransfer(const uint32_t id, const uint8_t dlc, const uint8_t* data) override + void handleFrame(const uavcan::CanRxFrame& can_frame) override { - obj_.handleIncomingTransfer(id, dlc, data); + obj_.handleIncomingTransfer(can_frame); } - kdeTransferForwarder(SelfType& obj, const kdeCanObjAddr target_object_address) : - KdeCanTransferListener(target_object_address), + kdeTransferForwarder(SelfType& obj) : + uavcan::IndependentTransferListener(uavcan::TransferProtocol::KDECANProtocol), obj_(obj) {;} }; + uavcan::INode& node_; Callback callback_; kdeTransferForwarder forwarder_; + kdeCanObjAddr target_object_address_; - KdeCanSubscriber() : + Subscriber(uavcan::INode& node) : + node_(node), callback_(), - forwarder_(*this, DataType_) + forwarder_(*this), + target_object_address_(DataType_) {;} - void handleIncomingTransfer(const uint32_t id, const uint8_t dlc, const uint8_t* data) + void handleIncomingTransfer(const uavcan::CanRxFrame& can_frame) { - if (callback_) + uint32_t object_address = can_frame.id & 0x000000FF; + uint32_t destination_id = (can_frame.id & 0x0000FF00) >> 8; + + if (destination_id == node_.getNodeID().get() && object_address == target_object_address_ && callback_) { - callback_(id, dlc, data); + callback_(can_frame.id, can_frame.dlc, can_frame.data); } } @@ -155,25 +135,42 @@ class KdeCanSubscriber callback_ = callback; } - KdeCanTransferListener* getKdeListener() + uavcan::IndependentTransferListener* getKdeListener() { return &(this->forwarder_); } }; -class KdeCanTransferSender +template +class Publisher { public: - KdeCanTransferSender() {;} + uavcan::INode& node_; + kdeCanObjAddr target_object_address_; - bool publish(uavcan::CanFrame& can_frame, - const uint8_t source_address, - const uint8_t destination_address, - const kdeCanObjAddr object_address, const uint8_t* data) + Publisher(uavcan::INode& node) : + node_(node), + target_object_address_(DataType_) + {;} + + bool publish(const uint8_t destination_address, + const uint8_t* data) { - KdeFrame kde_frame(source_address, destination_address, object_address, data); + KdeFrame kde_frame(node_.getNodeID().get(), destination_address, target_object_address_, data); + + uavcan::CanFrame can_frame; + kde_frame.compile(can_frame); - return kde_frame.compile(can_frame); + node_.getDispatcher().sendRaw(can_frame, + uavcan::TransferProtocol::KDECANProtocol, + node_.getMonotonicTime() + uavcan::MonotonicDuration::fromMSec(100), + uavcan::MonotonicTime(), + uavcan::CanTxQueue::Qos::Volatile, + uavcan::CanIOFlags(0), + (uint8_t)0xFF); + + // so far we do no checks in this + return true; } }; diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index 1bd5c247..36df2714 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -14,7 +14,7 @@ #include #include #include -#include +#include namespace uavcan { @@ -122,7 +122,7 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable ListenerRegistry lsrv_req_; ListenerRegistry lsrv_resp_; - LinkedListRoot kdeCanListener_list_; + LinkedListRoot IndependentCanListener_list_; #if !UAVCAN_TINY LoopbackFrameListenerRegistry loopback_listeners_; @@ -178,12 +178,12 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable bool registerMessageListener(TransferListener* listener); bool registerServiceRequestListener(TransferListener* listener); bool registerServiceResponseListener(TransferListener* listener); - bool registerKdeCanListener(kdecan::KdeCanTransferListener* listener); + bool registerIndependentCanListener(IndependentTransferListener* listener); void unregisterMessageListener(TransferListener* listener); void unregisterServiceRequestListener(TransferListener* listener); void unregisterServiceResponseListener(TransferListener* listener); - void unregisterKdeCanListener(kdecan::KdeCanTransferListener* listener); + void unregisterIndependentCanListener(IndependentTransferListener* listener); bool hasSubscriber(DataTypeID dtid) const; bool hasPublisher(DataTypeID dtid) const; diff --git a/libuavcan/include/uavcan/transport/independent.hpp b/libuavcan/include/uavcan/transport/independent.hpp new file mode 100644 index 00000000..cc6cba64 --- /dev/null +++ b/libuavcan/include/uavcan/transport/independent.hpp @@ -0,0 +1,41 @@ +#ifndef UAVCAN_INDEPENDENT_INDEPENDENT_HPP_INCLUDED +#define UAVCAN_INDEPENDENT_INDEPENDENT_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ + +class IndependentTransferListener : public LinkedListNode +{ +public: + TransferProtocol protocol_; + + IndependentTransferListener(const TransferProtocol protocol) : + protocol_(protocol) + {;} + + TransferProtocol getCANProtocol() { return protocol_; } + + virtual void handleFrame(const CanRxFrame& can_frame) = 0; +}; + +// right now this class is useless +class IndependentTransferSender +{ +public: + TransferProtocol protocol_; + + IndependentTransferSender(const TransferProtocol protocol) : + protocol_(protocol) + {;} + + TransferProtocol getCANProtocol() { return protocol_; } +}; + +}; + +#endif // UAVCAN_INDEPENDENT_INDEPENDENT_HPP_INCLUDED diff --git a/libuavcan/src/transport/uc_dispatcher.cpp b/libuavcan/src/transport/uc_dispatcher.cpp index 7b0720c8..621b1a17 100644 --- a/libuavcan/src/transport/uc_dispatcher.cpp +++ b/libuavcan/src/transport/uc_dispatcher.cpp @@ -193,17 +193,18 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame) } default: { - if (iface_protocol_[frame.getIfaceIndex()] == KDECANProtocol) + IndependentTransferListener* p = IndependentCanListener_list_.get(); + + while (p) { - kdecan::KdeCanTransferListener* p = kdeCanListener_list_.get(); - while (p) - { - kdecan::KdeCanTransferListener* const next = p->getNextListNode(); + IndependentTransferListener* const next = p->getNextListNode(); + if (p->getCANProtocol() == iface_protocol_[frame.getIfaceIndex()]) + { p->handleFrame(can_frame); // p may be modified - - p = next; } + + p = next; } } } @@ -390,9 +391,9 @@ bool Dispatcher::registerServiceResponseListener(TransferListener* listener) return lsrv_resp_.add(listener, ListenerRegistry::ManyListeners); // Multiple callers may call same srv } -bool Dispatcher::registerKdeCanListener(kdecan::KdeCanTransferListener* listener) +bool Dispatcher::registerIndependentCanListener(IndependentTransferListener* listener) { - kdeCanListener_list_.insert(listener); + IndependentCanListener_list_.insert(listener); return true; } @@ -412,9 +413,9 @@ void Dispatcher::unregisterServiceResponseListener(TransferListener* listener) lsrv_resp_.remove(listener); } -void Dispatcher::unregisterKdeCanListener(kdecan::KdeCanTransferListener* listener) +void Dispatcher::unregisterIndependentCanListener(IndependentTransferListener* listener) { - kdeCanListener_list_.remove(listener); + IndependentCanListener_list_.remove(listener); } bool Dispatcher::hasSubscriber(DataTypeID dtid) const From 2a2513f739b99219291c490b0dfe720e4a84c411 Mon Sep 17 00:00:00 2001 From: Duarte Dias Date: Sat, 17 Jun 2023 11:44:13 +0200 Subject: [PATCH 04/12] kdecan: node ID needs to be always 0 --- libuavcan/include/independent/kdecan.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/independent/kdecan.hpp b/libuavcan/include/independent/kdecan.hpp index 4f0eda9c..1f718803 100644 --- a/libuavcan/include/independent/kdecan.hpp +++ b/libuavcan/include/independent/kdecan.hpp @@ -12,6 +12,8 @@ static const int escNodeIdOffset = 2; static const int minPwmValue = 1100; static const int maxPwmValue = 1940; +static const int MASTER_NODE_ID = 0; + enum kdeCanObjAddr { ESCInformation = 0, @@ -124,7 +126,7 @@ class Subscriber uint32_t object_address = can_frame.id & 0x000000FF; uint32_t destination_id = (can_frame.id & 0x0000FF00) >> 8; - if (destination_id == node_.getNodeID().get() && object_address == target_object_address_ && callback_) + if (destination_id == MASTER_NODE_ID && object_address == target_object_address_ && callback_) { callback_(can_frame.id, can_frame.dlc, can_frame.data); } @@ -156,7 +158,7 @@ class Publisher bool publish(const uint8_t destination_address, const uint8_t* data) { - KdeFrame kde_frame(node_.getNodeID().get(), destination_address, target_object_address_, data); + KdeFrame kde_frame(MASTER_NODE_ID, destination_address, target_object_address_, data); uavcan::CanFrame can_frame; kde_frame.compile(can_frame); From 7971b540819c950bd7924165bab2332722b1dae1 Mon Sep 17 00:00:00 2001 From: Duarte Dias Date: Sat, 17 Jun 2023 22:45:55 +0200 Subject: [PATCH 05/12] removed concrete implementation of KDE can from the library. --- libuavcan/include/independent/kdecan.hpp | 181 ------------------ libuavcan/include/uavcan/transport/can_io.hpp | 6 + .../include/uavcan/transport/dispatcher.hpp | 25 +-- libuavcan/include/uavcan/transport/frame.hpp | 13 +- .../include/uavcan/transport/transfer.hpp | 3 +- libuavcan/src/transport/uc_can_io.cpp | 42 ++++ libuavcan/src/transport/uc_dispatcher.cpp | 34 +--- 7 files changed, 58 insertions(+), 246 deletions(-) delete mode 100644 libuavcan/include/independent/kdecan.hpp diff --git a/libuavcan/include/independent/kdecan.hpp b/libuavcan/include/independent/kdecan.hpp deleted file mode 100644 index 1f718803..00000000 --- a/libuavcan/include/independent/kdecan.hpp +++ /dev/null @@ -1,181 +0,0 @@ -#ifndef UAVCAN_INDEPENDENT_KDECAN_HPP_INCLUDED -#define UAVCAN_INDEPENDENT_KDECAN_HPP_INCLUDED - -#include -#include - -namespace kdecan -{ - -static const int escNodeIdBroadcast = 1; -static const int escNodeIdOffset = 2; -static const int minPwmValue = 1100; -static const int maxPwmValue = 1940; - -static const int MASTER_NODE_ID = 0; - -enum kdeCanObjAddr -{ - ESCInformation = 0, - PWMThrottle = 1, - ESCInputThrottle = 6, - ESCOutputThrottle = 7, - ESCStatus = 11, - GetMCUId = 8, - UpdateNodeAddress = 9, - StartESCEnumeration = 10, - Shutdown = 32, - Restart = 33 -}; - -class KdeFrame -{ -public: - enum { PayloadCapacity = 8 }; - - uint8_t source_address_; - uint8_t destination_address_; - kdeCanObjAddr object_address_; - uint8_t data_[PayloadCapacity]; - - KdeFrame(const uint8_t source_address, - const uint8_t destination_address, - kdeCanObjAddr object_address, - const uint8_t* data) : - source_address_(source_address), - destination_address_(destination_address), - object_address_(object_address) - { - memcpy(data_, data, PayloadCapacity); - } - - bool compile(uavcan::CanFrame& out_can_frame) - { - out_can_frame.id = ((uint32_t)0x00000000) | - ((uint32_t)object_address_) | - (((uint32_t)destination_address_) << 8) | - (((uint32_t)source_address_) << 16) | - uavcan::CanFrame::FlagEFF; - - switch(object_address_) - { - case ESCInformation: { out_can_frame.dlc = 0; } break; - case PWMThrottle: { out_can_frame.dlc = 2; } break; - case ESCInputThrottle: { out_can_frame.dlc = 0; } break; - case ESCOutputThrottle: { out_can_frame.dlc = 0; } break; - case ESCStatus: { out_can_frame.dlc = 0; } break; - case GetMCUId: { out_can_frame.dlc = 0; } break; - case UpdateNodeAddress: { out_can_frame.dlc = 8; } break; - case StartESCEnumeration: { out_can_frame.dlc = 2; } break; - case Shutdown: { out_can_frame.dlc = 0; } break; - case Restart: { out_can_frame.dlc = 0; } break; - - default: { return false; } - } - - // this function will take care of little/big endian conversions - (void)uavcan::copy(data_, data_ + out_can_frame.dlc, out_can_frame.data); - - return true; - } -}; - -template = UAVCAN_CPP11 - typename Callback_ = std::function -#else - typename Callback_ = void (*)(const uint32_t id, const uint8_t dlc, const uint8_t* data) -#endif -> -class Subscriber -{ -public: - typedef Subscriber SelfType; - typedef Callback_ Callback; - - class kdeTransferForwarder : public uavcan::IndependentTransferListener - { - public: - SelfType& obj_; - - void handleFrame(const uavcan::CanRxFrame& can_frame) override - { - obj_.handleIncomingTransfer(can_frame); - } - - kdeTransferForwarder(SelfType& obj) : - uavcan::IndependentTransferListener(uavcan::TransferProtocol::KDECANProtocol), - obj_(obj) - {;} - }; - - uavcan::INode& node_; - Callback callback_; - kdeTransferForwarder forwarder_; - kdeCanObjAddr target_object_address_; - - Subscriber(uavcan::INode& node) : - node_(node), - callback_(), - forwarder_(*this), - target_object_address_(DataType_) - {;} - - void handleIncomingTransfer(const uavcan::CanRxFrame& can_frame) - { - uint32_t object_address = can_frame.id & 0x000000FF; - uint32_t destination_id = (can_frame.id & 0x0000FF00) >> 8; - - if (destination_id == MASTER_NODE_ID && object_address == target_object_address_ && callback_) - { - callback_(can_frame.id, can_frame.dlc, can_frame.data); - } - } - - void setCallback(Callback callback) - { - callback_ = callback; - } - - uavcan::IndependentTransferListener* getKdeListener() - { - return &(this->forwarder_); - } -}; - -template -class Publisher -{ -public: - uavcan::INode& node_; - kdeCanObjAddr target_object_address_; - - Publisher(uavcan::INode& node) : - node_(node), - target_object_address_(DataType_) - {;} - - bool publish(const uint8_t destination_address, - const uint8_t* data) - { - KdeFrame kde_frame(MASTER_NODE_ID, destination_address, target_object_address_, data); - - uavcan::CanFrame can_frame; - kde_frame.compile(can_frame); - - node_.getDispatcher().sendRaw(can_frame, - uavcan::TransferProtocol::KDECANProtocol, - node_.getMonotonicTime() + uavcan::MonotonicDuration::fromMSec(100), - uavcan::MonotonicTime(), - uavcan::CanTxQueue::Qos::Volatile, - uavcan::CanIOFlags(0), - (uint8_t)0xFF); - - // so far we do no checks in this - return true; - } -}; - -}; - -#endif // UAVCAN_INDEPENDENT_KDECAN_HPP_INCLUDED diff --git a/libuavcan/include/uavcan/transport/can_io.hpp b/libuavcan/include/uavcan/transport/can_io.hpp index 11752f0f..e7740591 100644 --- a/libuavcan/include/uavcan/transport/can_io.hpp +++ b/libuavcan/include/uavcan/transport/can_io.hpp @@ -149,6 +149,7 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable LazyConstructor tx_queues_[MaxCanIfaces]; IfaceFrameCounters counters_[MaxCanIfaces]; + TransferProtocol iface_protocol_[MaxCanIfaces]; const uint8_t num_ifaces_; @@ -161,6 +162,9 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t mem_blocks_per_iface = 0); + bool changeIfaceProtocol(unsigned ifaceId, TransferProtocol protocol); + TransferProtocol getIfaceProtocol(unsigned ifaceId); + uint8_t getNumIfaces() const { return num_ifaces_; } CanIfacePerfCounters getIfacePerfCounters(uint8_t iface_index) const; @@ -178,6 +182,8 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable */ int send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags); + int send(const CanFrame& frame, TransferProtocol frame_protocol, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, + uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags); int receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags); }; diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index 36df2714..1e8d5e6e 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -86,8 +86,6 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable bool tao_disabled_ = false; bool canfd_ = false; - TransferProtocol iface_protocol_[UAVCAN_STM32H7_NUM_IFACES]; - class ListenerRegistry { LinkedListRoot list_; @@ -148,12 +146,7 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable #endif , self_node_id_(NodeID::Broadcast) // Default , self_node_id_is_set_(false) - { - for (unsigned i = 0; i < driver.getNumIfaces(); i++) - { - iface_protocol_[i] = UAVCANProtocol; - } - } + {;} /** * This version returns strictly when the deadline is reached. @@ -168,8 +161,6 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable /** * Refer to CanIOManager::send() for the parameter description */ - int sendRaw(const CanFrame& can_frame, TransferProtocol CAN_protocol, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos, - CanIOFlags flags, uint8_t iface_mask); int send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask); @@ -196,20 +187,6 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable unsigned getNumServiceRequestListeners() const { return lsrv_req_.getNumEntries(); } unsigned getNumServiceResponseListeners() const { return lsrv_resp_.getNumEntries(); } - bool changeIfaceProtocol(unsigned ifaceId, TransferProtocol protocol) - { - if (ifaceId < canio_.getNumIfaces()) - { - iface_protocol_[ifaceId] = protocol; - - return true; - } - else - { - return false; - } - } - /** * These methods can be used to retreive lists of messages, service requests and service responses the * dispatcher is currently listening to. diff --git a/libuavcan/include/uavcan/transport/frame.hpp b/libuavcan/include/uavcan/transport/frame.hpp index 26f5f917..a64af788 100644 --- a/libuavcan/include/uavcan/transport/frame.hpp +++ b/libuavcan/include/uavcan/transport/frame.hpp @@ -35,17 +35,13 @@ class UAVCAN_EXPORT Frame bool toggle_; bool canfd_frame_; - TransferProtocol CAN_protocol_; - public: Frame() : transfer_type_(TransferType(NumTransferTypes)), // Invalid value payload_len_(0), start_of_transfer_(false), end_of_transfer_(false), - toggle_(false), - canfd_frame_(false), - CAN_protocol_(UAVCANProtocol) + toggle_(false) { } Frame(DataTypeID data_type_id, @@ -63,9 +59,7 @@ class UAVCAN_EXPORT Frame transfer_id_(transfer_id), start_of_transfer_(false), end_of_transfer_(false), - toggle_(false), - canfd_frame_(canfd_frame), - CAN_protocol_(UAVCANProtocol) + toggle_(false) { UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast()); UAVCAN_ASSERT(data_type_id.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type))); @@ -98,9 +92,6 @@ class UAVCAN_EXPORT Frame NodeID getDstNodeID() const { return dst_node_id_; } TransferID getTransferID() const { return transfer_id_; } - void setCANProtocol(TransferProtocol CAN_protocol) { CAN_protocol_ = CAN_protocol; } - TransferProtocol getCANProtocol() const { return CAN_protocol_; } - void setStartOfTransfer(bool x) { start_of_transfer_ = x; } void setEndOfTransfer(bool x) { end_of_transfer_ = x; } diff --git a/libuavcan/include/uavcan/transport/transfer.hpp b/libuavcan/include/uavcan/transport/transfer.hpp index f98158ec..1339ce00 100644 --- a/libuavcan/include/uavcan/transport/transfer.hpp +++ b/libuavcan/include/uavcan/transport/transfer.hpp @@ -25,7 +25,8 @@ enum TransferType enum TransferProtocol { UAVCANProtocol = 0, - KDECANProtocol = 1 + KDECANProtocol = 1, + UNKNOWNProtocol = 2 }; static const uint8_t NumTransferTypes = 3; diff --git a/libuavcan/src/transport/uc_can_io.cpp b/libuavcan/src/transport/uc_can_io.cpp index bcca8834..09afddde 100644 --- a/libuavcan/src/transport/uc_can_io.cpp +++ b/libuavcan/src/transport/uc_can_io.cpp @@ -307,6 +307,32 @@ CanIOManager::CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISyste } } +bool CanIOManager::changeIfaceProtocol(unsigned ifaceId, TransferProtocol protocol) +{ + if (ifaceId < getNumIfaces()) + { + iface_protocol_[ifaceId] = protocol; + + return true; + } + else + { + return false; + } +} + +TransferProtocol CanIOManager::getIfaceProtocol(unsigned ifaceId) +{ + if (ifaceId < getNumIfaces()) + { + return iface_protocol_[ifaceId]; + } + else + { + return UNKNOWNProtocol; + } +} + uint8_t CanIOManager::makePendingTxMask() const { uint8_t write_mask = 0; @@ -337,11 +363,27 @@ CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) con int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags) +{ + return send(frame, UAVCANProtocol, tx_deadline, blocking_deadline, iface_mask, qos, flags); +} + +int CanIOManager::send(const CanFrame& frame, TransferProtocol frame_protocol, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, + uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags) { const uint8_t num_ifaces = getNumIfaces(); const uint8_t all_ifaces_mask = uint8_t((1U << num_ifaces) - 1); + iface_mask &= all_ifaces_mask; + // make sure we send frames of a specific protocol only for ifaces configured with the same protocol + for (unsigned i = 0; i < getNumIfaces(); i++) + { + if (iface_protocol_[i] != frame_protocol) + { + iface_mask &= ~(1 << i); + } + } + if (blocking_deadline > tx_deadline) { blocking_deadline = tx_deadline; diff --git a/libuavcan/src/transport/uc_dispatcher.cpp b/libuavcan/src/transport/uc_dispatcher.cpp index 621b1a17..fa66ce3c 100644 --- a/libuavcan/src/transport/uc_dispatcher.cpp +++ b/libuavcan/src/transport/uc_dispatcher.cpp @@ -162,7 +162,9 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame) return; } - switch(iface_protocol_[frame.getIfaceIndex()]) + const TransferProtocol iface_protocol = getCanIOManager().getIfaceProtocol(frame.getIfaceIndex()); + + switch(iface_protocol) { case UAVCANProtocol: { @@ -199,7 +201,7 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame) { IndependentTransferListener* const next = p->getNextListNode(); - if (p->getCANProtocol() == iface_protocol_[frame.getIfaceIndex()]) + if (p->getCANProtocol() == iface_protocol) { p->handleFrame(can_frame); // p may be modified } @@ -307,22 +309,6 @@ int Dispatcher::spinOnce() return num_frames_processed; } -int Dispatcher::sendRaw(const CanFrame& can_frame, TransferProtocol CAN_protocol, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, - CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask) -{ - uint8_t protocol_filtered_iface_mask = iface_mask; - - for (unsigned i = 0; i < canio_.getNumIfaces(); i++) - { - if (iface_protocol_[i] != CAN_protocol) - { - protocol_filtered_iface_mask &= ~(1 << i); - } - } - - return canio_.send(can_frame, tx_deadline, blocking_deadline, protocol_filtered_iface_mask, qos, flags); -} - int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask) { @@ -340,17 +326,7 @@ int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTim return -ErrLogic; } - uint8_t protocol_filtered_iface_mask = iface_mask; - - for (unsigned i = 0; i < canio_.getNumIfaces(); i++) - { - if (iface_protocol_[i] != frame.getCANProtocol()) - { - protocol_filtered_iface_mask &= ~(1 << i); - } - } - - return canio_.send(can_frame, tx_deadline, blocking_deadline, protocol_filtered_iface_mask, qos, flags); + return canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos, flags); } void Dispatcher::cleanup(MonotonicTime ts) From 0e4d78764192ea8d853cd39a60d689e49e551fd2 Mon Sep 17 00:00:00 2001 From: Duarte Dias Date: Sat, 17 Jun 2023 23:10:51 +0200 Subject: [PATCH 06/12] remove dependency of specific protocol definitions in the library. Note that there is less protection since we assume the protocol identifier is now an integer. --- libuavcan/include/uavcan/transport/can_io.hpp | 8 ++++---- libuavcan/include/uavcan/transport/independent.hpp | 12 ++++++------ libuavcan/include/uavcan/transport/transfer.hpp | 7 +------ libuavcan/src/transport/uc_can_io.cpp | 10 +++++----- libuavcan/src/transport/uc_dispatcher.cpp | 4 ++-- 5 files changed, 18 insertions(+), 23 deletions(-) diff --git a/libuavcan/include/uavcan/transport/can_io.hpp b/libuavcan/include/uavcan/transport/can_io.hpp index e7740591..7d3ff8ac 100644 --- a/libuavcan/include/uavcan/transport/can_io.hpp +++ b/libuavcan/include/uavcan/transport/can_io.hpp @@ -149,7 +149,7 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable LazyConstructor tx_queues_[MaxCanIfaces]; IfaceFrameCounters counters_[MaxCanIfaces]; - TransferProtocol iface_protocol_[MaxCanIfaces]; + int iface_protocol_[MaxCanIfaces]; const uint8_t num_ifaces_; @@ -162,8 +162,8 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t mem_blocks_per_iface = 0); - bool changeIfaceProtocol(unsigned ifaceId, TransferProtocol protocol); - TransferProtocol getIfaceProtocol(unsigned ifaceId); + bool changeIfaceProtocol(unsigned ifaceId, int protocol); + int getIfaceProtocol(unsigned ifaceId); uint8_t getNumIfaces() const { return num_ifaces_; } @@ -182,7 +182,7 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable */ int send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags); - int send(const CanFrame& frame, TransferProtocol frame_protocol, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, + int send(const CanFrame& frame, int frame_protocol, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags); int receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags); }; diff --git a/libuavcan/include/uavcan/transport/independent.hpp b/libuavcan/include/uavcan/transport/independent.hpp index cc6cba64..d0baa2d2 100644 --- a/libuavcan/include/uavcan/transport/independent.hpp +++ b/libuavcan/include/uavcan/transport/independent.hpp @@ -12,13 +12,13 @@ namespace uavcan class IndependentTransferListener : public LinkedListNode { public: - TransferProtocol protocol_; + int protocol_; - IndependentTransferListener(const TransferProtocol protocol) : + IndependentTransferListener(const int protocol) : protocol_(protocol) {;} - TransferProtocol getCANProtocol() { return protocol_; } + int getCANProtocol() { return protocol_; } virtual void handleFrame(const CanRxFrame& can_frame) = 0; }; @@ -27,13 +27,13 @@ class IndependentTransferListener : public LinkedListNode Date: Sun, 18 Jun 2023 11:49:25 +0200 Subject: [PATCH 07/12] small cleanup and readded enum for protocolID --- libuavcan/include/uavcan/transport/can_io.hpp | 6 +++--- .../{independent.hpp => custom_protocols.hpp} | 6 +++--- libuavcan/include/uavcan/transport/dispatcher.hpp | 2 +- libuavcan/include/uavcan/transport/transfer.hpp | 9 ++++++++- libuavcan/src/transport/uc_can_io.cpp | 10 +++++----- libuavcan/src/transport/uc_dispatcher.cpp | 4 ++-- 6 files changed, 22 insertions(+), 15 deletions(-) rename libuavcan/include/uavcan/transport/{independent.hpp => custom_protocols.hpp} (81%) diff --git a/libuavcan/include/uavcan/transport/can_io.hpp b/libuavcan/include/uavcan/transport/can_io.hpp index 7d3ff8ac..2739f00e 100644 --- a/libuavcan/include/uavcan/transport/can_io.hpp +++ b/libuavcan/include/uavcan/transport/can_io.hpp @@ -149,7 +149,7 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable LazyConstructor tx_queues_[MaxCanIfaces]; IfaceFrameCounters counters_[MaxCanIfaces]; - int iface_protocol_[MaxCanIfaces]; + Protocol iface_protocol_[MaxCanIfaces]; const uint8_t num_ifaces_; @@ -162,8 +162,8 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t mem_blocks_per_iface = 0); - bool changeIfaceProtocol(unsigned ifaceId, int protocol); - int getIfaceProtocol(unsigned ifaceId); + bool changeIfaceProtocol(unsigned ifaceId, Protocol protocol); + Protocol getIfaceProtocol(unsigned ifaceId); uint8_t getNumIfaces() const { return num_ifaces_; } diff --git a/libuavcan/include/uavcan/transport/independent.hpp b/libuavcan/include/uavcan/transport/custom_protocols.hpp similarity index 81% rename from libuavcan/include/uavcan/transport/independent.hpp rename to libuavcan/include/uavcan/transport/custom_protocols.hpp index d0baa2d2..b01786e4 100644 --- a/libuavcan/include/uavcan/transport/independent.hpp +++ b/libuavcan/include/uavcan/transport/custom_protocols.hpp @@ -1,5 +1,5 @@ -#ifndef UAVCAN_INDEPENDENT_INDEPENDENT_HPP_INCLUDED -#define UAVCAN_INDEPENDENT_INDEPENDENT_HPP_INCLUDED +#ifndef UAVCAN_CUSTOM_PROTOCOLS_HPP_INCLUDED +#define UAVCAN_CUSTOM_PROTOCOLS_HPP_INCLUDED #include #include @@ -38,4 +38,4 @@ class IndependentTransferSender }; -#endif // UAVCAN_INDEPENDENT_INDEPENDENT_HPP_INCLUDED +#endif // UAVCAN_CUSTOM_PROTOCOLS_HPP_INCLUDED diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index 1e8d5e6e..e302df8a 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -14,7 +14,7 @@ #include #include #include -#include +#include namespace uavcan { diff --git a/libuavcan/include/uavcan/transport/transfer.hpp b/libuavcan/include/uavcan/transport/transfer.hpp index 19388946..56291e31 100644 --- a/libuavcan/include/uavcan/transport/transfer.hpp +++ b/libuavcan/include/uavcan/transport/transfer.hpp @@ -22,7 +22,14 @@ enum TransferType TransferTypeMessageBroadcast = 2 }; -static const unsigned protocolID = 0; +enum Protocol +{ + Standard = 0, + Custom1 = 1, + Custom2 = 2, + Custom3 = 3, + Invalid = 4 +}; static const uint8_t NumTransferTypes = 3; diff --git a/libuavcan/src/transport/uc_can_io.cpp b/libuavcan/src/transport/uc_can_io.cpp index 4f35a361..d54086ca 100644 --- a/libuavcan/src/transport/uc_can_io.cpp +++ b/libuavcan/src/transport/uc_can_io.cpp @@ -307,9 +307,9 @@ CanIOManager::CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISyste } } -bool CanIOManager::changeIfaceProtocol(unsigned ifaceId, int protocol) +bool CanIOManager::changeIfaceProtocol(unsigned ifaceId, Protocol protocol) { - if (ifaceId < getNumIfaces()) + if (ifaceId < getNumIfaces() && protocol != Protocol::Invalid) { iface_protocol_[ifaceId] = protocol; @@ -321,7 +321,7 @@ bool CanIOManager::changeIfaceProtocol(unsigned ifaceId, int protocol) } } -int CanIOManager::getIfaceProtocol(unsigned ifaceId) +Protocol CanIOManager::getIfaceProtocol(unsigned ifaceId) { if (ifaceId < getNumIfaces()) { @@ -329,7 +329,7 @@ int CanIOManager::getIfaceProtocol(unsigned ifaceId) } else { - return -1; + return Protocol::Invalid; } } @@ -364,7 +364,7 @@ CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) con int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags) { - return send(frame, uavcan::protocolID, tx_deadline, blocking_deadline, iface_mask, qos, flags); + return send(frame, Protocol::Standard, tx_deadline, blocking_deadline, iface_mask, qos, flags); } int CanIOManager::send(const CanFrame& frame, int frame_protocol, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, diff --git a/libuavcan/src/transport/uc_dispatcher.cpp b/libuavcan/src/transport/uc_dispatcher.cpp index 73b89ea1..27baa270 100644 --- a/libuavcan/src/transport/uc_dispatcher.cpp +++ b/libuavcan/src/transport/uc_dispatcher.cpp @@ -162,11 +162,11 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame) return; } - const int iface_protocol = getCanIOManager().getIfaceProtocol(frame.getIfaceIndex()); + const Protocol iface_protocol = getCanIOManager().getIfaceProtocol(frame.getIfaceIndex()); switch(iface_protocol) { - case uavcan::protocolID: + case Protocol::Standard: { switch (frame.getTransferType()) { From 9a0f715aebce23a4f642650fbbf27e0925cd34f1 Mon Sep 17 00:00:00 2001 From: Duarte Dias Date: Sun, 18 Jun 2023 11:57:27 +0200 Subject: [PATCH 08/12] Small renaming. --- .../include/uavcan/transport/custom_protocols.hpp | 8 ++++---- libuavcan/include/uavcan/transport/dispatcher.hpp | 6 +++--- libuavcan/src/transport/uc_dispatcher.cpp | 12 ++++++------ 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/libuavcan/include/uavcan/transport/custom_protocols.hpp b/libuavcan/include/uavcan/transport/custom_protocols.hpp index b01786e4..b0e0112b 100644 --- a/libuavcan/include/uavcan/transport/custom_protocols.hpp +++ b/libuavcan/include/uavcan/transport/custom_protocols.hpp @@ -9,12 +9,12 @@ namespace uavcan { -class IndependentTransferListener : public LinkedListNode +class CustomTransferListener : public LinkedListNode { public: int protocol_; - IndependentTransferListener(const int protocol) : + CustomTransferListener(const int protocol) : protocol_(protocol) {;} @@ -24,12 +24,12 @@ class IndependentTransferListener : public LinkedListNode IndependentCanListener_list_; + LinkedListRoot CustomCanListener_list_; #if !UAVCAN_TINY LoopbackFrameListenerRegistry loopback_listeners_; @@ -169,12 +169,12 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable bool registerMessageListener(TransferListener* listener); bool registerServiceRequestListener(TransferListener* listener); bool registerServiceResponseListener(TransferListener* listener); - bool registerIndependentCanListener(IndependentTransferListener* listener); + bool registerCustomCanListener(CustomTransferListener* listener); void unregisterMessageListener(TransferListener* listener); void unregisterServiceRequestListener(TransferListener* listener); void unregisterServiceResponseListener(TransferListener* listener); - void unregisterIndependentCanListener(IndependentTransferListener* listener); + void unregisterCustomCanListener(CustomTransferListener* listener); bool hasSubscriber(DataTypeID dtid) const; bool hasPublisher(DataTypeID dtid) const; diff --git a/libuavcan/src/transport/uc_dispatcher.cpp b/libuavcan/src/transport/uc_dispatcher.cpp index 27baa270..8d5fe719 100644 --- a/libuavcan/src/transport/uc_dispatcher.cpp +++ b/libuavcan/src/transport/uc_dispatcher.cpp @@ -195,11 +195,11 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame) } default: { - IndependentTransferListener* p = IndependentCanListener_list_.get(); + CustomTransferListener* p = CustomCanListener_list_.get(); while (p) { - IndependentTransferListener* const next = p->getNextListNode(); + CustomTransferListener* const next = p->getNextListNode(); if (p->getCANProtocol() == iface_protocol) { @@ -367,9 +367,9 @@ bool Dispatcher::registerServiceResponseListener(TransferListener* listener) return lsrv_resp_.add(listener, ListenerRegistry::ManyListeners); // Multiple callers may call same srv } -bool Dispatcher::registerIndependentCanListener(IndependentTransferListener* listener) +bool Dispatcher::registerCustomCanListener(CustomTransferListener* listener) { - IndependentCanListener_list_.insert(listener); + CustomCanListener_list_.insert(listener); return true; } @@ -389,9 +389,9 @@ void Dispatcher::unregisterServiceResponseListener(TransferListener* listener) lsrv_resp_.remove(listener); } -void Dispatcher::unregisterIndependentCanListener(IndependentTransferListener* listener) +void Dispatcher::unregisterCustomCanListener(CustomTransferListener* listener) { - IndependentCanListener_list_.remove(listener); + CustomCanListener_list_.remove(listener); } bool Dispatcher::hasSubscriber(DataTypeID dtid) const From 34c5a33df145944104608d0b06b1f7392662ef9e Mon Sep 17 00:00:00 2001 From: Duarte Dias Date: Sun, 18 Jun 2023 13:12:55 +0200 Subject: [PATCH 09/12] cleaning unecessary additions to method_binder. --- libuavcan/include/uavcan/util/method_binder.hpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/libuavcan/include/uavcan/util/method_binder.hpp b/libuavcan/include/uavcan/util/method_binder.hpp index 7da8c9fe..f3bbf424 100644 --- a/libuavcan/include/uavcan/util/method_binder.hpp +++ b/libuavcan/include/uavcan/util/method_binder.hpp @@ -77,13 +77,6 @@ class UAVCAN_EXPORT MethodBinder validateBeforeCall(); (obj_->*fun_)(p1, p2); } - - template - void operator()(Par1& p1, Par2& p2, Par3& p3) - { - validateBeforeCall(); - (obj_->*fun_)(p1, p2, p3); - } }; } From 400467d6c002396ae818ac044f4baf99465ef3ed Mon Sep 17 00:00:00 2001 From: Duarte Dias Date: Fri, 23 Jun 2023 00:39:40 +0200 Subject: [PATCH 10/12] Allowing filtering on the same can bus line by extended/not extended can ID --- .../uavcan/transport/custom_protocols.hpp | 18 ++-- libuavcan/src/transport/uc_dispatcher.cpp | 99 ++++++++++--------- 2 files changed, 64 insertions(+), 53 deletions(-) diff --git a/libuavcan/include/uavcan/transport/custom_protocols.hpp b/libuavcan/include/uavcan/transport/custom_protocols.hpp index b0e0112b..2bd85570 100644 --- a/libuavcan/include/uavcan/transport/custom_protocols.hpp +++ b/libuavcan/include/uavcan/transport/custom_protocols.hpp @@ -12,28 +12,32 @@ namespace uavcan class CustomTransferListener : public LinkedListNode { public: - int protocol_; + Protocol protocol_; - CustomTransferListener(const int protocol) : + CustomTransferListener() : protocol_(Protocol::Standard) {;} + + CustomTransferListener(const Protocol protocol) : protocol_(protocol) {;} - int getCANProtocol() { return protocol_; } + Protocol getCANProtocol() { return protocol_; } - virtual void handleFrame(const CanRxFrame& can_frame) = 0; + virtual bool handleFrame(const CanRxFrame& can_frame, const Protocol protocol) = 0; }; // right now this class is useless class CustomTransferSender { public: - int protocol_; + Protocol protocol_; + + CustomTransferSender() : protocol_(Protocol::Standard) {;} - CustomTransferSender(const int protocol) : + CustomTransferSender(const Protocol protocol) : protocol_(protocol) {;} - int getCANProtocol() { return protocol_; } + Protocol getCANProtocol() { return protocol_; } }; }; diff --git a/libuavcan/src/transport/uc_dispatcher.cpp b/libuavcan/src/transport/uc_dispatcher.cpp index 8d5fe719..804e0424 100644 --- a/libuavcan/src/transport/uc_dispatcher.cpp +++ b/libuavcan/src/transport/uc_dispatcher.cpp @@ -149,65 +149,72 @@ void Dispatcher::ListenerRegistry::handleFrame(const RxFrame& frame, bool tao_di void Dispatcher::handleFrame(const CanRxFrame& can_frame) { RxFrame frame; - if (!frame.parse(can_frame)) - { - // This is not counted as a transport error - UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str()); - return; - } - if ((frame.getDstNodeID() != NodeID::Broadcast) && - (frame.getDstNodeID() != getNodeID())) + const Protocol iface_protocol = getCanIOManager().getIfaceProtocol(can_frame.iface_index); + + // next block of operations only valid for UAVCAN, which requires extended frame + if (can_frame.isExtended() && iface_protocol == Protocol::Standard) { - return; - } + if (!frame.parse(can_frame)) + { + // This is not counted as a transport error + UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str()); + return; + } - const Protocol iface_protocol = getCanIOManager().getIfaceProtocol(frame.getIfaceIndex()); + if ((frame.getDstNodeID() != NodeID::Broadcast) && + (frame.getDstNodeID() != getNodeID())) + { + return; + } + } - switch(iface_protocol) + if (iface_protocol == Protocol::Standard && can_frame.isExtended()) { - case Protocol::Standard: + switch (frame.getTransferType()) { - switch (frame.getTransferType()) - { - case TransferTypeMessageBroadcast: - { - lmsg_.handleFrame(frame); - break; - } - case TransferTypeServiceRequest: - { - lsrv_req_.handleFrame(frame); - break; - } - case TransferTypeServiceResponse: - { - lsrv_resp_.handleFrame(frame); - break; - } - default: - { - UAVCAN_ASSERT(0); - break; - } - } + case TransferTypeMessageBroadcast: + { + lmsg_.handleFrame(frame); + break; + } + case TransferTypeServiceRequest: + { + lsrv_req_.handleFrame(frame); + break; + } + case TransferTypeServiceResponse: + { + lsrv_resp_.handleFrame(frame); break; } default: { - CustomTransferListener* p = CustomCanListener_list_.get(); + UAVCAN_ASSERT(0); + break; + } + } + } + else + { + bool success = false; - while (p) - { - CustomTransferListener* const next = p->getNextListNode(); + CustomTransferListener* p = CustomCanListener_list_.get(); + + while (p) + { + CustomTransferListener* const next = p->getNextListNode(); - if (p->getCANProtocol() == iface_protocol) - { - p->handleFrame(can_frame); // p may be modified - } + success = success || p->handleFrame(can_frame, iface_protocol); // p may be modified - p = next; - } + p = next; + } + + if (!success) + { + // This is not counted as a transport error + UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str()); + return; } } } From f350db10750979db5e7f489b288894c47fbf7943 Mon Sep 17 00:00:00 2001 From: Duarte Dias Date: Sun, 3 Sep 2023 14:41:14 +0200 Subject: [PATCH 11/12] uc_dispatcher: resolved some conflicts with upstream --- libuavcan/src/transport/uc_dispatcher.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libuavcan/src/transport/uc_dispatcher.cpp b/libuavcan/src/transport/uc_dispatcher.cpp index 804e0424..7f46dac3 100644 --- a/libuavcan/src/transport/uc_dispatcher.cpp +++ b/libuavcan/src/transport/uc_dispatcher.cpp @@ -175,17 +175,17 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame) { case TransferTypeMessageBroadcast: { - lmsg_.handleFrame(frame); + lmsg_.handleFrame(frame, tao_disabled_); break; } case TransferTypeServiceRequest: { - lsrv_req_.handleFrame(frame); + lsrv_req_.handleFrame(frame, tao_disabled_); break; } case TransferTypeServiceResponse: { - lsrv_resp_.handleFrame(frame); + lsrv_resp_.handleFrame(frame, tao_disabled_); break; } default: From bce5b061d99c793ae730e27c48ee2781cce77258 Mon Sep 17 00:00:00 2001 From: Duarte Dias Date: Sun, 3 Sep 2023 14:57:28 +0200 Subject: [PATCH 12/12] frame: solved additional upstream conflicts --- libuavcan/include/uavcan/transport/frame.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/transport/frame.hpp b/libuavcan/include/uavcan/transport/frame.hpp index a64af788..47f12e0c 100644 --- a/libuavcan/include/uavcan/transport/frame.hpp +++ b/libuavcan/include/uavcan/transport/frame.hpp @@ -41,7 +41,8 @@ class UAVCAN_EXPORT Frame payload_len_(0), start_of_transfer_(false), end_of_transfer_(false), - toggle_(false) + toggle_(false), + canfd_frame_(false) { } Frame(DataTypeID data_type_id, @@ -59,7 +60,8 @@ class UAVCAN_EXPORT Frame transfer_id_(transfer_id), start_of_transfer_(false), end_of_transfer_(false), - toggle_(false) + toggle_(false), + canfd_frame_(canfd_frame) { UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast()); UAVCAN_ASSERT(data_type_id.isValidForDataTypeKind(getDataTypeKindForTransferType(transfer_type)));