diff --git a/libuavcan/include/uavcan/node/generic_publisher.hpp b/libuavcan/include/uavcan/node/generic_publisher.hpp index d91123a75e..063555665c 100644 --- a/libuavcan/include/uavcan/node/generic_publisher.hpp +++ b/libuavcan/include/uavcan/node/generic_publisher.hpp @@ -17,94 +17,16 @@ namespace uavcan { -/** - * Generic publisher, suitable for messages and services. - * DataSpec - data type specification class - * DataStruct - instantiable class - */ -template -class UAVCAN_EXPORT GenericPublisher +class GenericPublisherBase { - enum - { - Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ? - CanTxQueue::Volatile : CanTxQueue::Persistent - }; - const MonotonicDuration max_transfer_interval_; // TODO: memory usage can be reduced MonotonicDuration tx_timeout_; INode& node_; LazyConstructor sender_; - int checkInit() - { - if (sender_) - { - return 0; - } - - GlobalDataTypeRegistry::instance().freeze(); - - const DataTypeDescriptor* const descr = - GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind), - DataSpec::getDataTypeFullName()); - if (!descr) - { - UAVCAN_TRACE("GenericPublisher", "Type [%s] is not registered", DataSpec::getDataTypeFullName()); - return -ErrUnknownDataType; - } - sender_.template construct - (node_.getDispatcher(), *descr, CanTxQueue::Qos(Qos), max_transfer_interval_); - return 0; - } - - MonotonicTime getTxDeadline() const { return node_.getMonotonicTime() + tx_timeout_; } - - IMarshalBuffer* getBuffer() - { - return node_.getMarshalBufferProvider().getBuffer(BitLenToByteLen::Result); - } - - int genericPublish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id, - TransferID* tid, MonotonicTime blocking_deadline) - { - const int res = checkInit(); - if (res < 0) - { - return res; - } - - IMarshalBuffer* const buf = getBuffer(); - if (!buf) - { - return -ErrMemory; - } - - { - BitStream bitstream(*buf); - ScalarCodec codec(bitstream); - const int encode_res = DataStruct::encode(message, codec); - if (encode_res <= 0) - { - assert(0); // Impossible, internal error - return -ErrInvalidMarshalData; - } - } - if (tid) - { - return sender_->send(buf->getDataPtr(), buf->getDataLength(), getTxDeadline(), - blocking_deadline, transfer_type, dst_node_id, *tid); - } - else - { - return sender_->send(buf->getDataPtr(), buf->getDataLength(), getTxDeadline(), - blocking_deadline, transfer_type, dst_node_id); - } - } - -public: - GenericPublisher(INode& node, MonotonicDuration tx_timeout, - MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval()) +protected: + GenericPublisherBase(INode& node, MonotonicDuration tx_timeout, + MonotonicDuration max_transfer_interval) : max_transfer_interval_(max_transfer_interval) , tx_timeout_(tx_timeout) , node_(node) @@ -115,6 +37,58 @@ public: #endif } + ~GenericPublisherBase() { } + + bool isInited() const; + + int doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos); + + MonotonicTime getTxDeadline() const; + + IMarshalBuffer* getBuffer(unsigned byte_len); + + int genericPublish(const IMarshalBuffer& buffer, TransferType transfer_type, NodeID dst_node_id, + TransferID* tid, MonotonicTime blocking_deadline); + + TransferSender* getTransferSender(); + +public: + static MonotonicDuration getMinTxTimeout() { return MonotonicDuration::fromUSec(200); } + static MonotonicDuration getMaxTxTimeout() { return MonotonicDuration::fromMSec(60000); } + + MonotonicDuration getTxTimeout() const { return tx_timeout_; } + void setTxTimeout(MonotonicDuration tx_timeout); + + INode& getNode() const { return node_; } +}; + +/** + * Generic publisher, suitable for messages and services. + * DataSpec - data type specification class + * DataStruct - instantiable class + */ +template +class UAVCAN_EXPORT GenericPublisher : public GenericPublisherBase +{ + enum + { + Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ? + CanTxQueue::Volatile : CanTxQueue::Persistent + }; + + int checkInit(); + + int doEncode(const DataStruct& message, IMarshalBuffer& buffer) const; + + int genericPublish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id, + TransferID* tid, MonotonicTime blocking_deadline); + +public: + GenericPublisher(INode& node, MonotonicDuration tx_timeout, + MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval()) + : GenericPublisherBase(node, tx_timeout, max_transfer_interval) + { } + ~GenericPublisher() { } int init() @@ -137,21 +111,57 @@ public: TransferSender* getTransferSender() { (void)checkInit(); - return sender_.isConstructed() ? static_cast(sender_) : NULL; + return GenericPublisherBase::getTransferSender(); } - - static MonotonicDuration getMinTxTimeout() { return MonotonicDuration::fromUSec(200); } - static MonotonicDuration getMaxTxTimeout() { return MonotonicDuration::fromMSec(60000); } - - MonotonicDuration getTxTimeout() const { return tx_timeout_; } - void setTxTimeout(MonotonicDuration tx_timeout) - { - tx_timeout = std::max(tx_timeout, getMinTxTimeout()); - tx_timeout = std::min(tx_timeout, getMaxTxTimeout()); - tx_timeout_ = tx_timeout; - } - - INode& getNode() const { return node_; } }; +// ---------------------------------------------------------------------------- + +template +int GenericPublisher::checkInit() +{ + if (isInited()) + { + return 0; + } + return doInit(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName(), CanTxQueue::Qos(Qos)); +} + +template +int GenericPublisher::doEncode(const DataStruct& message, IMarshalBuffer& buffer) const +{ + BitStream bitstream(buffer); + ScalarCodec codec(bitstream); + const int encode_res = DataStruct::encode(message, codec); + if (encode_res <= 0) + { + assert(0); // Impossible, internal error + return -ErrInvalidMarshalData; + } + return encode_res; +} + +template +int GenericPublisher::genericPublish(const DataStruct& message, TransferType transfer_type, + NodeID dst_node_id, TransferID* tid, + MonotonicTime blocking_deadline) +{ + const int res = checkInit(); + if (res < 0) + { + return res; + } + IMarshalBuffer* const buf = getBuffer(BitLenToByteLen::Result); + if (!buf) + { + return -ErrMemory; + } + const int encode_res = doEncode(message, *buf); + if (encode_res < 0) + { + return encode_res; + } + return GenericPublisherBase::genericPublish(*buf, transfer_type, dst_node_id, tid, blocking_deadline); +} + } diff --git a/libuavcan/src/node/uc_generic_publisher.cpp b/libuavcan/src/node/uc_generic_publisher.cpp new file mode 100644 index 0000000000..94f2255970 --- /dev/null +++ b/libuavcan/src/node/uc_generic_publisher.cpp @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ + +bool GenericPublisherBase::isInited() const +{ + return bool(sender_); +} + +int GenericPublisherBase::doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos) +{ + if (isInited()) + { + return 0; + } + + GlobalDataTypeRegistry::instance().freeze(); + + const DataTypeDescriptor* const descr = GlobalDataTypeRegistry::instance().find(dtkind, dtname); + if (!descr) + { + UAVCAN_TRACE("GenericPublisher", "Type [%s] is not registered", dtname); + return -ErrUnknownDataType; + } + sender_.construct + (node_.getDispatcher(), *descr, qos, max_transfer_interval_); + return 0; +} + +MonotonicTime GenericPublisherBase::getTxDeadline() const +{ + return node_.getMonotonicTime() + tx_timeout_; +} + +IMarshalBuffer* GenericPublisherBase::getBuffer(unsigned byte_len) +{ + return node_.getMarshalBufferProvider().getBuffer(byte_len); +} + +int GenericPublisherBase::genericPublish(const IMarshalBuffer& buffer, TransferType transfer_type, NodeID dst_node_id, + TransferID* tid, MonotonicTime blocking_deadline) +{ + if (tid) + { + return sender_->send(buffer.getDataPtr(), buffer.getDataLength(), getTxDeadline(), + blocking_deadline, transfer_type, dst_node_id, *tid); + } + else + { + return sender_->send(buffer.getDataPtr(), buffer.getDataLength(), getTxDeadline(), + blocking_deadline, transfer_type, dst_node_id); + } +} + +TransferSender* GenericPublisherBase::getTransferSender() +{ + return sender_.isConstructed() ? static_cast(sender_) : NULL; +} + +void GenericPublisherBase::setTxTimeout(MonotonicDuration tx_timeout) +{ + tx_timeout = std::max(tx_timeout, getMinTxTimeout()); + tx_timeout = std::min(tx_timeout, getMaxTxTimeout()); + tx_timeout_ = tx_timeout; +} + +}