Refactoring - Scheduler, GenericPublisher, GenericSubscruber moved into /uavcan/internal/node/*

2026-07-19 02:50:35 +08:00 · 2014-03-10 13:53:10 +04:00
parent 6573d79fd1
commit 4b92497aee
10 changed files with 372 additions and 261 deletions
@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/internal/node/scheduler.hpp>
+#include <uavcan/data_type.hpp>
+#include <uavcan/internal/node/marshal_buffer.hpp>
+#include <uavcan/global_data_type_registry.hpp>
+#include <uavcan/util/lazy_constructor.hpp>
+#include <uavcan/internal/debug.hpp>
+#include <uavcan/internal/transport/transfer_sender.hpp>
+#include <uavcan/internal/marshal/scalar_codec.hpp>
+#include <uavcan/internal/marshal/types.hpp>
+
+namespace uavcan
+{
+
+/**
+ * Generic publisher, suitable for messages and services.
+ * DataSpec - data type specification class
+ * DataStruct - instantiable class
+ */
+template <typename DataSpec, typename DataStruct>
+class GenericPublisher
+{
+public:
+    enum { DefaultTxTimeoutUsec = 2500 };    // 2500 ms --> 400Hz max
+    enum { MinTxTimeoutUsec = 200 };
+
+private:
+    enum { Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ?
+                  CanTxQueue::Volatile : CanTxQueue::Persistent };
+
+    const uint64_t max_transfer_interval_;   // TODO: memory usage can be reduced
+    uint64_t tx_timeout_;
+    Scheduler& scheduler_;
+    IMarshalBufferProvider& buffer_provider_;
+    LazyConstructor<TransferSender> sender_;
+
+    bool checkInit()
+    {
+        if (sender_)
+            return true;
+
+        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 false;
+        }
+        sender_.template construct<Dispatcher&, const DataTypeDescriptor&, CanTxQueue::Qos, uint64_t>
+            (scheduler_.getDispatcher(), *descr, CanTxQueue::Qos(Qos), max_transfer_interval_);
+        return true;
+    }
+
+    uint64_t getTxDeadline() const { return scheduler_.getMonotonicTimestamp() + tx_timeout_; }
+
+    IMarshalBuffer* getBuffer()
+    {
+        return buffer_provider_.getBuffer(BitLenToByteLen<DataStruct::MaxBitLen>::Result);
+    }
+
+    int genericPublish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id,
+                       TransferID* tid, uint64_t monotonic_blocking_deadline)
+    {
+        if (!checkInit())
+            return -1;
+
+        IMarshalBuffer* const buf = getBuffer();
+        if (!buf)
+            return -1;
+
+        {
+            BitStream bitstream(*buf);
+            ScalarCodec codec(bitstream);
+            const int encode_res = DataStruct::encode(message, codec);
+            if (encode_res <= 0)
+            {
+                assert(0);   // Impossible, internal error
+                return -1;
+            }
+        }
+        if (tid)
+        {
+            return sender_->send(buf->getDataPtr(), buf->getDataLength(), getTxDeadline(),
+                                 monotonic_blocking_deadline, transfer_type, dst_node_id, *tid);
+        }
+        else
+        {
+            return sender_->send(buf->getDataPtr(), buf->getDataLength(), getTxDeadline(),
+                                 monotonic_blocking_deadline, transfer_type, dst_node_id);
+        }
+    }
+
+protected:
+    GenericPublisher(Scheduler& scheduler, IMarshalBufferProvider& buffer_provider,
+                     uint64_t max_transfer_interval = TransferSender::DefaultMaxTransferInterval)
+    : max_transfer_interval_(max_transfer_interval)
+    , tx_timeout_(DefaultTxTimeoutUsec)
+    , scheduler_(scheduler)
+    , buffer_provider_(buffer_provider)
+    { }
+
+    ~GenericPublisher() { }
+
+    int publish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id,
+                uint64_t monotonic_blocking_deadline = 0)
+    {
+        return genericPublish(message, transfer_type, dst_node_id, NULL, monotonic_blocking_deadline);
+    }
+
+    int publish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id, TransferID tid,
+                uint64_t monotonic_blocking_deadline = 0)
+    {
+        return genericPublish(message, transfer_type, dst_node_id, &tid, monotonic_blocking_deadline);
+    }
+
+public:
+    uint64_t getTxTimeout() const { return tx_timeout_; }
+    void setTxTimeout(uint64_t usec)
+    {
+        tx_timeout_ = std::max(usec, uint64_t(MinTxTimeoutUsec));
+    }
+
+    Scheduler& getScheduler() const { return scheduler_; }
+};
+
+}
@@ -0,0 +1,205 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/internal/node/scheduler.hpp>
+#include <uavcan/data_type.hpp>
+#include <uavcan/global_data_type_registry.hpp>
+#include <uavcan/util/compile_time.hpp>
+#include <uavcan/util/lazy_constructor.hpp>
+#include <uavcan/internal/debug.hpp>
+#include <uavcan/internal/transport/transfer_listener.hpp>
+#include <uavcan/internal/marshal/scalar_codec.hpp>
+#include <uavcan/internal/marshal/types.hpp>
+
+namespace uavcan
+{
+
+template <typename DataType_>
+class ReceivedDataStructure : public DataType_
+{
+    const IncomingTransfer* transfer_;
+
+    template <typename Ret, Ret (IncomingTransfer::*Fun)() const>
+    Ret safeget() const
+    {
+        if (!transfer_)
+        {
+            assert(0);
+            return Ret();
+        }
+        return (transfer_->*Fun)();
+    }
+
+protected:
+    ReceivedDataStructure() : transfer_(NULL) { }
+
+    void setTransfer(const IncomingTransfer* transfer)
+    {
+        assert(transfer);
+        transfer_ = transfer;
+    }
+
+public:
+    typedef DataType_ DataType;
+
+    uint64_t getMonotonicTimestamp() const { return safeget<uint64_t, &IncomingTransfer::getMonotonicTimestamp>(); }
+    uint64_t getUtcTimestamp()       const { return safeget<uint64_t, &IncomingTransfer::getUtcTimestamp>(); }
+    TransferType getTransferType()   const { return safeget<TransferType, &IncomingTransfer::getTransferType>(); }
+    TransferID getTransferID()       const { return safeget<TransferID, &IncomingTransfer::getTransferID>(); }
+    NodeID getSrcNodeID()            const { return safeget<NodeID, &IncomingTransfer::getSrcNodeID>(); }
+};
+
+
+template <typename DataSpec, typename DataStruct, unsigned int NumStaticReceivers, unsigned int NumStaticBufs_>
+class GenericSubscriber : Noncopyable
+{
+    typedef GenericSubscriber<DataSpec, DataStruct, NumStaticReceivers, NumStaticBufs_> SelfType;
+
+    enum { DataTypeMaxByteLen = BitLenToByteLen<DataStruct::MaxBitLen>::Result };
+    enum { NeedsBuffer = int(DataTypeMaxByteLen) > int(MaxSingleFrameTransferPayloadLen) };
+    enum { BufferSize = NeedsBuffer ? DataTypeMaxByteLen : 0 };
+    enum { NumStaticBufs = NeedsBuffer ? (NumStaticBufs_ ? NumStaticBufs_ : 1) : 0 };
+
+    typedef TransferListener<BufferSize, NumStaticBufs,  // TODO: support for zero static bufs
+                             NumStaticReceivers ? NumStaticReceivers : 1> TransferListenerType;
+
+    // We need to break the inheritance chain here to implement lazy initialization
+    class TransferForwarder : public TransferListenerType
+    {
+        SelfType& obj_;
+
+        void handleIncomingTransfer(IncomingTransfer& transfer)
+        {
+            obj_.handleIncomingTransfer(transfer);
+        }
+
+    public:
+        TransferForwarder(SelfType& obj, const DataTypeDescriptor& data_type, IAllocator& allocator)
+        : TransferListenerType(data_type, allocator)
+        , obj_(obj)
+        { }
+    };
+
+    struct ReceivedDataStructureSpec : public ReceivedDataStructure<DataStruct>
+    {
+        using ReceivedDataStructure<DataStruct>::setTransfer;
+    };
+
+    Scheduler& scheduler_;
+    IAllocator& allocator_;
+    LazyConstructor<TransferForwarder> forwarder_;
+    ReceivedDataStructureSpec message_;
+    uint32_t failure_count_;
+
+    bool checkInit()
+    {
+        if (forwarder_)
+            return true;
+
+        GlobalDataTypeRegistry::instance().freeze();
+
+        const DataTypeDescriptor* const descr =
+            GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind),
+                                                    DataSpec::getDataTypeFullName());
+        if (!descr)
+        {
+            UAVCAN_TRACE("GenericSubscriber", "Type [%s] is not registered", DataSpec::getDataTypeFullName());
+            return false;
+        }
+        forwarder_.template construct<SelfType&, const DataTypeDescriptor&, IAllocator&>(*this, *descr, allocator_);
+        return true;
+    }
+
+    bool decodeTransfer(IncomingTransfer& transfer)
+    {
+        BitStream bitstream(transfer);
+        ScalarCodec codec(bitstream);
+
+        message_.setTransfer(&transfer);
+
+        const int decode_res = DataSpec::decode(message_, codec);
+        // We don't need the data anymore, the memory can be reused from the callback:
+        transfer.release();
+        if (decode_res <= 0)
+        {
+            UAVCAN_TRACE("GenericSubscriber", "Unable to decode the message [%i] [%s]",
+                         decode_res, DataSpec::getDataTypeFullName());
+            failure_count_++;
+            return false;
+        }
+        return true;
+    }
+
+    void handleIncomingTransfer(IncomingTransfer& transfer)
+    {
+        if (decodeTransfer(transfer))
+        {
+            handleReceivedDataStruct(message_);
+        }
+    }
+
+    int genericStart(bool(Dispatcher::*registration_method)(TransferListenerBase* listener))
+    {
+        stop();
+
+        if (!checkInit())
+        {
+            UAVCAN_TRACE("GenericSubscriber", "Initialization failure [%s]", DataSpec::getDataTypeFullName());
+            return -1;
+        }
+
+        if (!(scheduler_.getDispatcher().*registration_method)(forwarder_))
+        {
+            UAVCAN_TRACE("GenericSubscriber", "Failed to register transfer listener [%s]",
+                         DataSpec::getDataTypeFullName());
+            return -1;
+        }
+        return 1;
+    }
+
+protected:
+    GenericSubscriber(Scheduler& scheduler, IAllocator& allocator)
+    : scheduler_(scheduler)
+    , allocator_(allocator)
+    , failure_count_(0)
+    { }
+
+    virtual ~GenericSubscriber() { stop(); }
+
+    virtual void handleReceivedDataStruct(ReceivedDataStructure<DataStruct>&) = 0;
+
+    int startAsMessageListener()
+    {
+        return genericStart(&Dispatcher::registerMessageListener);
+    }
+
+    int startAsServiceRequestListener()
+    {
+        return genericStart(&Dispatcher::registerServiceRequestListener);
+    }
+
+    int startAsServiceResponseListener()
+    {
+        return genericStart(&Dispatcher::registerServiceResponseListener);
+    }
+
+    void stop()
+    {
+        if (forwarder_)
+        {
+            scheduler_.getDispatcher().unregisterMessageListener(forwarder_);
+            scheduler_.getDispatcher().unregisterServiceRequestListener(forwarder_);
+            scheduler_.getDispatcher().unregisterServiceResponseListener(forwarder_);
+        }
+    }
+
+public:
+    Scheduler& getScheduler() const { return scheduler_; }
+
+    uint32_t getFailureCount() const { return failure_count_; }
+};
+
+}
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/internal/transport/transfer.hpp>
+#include <uavcan/internal/transport/transfer_buffer.hpp>
+
+namespace uavcan
+{
+
+class IMarshalBuffer : public ITransferBuffer
+{
+public:
+    virtual const uint8_t* getDataPtr() const = 0;
+    virtual unsigned int getDataLength() const = 0;
+};
+
+
+class IMarshalBufferProvider
+{
+public:
+    virtual ~IMarshalBufferProvider() { }
+    virtual IMarshalBuffer* getBuffer(unsigned int size) = 0;
+};
+
+
+template <unsigned int MaxSize_ = MaxTransferPayloadLen>
+class MarshalBufferProvider : public IMarshalBufferProvider
+{
+    class Buffer : public IMarshalBuffer
+    {
+        StaticTransferBuffer<MaxSize_> buf_;
+
+        int read(unsigned int offset, uint8_t* data, unsigned int len) const
+        {
+            return buf_.read(offset, data, len);
+        }
+
+        int write(unsigned int offset, const uint8_t* data, unsigned int len)
+        {
+            return buf_.write(offset, data, len);
+        }
+
+        const uint8_t* getDataPtr() const { return buf_.getRawPtr(); }
+
+        unsigned int getDataLength() const { return buf_.getMaxWritePos(); }
+
+    public:
+        void reset() { buf_.reset(); }
+    };
+
+    Buffer buffer_;
+
+public:
+    enum { MaxSize = MaxSize_ };
+
+    IMarshalBuffer* getBuffer(unsigned int size)
+    {
+        if (size > MaxSize)
+            return NULL;
+        buffer_.reset();
+        return &buffer_;
+    }
+};
+
+}
@@ -0,0 +1,113 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/internal/linked_list.hpp>
+#include <uavcan/internal/transport/dispatcher.hpp>
+
+namespace uavcan
+{
+
+class Scheduler;
+
+class MonotonicDeadlineHandler : public LinkedListNode<MonotonicDeadlineHandler>, Noncopyable
+{
+    uint64_t monotonic_deadline_;
+
+protected:
+    Scheduler& scheduler_;
+
+    explicit MonotonicDeadlineHandler(Scheduler& scheduler)
+    : monotonic_deadline_(0)
+    , scheduler_(scheduler)
+    { }
+
+    virtual ~MonotonicDeadlineHandler() { stop(); }
+
+public:
+    virtual void onMonotonicDeadline(uint64_t monotonic_timestamp) = 0;
+
+    void startWithDeadline(uint64_t monotonic_deadline);
+    void startWithDelay(uint64_t delay_usec);
+
+    void stop();
+
+    bool isRunning() const;
+
+    uint64_t getMonotonicDeadline() const { return monotonic_deadline_; }
+    Scheduler& getScheduler() const { return scheduler_; }
+};
+
+
+class MonotonicDeadlineScheduler : Noncopyable
+{
+    LinkedListRoot<MonotonicDeadlineHandler> handlers_;  // Ordered by deadline, lowest first
+
+public:
+    void add(MonotonicDeadlineHandler* mdh);
+    void remove(MonotonicDeadlineHandler* mdh);
+    bool doesExist(const MonotonicDeadlineHandler* mdh) const;
+    unsigned int getNumHandlers() const { return handlers_.getLength(); }
+
+    uint64_t pollAndGetMonotonicTimestamp(ISystemClock& sysclock);
+    uint64_t getEarliestDeadline() const;
+};
+
+
+class Scheduler : Noncopyable
+{
+    enum { DefaultMonotonicDeadlineResolutionMs = 5 };
+    enum { MinMonotonicDeadlineResolutionMs = 1 };
+    enum { MaxMonotonicDeadlineResolutionMs = 100 };
+
+    enum { DefaultCleanupPeriodMs = 1000 };
+    enum { MinCleanupPeriodMs = 10 };
+    enum { MaxCleanupPeriodMs = 10000 };
+
+    MonotonicDeadlineScheduler deadline_scheduler_;
+    Dispatcher dispatcher_;
+    uint64_t prev_cleanup_ts_;
+    uint64_t monotonic_deadline_resolution_;
+    uint64_t cleanup_period_;
+
+    uint64_t computeDispatcherSpinDeadline(uint64_t spin_deadline) const;
+    void pollCleanup(uint64_t mono_ts, uint32_t num_frames_processed_with_last_spin);
+
+public:
+    Scheduler(ICanDriver& can_driver, IAllocator& allocator, ISystemClock& sysclock, IOutgoingTransferRegistry& otr,
+             NodeID self_node_id)
+    : dispatcher_(can_driver, allocator, sysclock, otr, self_node_id)
+    , prev_cleanup_ts_(sysclock.getMonotonicMicroseconds())
+    , monotonic_deadline_resolution_(DefaultMonotonicDeadlineResolutionMs * 1000)
+    , cleanup_period_(DefaultCleanupPeriodMs * 1000)
+    { }
+
+    int spin(uint64_t monotonic_deadline);
+
+    MonotonicDeadlineScheduler& getMonotonicDeadlineScheduler() { return deadline_scheduler_; }
+    Dispatcher& getDispatcher() { return dispatcher_; }
+
+    ISystemClock& getSystemClock()         { return dispatcher_.getSystemClock(); }
+    uint64_t getMonotonicTimestamp() const { return dispatcher_.getSystemClock().getMonotonicMicroseconds(); }
+    uint64_t getUtcTimestamp()       const { return dispatcher_.getSystemClock().getUtcMicroseconds(); }
+
+    uint64_t getMonotonicDeadlineResolution() const { return monotonic_deadline_resolution_; }
+    void setMonotonicDeadlineResolution(uint64_t res_usec)
+    {
+        res_usec = std::min(res_usec, MaxMonotonicDeadlineResolutionMs * uint64_t(1000));
+        res_usec = std::max(res_usec, MinMonotonicDeadlineResolutionMs * uint64_t(1000));
+        monotonic_deadline_resolution_ = res_usec;
+    }
+
+    uint64_t getCleanupPeriod() const { return cleanup_period_; }
+    void setCleanupPeriod(uint64_t period_usec)
+    {
+        period_usec = std::min(period_usec, MaxCleanupPeriodMs * uint64_t(1000));
+        period_usec = std::max(period_usec, MinCleanupPeriodMs * uint64_t(1000));
+        cleanup_period_ = period_usec;
+    }
+};
+
+}