uc_scheduler.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan/node/scheduler.hpp>
00006 #include <uavcan/debug.hpp>
00007 #include <cassert>
00008 
00009 namespace uavcan
00010 {
00011 /*
00012  * MonotonicDeadlineHandler
00013  */
00014 void DeadlineHandler::startWithDeadline(MonotonicTime deadline)
00015 {
00016     UAVCAN_ASSERT(!deadline.isZero());
00017     stop();
00018     deadline_ = deadline;
00019     scheduler_.getDeadlineScheduler().add(this);
00020 }
00021 
00022 void DeadlineHandler::startWithDelay(MonotonicDuration delay)
00023 {
00024     startWithDeadline(scheduler_.getMonotonicTime() + delay);
00025 }
00026 
00027 void DeadlineHandler::stop()
00028 {
00029     scheduler_.getDeadlineScheduler().remove(this);
00030 }
00031 
00032 bool DeadlineHandler::isRunning() const
00033 {
00034     return scheduler_.getDeadlineScheduler().doesExist(this);
00035 }
00036 
00037 /*
00038  * MonotonicDeadlineScheduler
00039  */
00040 struct MonotonicDeadlineHandlerInsertionComparator
00041 {
00042     const MonotonicTime ts;
00043     explicit MonotonicDeadlineHandlerInsertionComparator(MonotonicTime arg_ts) : ts(arg_ts) { }
00044     bool operator()(const DeadlineHandler* t) const
00045     {
00046         return t->getDeadline() > ts;
00047     }
00048 };
00049 
00050 void DeadlineScheduler::add(DeadlineHandler* mdh)
00051 {
00052     UAVCAN_ASSERT(mdh);
00053     handlers_.insertBefore(mdh, MonotonicDeadlineHandlerInsertionComparator(mdh->getDeadline()));
00054 }
00055 
00056 void DeadlineScheduler::remove(DeadlineHandler* mdh)
00057 {
00058     UAVCAN_ASSERT(mdh);
00059     handlers_.remove(mdh);
00060 }
00061 
00062 bool DeadlineScheduler::doesExist(const DeadlineHandler* mdh) const
00063 {
00064     UAVCAN_ASSERT(mdh);
00065     const DeadlineHandler* p = handlers_.get();
00066 #if UAVCAN_DEBUG
00067     MonotonicTime prev_deadline;
00068 #endif
00069     while (p)
00070     {
00071 #if UAVCAN_DEBUG
00072         if (prev_deadline > p->getDeadline())  // Self check
00073         {
00074             std::abort();
00075         }
00076         prev_deadline = p->getDeadline();
00077 #endif
00078         if (p == mdh)
00079         {
00080             return true;
00081         }
00082         p = p->getNextListNode();
00083     }
00084     return false;
00085 }
00086 
00087 MonotonicTime DeadlineScheduler::pollAndGetMonotonicTime(ISystemClock& sysclock)
00088 {
00089     while (true)
00090     {
00091         DeadlineHandler* const mdh = handlers_.get();
00092         if (!mdh)
00093         {
00094             return sysclock.getMonotonic();
00095         }
00096 #if UAVCAN_DEBUG
00097         if (mdh->getNextListNode())      // Order check
00098         {
00099             UAVCAN_ASSERT(mdh->getDeadline() <= mdh->getNextListNode()->getDeadline());
00100         }
00101 #endif
00102 
00103         const MonotonicTime ts = sysclock.getMonotonic();
00104         if (ts < mdh->getDeadline())
00105         {
00106             return ts;
00107         }
00108 
00109         handlers_.remove(mdh);
00110         mdh->handleDeadline(ts);   // This handler can be re-registered immediately
00111     }
00112     UAVCAN_ASSERT(0);
00113     return MonotonicTime();
00114 }
00115 
00116 MonotonicTime DeadlineScheduler::getEarliestDeadline() const
00117 {
00118     const DeadlineHandler* const mdh = handlers_.get();
00119     if (mdh)
00120     {
00121         return mdh->getDeadline();
00122     }
00123     return MonotonicTime::getMax();
00124 }
00125 
00126 /*
00127  * Scheduler
00128  */
00129 MonotonicTime Scheduler::computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const
00130 {
00131     const MonotonicTime earliest = min(deadline_scheduler_.getEarliestDeadline(), spin_deadline);
00132     const MonotonicTime ts = getMonotonicTime();
00133     if (earliest > ts)
00134     {
00135         if (earliest - ts > deadline_resolution_)
00136         {
00137             return ts + deadline_resolution_;
00138         }
00139     }
00140     return earliest;
00141 }
00142 
00143 void Scheduler::pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin)
00144 {
00145     // cleanup will be performed less frequently if the stack handles more frames per second
00146     const MonotonicTime deadline = prev_cleanup_ts_ + cleanup_period_ * (num_frames_processed_with_last_spin + 1);
00147     if (mono_ts > deadline)
00148     {
00149         //UAVCAN_TRACE("Scheduler", "Cleanup with %u processed frames", num_frames_processed_with_last_spin);
00150         prev_cleanup_ts_ = mono_ts;
00151         dispatcher_.cleanup(mono_ts);
00152     }
00153 }
00154 
00155 int Scheduler::spin(MonotonicTime deadline)
00156 {
00157     if (inside_spin_)  // Preventing recursive calls
00158     {
00159         UAVCAN_ASSERT(0);
00160         return -ErrRecursiveCall;
00161     }
00162     InsideSpinSetter iss(*this);
00163     UAVCAN_ASSERT(inside_spin_);
00164 
00165     int retval = 0;
00166     while (true)
00167     {
00168         const MonotonicTime dl = computeDispatcherSpinDeadline(deadline);
00169         retval = dispatcher_.spin(dl);
00170         if (retval < 0)
00171         {
00172             break;
00173         }
00174 
00175         const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock());
00176         pollCleanup(ts, unsigned(retval));
00177         if (ts >= deadline)
00178         {
00179             break;
00180         }
00181     }
00182 
00183     return retval;
00184 }
00185 
00186 int Scheduler::spinOnce()
00187 {
00188     if (inside_spin_)  // Preventing recursive calls
00189     {
00190         UAVCAN_ASSERT(0);
00191         return -ErrRecursiveCall;
00192     }
00193     InsideSpinSetter iss(*this);
00194     UAVCAN_ASSERT(inside_spin_);
00195 
00196     const int retval = dispatcher_.spinOnce();
00197     if (retval < 0)
00198     {
00199         return retval;
00200     }
00201 
00202     const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock());
00203     pollCleanup(ts, unsigned(retval));
00204 
00205     return retval;
00206 }
00207 
00208 }