Reorganized directory structure - directory 'internal' pulled up one level. No logical changes.

2026-10-21 23:33:34 +08:00 · 2014-03-14 15:01:20 +04:00
parent ab34c94ba2
commit 15cbf96378
90 changed files with 165 additions and 166 deletions
@@ -0,0 +1,206 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cassert>
+#include <cstdlib>
+#include <uavcan/node/global_data_type_registry.hpp>
+#include <uavcan/debug.hpp>
+
+namespace uavcan
+{
+
+const GlobalDataTypeRegistry::List* GlobalDataTypeRegistry::selectList(DataTypeKind kind) const
+{
+    switch (kind)
+    {
+    case DataTypeKindMessage: return &msgs_;
+    case DataTypeKindService: return &srvs_;
+    default:
+        assert(0);
+        return NULL;
+    }
+}
+
+GlobalDataTypeRegistry::List* GlobalDataTypeRegistry::selectList(DataTypeKind kind)
+{
+    switch (kind)
+    {
+    case DataTypeKindMessage: return &msgs_;
+    case DataTypeKindService: return &srvs_;
+    default:
+        assert(0);
+        return NULL;
+    }
+}
+
+GlobalDataTypeRegistry::RegistResult GlobalDataTypeRegistry::remove(Entry* dtd)
+{
+    if (!dtd)
+    {
+        assert(0);
+        return RegistResultInvalidParams;
+    }
+    if (isFrozen())
+        return RegistResultFrozen;
+
+    List* list = selectList(dtd->descriptor.getKind());
+    if (!list)
+        return RegistResultInvalidParams;
+
+    list->remove(dtd);       // If this call came from regist<>(), that would be enough
+    Entry* p = list->get();  // But anyway
+    while (p)
+    {
+        Entry* const next = p->getNextListNode();
+        if (p->descriptor.match(dtd->descriptor.getKind(), dtd->descriptor.getFullName()))
+            list->remove(p);
+        p = next;
+    }
+    return RegistResultOk;
+}
+
+GlobalDataTypeRegistry::RegistResult GlobalDataTypeRegistry::registImpl(Entry* dtd)
+{
+    if (!dtd || (dtd->descriptor.getID() > DataTypeID::Max))
+    {
+        assert(0);
+        return RegistResultInvalidParams;
+    }
+    if (isFrozen())
+        return RegistResultFrozen;
+
+    List* list = selectList(dtd->descriptor.getKind());
+    if (!list)
+        return RegistResultInvalidParams;
+
+    {   // Collision check
+        Entry* p = list->get();
+        while (p)
+        {
+            if (p->descriptor.getID() == dtd->descriptor.getID()) // ID collision
+                return RegistResultCollision;
+            if (!std::strcmp(p->descriptor.getFullName(), dtd->descriptor.getFullName())) // Name collision
+                return RegistResultCollision;
+            p = p->getNextListNode();
+        }
+    }
+    list->insertBefore(dtd, EntryInsertionComparator(dtd));
+
+#if UAVCAN_DEBUG
+    {   // Order check
+        Entry* p = list->get();
+        int id = -1;
+        while (p)
+        {
+            if (id >= p->descriptor.getID().get())
+            {
+                assert(0);
+                std::abort();
+            }
+            id = p->descriptor.getID().get();
+            p = p->getNextListNode();
+        }
+    }
+#endif
+    return RegistResultOk;
+}
+
+GlobalDataTypeRegistry& GlobalDataTypeRegistry::instance()
+{
+    static GlobalDataTypeRegistry inst;
+    return inst;
+}
+
+void GlobalDataTypeRegistry::freeze()
+{
+    if (!frozen_)
+    {
+        frozen_ = true;
+        UAVCAN_TRACE("GlobalDataTypeRegistry", "Frozen; num msgs: %u, num srvs: %u",
+                     getNumMessageTypes(), getNumServiceTypes());
+    }
+}
+
+const DataTypeDescriptor* GlobalDataTypeRegistry::find(DataTypeKind kind, const char* name) const
+{
+    const List* list = selectList(kind);
+    if (!list)
+    {
+        assert(0);
+        return NULL;
+    }
+    Entry* p = list->get();
+    while (p)
+    {
+        if (p->descriptor.match(kind, name))
+            return &p->descriptor;
+        p = p->getNextListNode();
+    }
+    return NULL;
+}
+
+DataTypeSignature GlobalDataTypeRegistry::computeAggregateSignature(DataTypeKind kind,
+                                                                    DataTypeIDMask& inout_id_mask) const
+{
+    assert(isFrozen());  // Computing the signature if the registry is not frozen is pointless
+
+    const List* list = selectList(kind);
+    if (!list)
+    {
+        assert(0);
+        return DataTypeSignature();
+    }
+
+    int prev_dtid = -1;
+    DataTypeSignature signature;
+    bool signature_initialized = false;
+    Entry* p = list->get();
+    while (p)
+    {
+        const DataTypeDescriptor& desc = p->descriptor;
+        const int dtid = desc.getID().get();
+
+        if (inout_id_mask[dtid])
+        {
+            if (signature_initialized)
+                signature.extend(desc.getSignature());
+            else
+                signature = DataTypeSignature(desc.getSignature());
+            signature_initialized = true;
+        }
+
+        assert(prev_dtid < dtid);  // Making sure that list is ordered properly
+        prev_dtid++;
+        while (prev_dtid < dtid)
+            inout_id_mask[prev_dtid++] = false; // Erasing bits for missing types
+        assert(prev_dtid == dtid);
+
+        p = p->getNextListNode();
+    }
+    prev_dtid++;
+    while (prev_dtid <= DataTypeID::Max)
+        inout_id_mask[prev_dtid++] = false;
+
+    return signature;
+}
+
+void GlobalDataTypeRegistry::getDataTypeIDMask(DataTypeKind kind, DataTypeIDMask& mask) const
+{
+    mask.reset();
+    const List* list = selectList(kind);
+    if (!list)
+    {
+        assert(0);
+        return;
+    }
+    Entry* p = list->get();
+    while (p)
+    {
+        assert(p->descriptor.getKind() == kind);
+        mask[p->descriptor.getID().get()] = true;
+        p = p->getNextListNode();
+    }
+}
+
+}
@@ -0,0 +1,159 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cassert>
+#include <uavcan/node/scheduler.hpp>
+#include <uavcan/debug.hpp>
+
+namespace uavcan
+{
+/*
+ * MonotonicDeadlineHandler
+ */
+void DeadlineHandler::startWithDeadline(MonotonicTime deadline)
+{
+    assert(!deadline.isZero());
+    stop();
+    deadline_ = deadline;
+    scheduler_.getDeadlineScheduler().add(this);
+}
+
+void DeadlineHandler::startWithDelay(MonotonicDuration delay)
+{
+    startWithDeadline(scheduler_.getMonotonicTimestamp() + delay);
+}
+
+void DeadlineHandler::stop()
+{
+    scheduler_.getDeadlineScheduler().remove(this);
+}
+
+bool DeadlineHandler::isRunning() const
+{
+    return scheduler_.getDeadlineScheduler().doesExist(this);
+}
+
+/*
+ * MonotonicDeadlineScheduler
+ */
+struct MonotonicDeadlineHandlerInsertionComparator
+{
+    const MonotonicTime ts;
+    MonotonicDeadlineHandlerInsertionComparator(MonotonicTime ts) : ts(ts) { }
+    bool operator()(const DeadlineHandler* t) const
+    {
+        return t->getDeadline() > ts;
+    }
+};
+
+void DeadlineScheduler::add(DeadlineHandler* mdh)
+{
+    assert(mdh);
+    handlers_.insertBefore(mdh, MonotonicDeadlineHandlerInsertionComparator(mdh->getDeadline()));
+}
+
+void DeadlineScheduler::remove(DeadlineHandler* mdh)
+{
+    assert(mdh);
+    handlers_.remove(mdh);
+}
+
+bool DeadlineScheduler::doesExist(const DeadlineHandler* mdh) const
+{
+    assert(mdh);
+    const DeadlineHandler* p = handlers_.get();
+#if UAVCAN_DEBUG
+    MonotonicTime prev_deadline;
+#endif
+    while (p)
+    {
+#if UAVCAN_DEBUG
+        if (prev_deadline > p->getDeadline())  // Self check
+            std::abort();
+        prev_deadline = p->getDeadline();
+#endif
+        if (p == mdh)
+            return true;
+        p = p->getNextListNode();
+    }
+    return false;
+}
+
+MonotonicTime DeadlineScheduler::pollAndGetMonotonicTimestamp(ISystemClock& sysclock)
+{
+    while (true)
+    {
+        DeadlineHandler* const mdh = handlers_.get();
+        if (!mdh)
+            return sysclock.getMonotonic();
+#if UAVCAN_DEBUG
+        if (mdh->getNextListNode())      // Order check
+            assert(mdh->getDeadline() <= mdh->getNextListNode()->getDeadline());
+#endif
+
+        const MonotonicTime ts = sysclock.getMonotonic();
+        if (ts < mdh->getDeadline())
+            return ts;
+
+        handlers_.remove(mdh);
+        mdh->handleDeadline(ts);   // This handler can be re-registered immediately
+    }
+    assert(0);
+    return MonotonicTime();
+}
+
+MonotonicTime DeadlineScheduler::getEarliestDeadline() const
+{
+    const DeadlineHandler* const mdh = handlers_.get();
+    if (mdh)
+        return mdh->getDeadline();
+    return MonotonicTime::getMax();
+}
+
+/*
+ * Scheduler
+ */
+MonotonicTime Scheduler::computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const
+{
+    const MonotonicTime earliest = std::min(deadline_scheduler_.getEarliestDeadline(), spin_deadline);
+    const MonotonicTime ts = getMonotonicTimestamp();
+    if (earliest > ts)
+    {
+        if (ts - earliest > deadline_resolution_)
+            return ts + deadline_resolution_;
+    }
+    return earliest;
+}
+
+void Scheduler::pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin)
+{
+    // cleanup will be performed less frequently if the stack handles more frames per second
+    const MonotonicTime deadline = prev_cleanup_ts_ + cleanup_period_ * (num_frames_processed_with_last_spin + 1);
+    if (mono_ts > deadline)
+    {
+        UAVCAN_TRACE("Scheduler", "Cleanup with %u processed frames", num_frames_processed_with_last_spin);
+        prev_cleanup_ts_ = mono_ts;
+        dispatcher_.cleanup(mono_ts);
+    }
+}
+
+int Scheduler::spin(MonotonicTime deadline)
+{
+    int retval = 0;
+    while (true)
+    {
+        const MonotonicTime dl = computeDispatcherSpinDeadline(deadline);
+        retval = dispatcher_.spin(dl);
+        if (retval < 0)
+            break;
+
+        const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTimestamp(getSystemClock());
+        pollCleanup(ts, retval);
+        if (ts >= deadline)
+            break;
+    }
+    return retval;
+}
+
+}
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cassert>
+#include <uavcan/node/timer.hpp>
+
+namespace uavcan
+{
+
+void Timer::handleDeadline(MonotonicTime current_timestamp)
+{
+    assert(!isRunning());
+
+    const MonotonicTime scheduled_time = getDeadline();
+
+    if (period_ < MonotonicDuration::getInfinite())
+        startWithDeadline(scheduled_time + period_);
+
+    // Application can re-register the timer with different params, it's OK
+    handleTimerEvent(TimerEvent(scheduled_time, current_timestamp));
+}
+
+void Timer::startOneShotWithDeadline(MonotonicTime deadline)
+{
+    stop();
+    period_ = MonotonicDuration::getInfinite();
+    DeadlineHandler::startWithDeadline(deadline);
+}
+
+void Timer::startOneShotWithDelay(MonotonicDuration delay)
+{
+    stop();
+    period_ = MonotonicDuration::getInfinite();
+    DeadlineHandler::startWithDelay(delay);
+}
+
+void Timer::startPeriodic(MonotonicDuration period)
+{
+    assert(period < MonotonicDuration::getInfinite());
+    stop();
+    period_ = period;
+    DeadlineHandler::startWithDelay(period);
+}
+
+}