diff --git a/libuavcan/include/uavcan/transport/can_io.hpp b/libuavcan/include/uavcan/transport/can_io.hpp index 11752f0f..2739f00e 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]; + Protocol 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, Protocol protocol); + Protocol 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, 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/custom_protocols.hpp b/libuavcan/include/uavcan/transport/custom_protocols.hpp new file mode 100644 index 00000000..2bd85570 --- /dev/null +++ b/libuavcan/include/uavcan/transport/custom_protocols.hpp @@ -0,0 +1,45 @@ +#ifndef UAVCAN_CUSTOM_PROTOCOLS_HPP_INCLUDED +#define UAVCAN_CUSTOM_PROTOCOLS_HPP_INCLUDED + +#include +#include +#include +#include + +namespace uavcan +{ + +class CustomTransferListener : public LinkedListNode +{ +public: + Protocol protocol_; + + CustomTransferListener() : protocol_(Protocol::Standard) {;} + + CustomTransferListener(const Protocol protocol) : + protocol_(protocol) + {;} + + Protocol getCANProtocol() { return protocol_; } + + virtual bool handleFrame(const CanRxFrame& can_frame, const Protocol protocol) = 0; +}; + +// right now this class is useless +class CustomTransferSender +{ +public: + Protocol protocol_; + + CustomTransferSender() : protocol_(Protocol::Standard) {;} + + CustomTransferSender(const Protocol protocol) : + protocol_(protocol) + {;} + + Protocol getCANProtocol() { return protocol_; } +}; + +}; + +#endif // UAVCAN_CUSTOM_PROTOCOLS_HPP_INCLUDED diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index 50d06d78..db4be5ea 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_; @@ -120,6 +120,8 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable ListenerRegistry lsrv_req_; ListenerRegistry lsrv_resp_; + LinkedListRoot CustomCanListener_list_; + #if !UAVCAN_TINY LoopbackFrameListenerRegistry loopback_listeners_; IRxFrameListener* rx_listener_; @@ -144,7 +146,7 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable #endif , self_node_id_(NodeID::Broadcast) // Default , self_node_id_is_set_(false) - { } + {;} /** * This version returns strictly when the deadline is reached. @@ -167,10 +169,12 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable bool registerMessageListener(TransferListener* listener); bool registerServiceRequestListener(TransferListener* listener); bool registerServiceResponseListener(TransferListener* listener); + bool registerCustomCanListener(CustomTransferListener* listener); void unregisterMessageListener(TransferListener* listener); void unregisterServiceRequestListener(TransferListener* listener); void unregisterServiceResponseListener(TransferListener* listener); + void unregisterCustomCanListener(CustomTransferListener* listener); bool hasSubscriber(DataTypeID dtid) const; bool hasPublisher(DataTypeID dtid) const; diff --git a/libuavcan/include/uavcan/transport/transfer.hpp b/libuavcan/include/uavcan/transport/transfer.hpp index 7e5d5b88..56291e31 100644 --- a/libuavcan/include/uavcan/transport/transfer.hpp +++ b/libuavcan/include/uavcan/transport/transfer.hpp @@ -22,6 +22,15 @@ enum TransferType TransferTypeMessageBroadcast = 2 }; +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 bcca8834..d54086ca 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, Protocol protocol) +{ + if (ifaceId < getNumIfaces() && protocol != Protocol::Invalid) + { + iface_protocol_[ifaceId] = protocol; + + return true; + } + else + { + return false; + } +} + +Protocol CanIOManager::getIfaceProtocol(unsigned ifaceId) +{ + if (ifaceId < getNumIfaces()) + { + return iface_protocol_[ifaceId]; + } + else + { + return Protocol::Invalid; + } +} + 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, 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, + 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 9db653c5..7f46dac3 100644 --- a/libuavcan/src/transport/uc_dispatcher.cpp +++ b/libuavcan/src/transport/uc_dispatcher.cpp @@ -149,41 +149,73 @@ 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())) - { - return; - } + const Protocol iface_protocol = getCanIOManager().getIfaceProtocol(can_frame.iface_index); - switch (frame.getTransferType()) + // next block of operations only valid for UAVCAN, which requires extended frame + if (can_frame.isExtended() && iface_protocol == Protocol::Standard) { - case TransferTypeMessageBroadcast: - { - lmsg_.handleFrame(frame, tao_disabled_); - break; - } - case TransferTypeServiceRequest: - { - lsrv_req_.handleFrame(frame, tao_disabled_); - break; + 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())) + { + return; + } } - case TransferTypeServiceResponse: + + if (iface_protocol == Protocol::Standard && can_frame.isExtended()) { - lsrv_resp_.handleFrame(frame, tao_disabled_); - break; + switch (frame.getTransferType()) + { + 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; + } + } } - default: + else { - UAVCAN_ASSERT(0); - break; - } + bool success = false; + + CustomTransferListener* p = CustomCanListener_list_.get(); + + while (p) + { + CustomTransferListener* const next = p->getNextListNode(); + + success = success || p->handleFrame(can_frame, iface_protocol); // p may be modified + + 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; + } } } @@ -300,6 +332,7 @@ 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); } @@ -341,6 +374,13 @@ bool Dispatcher::registerServiceResponseListener(TransferListener* listener) return lsrv_resp_.add(listener, ListenerRegistry::ManyListeners); // Multiple callers may call same srv } +bool Dispatcher::registerCustomCanListener(CustomTransferListener* listener) +{ + CustomCanListener_list_.insert(listener); + + return true; +} + void Dispatcher::unregisterMessageListener(TransferListener* listener) { lmsg_.remove(listener); @@ -356,6 +396,11 @@ void Dispatcher::unregisterServiceResponseListener(TransferListener* listener) lsrv_resp_.remove(listener); } +void Dispatcher::unregisterCustomCanListener(CustomTransferListener* listener) +{ + CustomCanListener_list_.remove(listener); +} + bool Dispatcher::hasSubscriber(DataTypeID dtid) const { return lmsg_.exists(dtid);