Improve ArduControl task scheduling

The new scheduler can handle multiple tasks with the same priority, so
other things won't get blocked by the priority command queue for too
long.  It also handles sleeping at the scheduler rather than individual
tasks.

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 4acfc60e0e761a0c5aaa7d4c260a0023988a8034..40b3d35fd9f84693f233ad9490eb5013fa7bab57 100644 (file)
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -713,7 +713,24 @@ void ArduControl::CommandQueueTask::push(const PendingCommand &cmd)
 }
 
 
+ArduControl::Task::Task(const string &n, unsigned p):
+       name(n),
+       priority(p)
+{ }
+
+void ArduControl::Task::sleep(const Time::TimeDelta &dt)
+{
+       sleep_timeout = Time::now()+dt;
+}
+
+
+ArduControl::CommandQueueTask::CommandQueueTask():
+       Task("CommandQueue")
+{ }
+
+
 ArduControl::RefreshTask::RefreshTask():
+       Task("Refresh", 2),
        next(cycle.end()),
        round(0),
        loco(0),
@@ -807,19 +824,14 @@ void ArduControl::RefreshTask::advance()
 
 
 ArduControl::S88Task::S88Task(ArduControl &c):
+       Task("S88"),
        control(c),
        n_octets(0),
-       octets_remaining(0),
-       delay(0)
+       octets_remaining(0)
 { }
 
 bool ArduControl::S88Task::get_work(PendingCommand &cmd)
 {
-       if(delay)
-       {
-               --delay;
-               return false;
-       }
        if(octets_remaining || !n_octets)
                return false;
 
@@ -828,7 +840,7 @@ bool ArduControl::S88Task::get_work(PendingCommand &cmd)
        cmd.command[1] = octets_remaining;
        cmd.length = 2;
 
-       delay = 4;
+       sleep(100*Time::msec);
 
        return true;
 }
@@ -877,20 +889,18 @@ void ArduControl::S88Task::grow_n_octets(unsigned n)
 
 
 ArduControl::MfxAnnounceTask::MfxAnnounceTask():
+       Task("MfxAnnounce", 1),
        serial(0)
 { }
 
 bool ArduControl::MfxAnnounceTask::get_work(PendingCommand &cmd)
 {
-       Time::TimeStamp t = Time::now();
-       if(t<next)
-               return false;
-
        cmd.command[0] = MFX_ANNOUNCE;
        cmd.command[1] = serial>>8;
        cmd.command[2] = serial;
        cmd.length = 3;
-       next = t+400*Time::msec;
+
+       sleep(400*Time::msec);
 
        return true;
 }
@@ -902,6 +912,7 @@ void ArduControl::MfxAnnounceTask::set_serial(unsigned s)
 
 
 ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c):
+       Task("MfxSearch", 1),
        control(c),
        next_address(1),
        size(0),
@@ -941,17 +952,13 @@ bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
                return true;
        }
 
-       Time::TimeStamp t = Time::now();
-       if(t<next)
-               return false;
-
        cmd.command[0] = MFX_SEARCH;
        for(unsigned i=0; i<4; ++i)
                cmd.command[1+i] = bits>>(24-i*8);
        cmd.command[5] = size;
        cmd.length = 6;
 
-       next = t+200*Time::msec;
+       sleep(200*Time::msec);
 
        if(control.debug>=1)
                IO::print("Search %08X/%d\n", bits, size);
@@ -976,7 +983,7 @@ void ArduControl::MfxSearchTask::process_reply(const char *reply, unsigned lengt
                }
                else
                {
-                       next = Time::now()+2*Time::sec;
+                       sleep(2*Time::sec);
                        bits = 0;
                        size = 0;
                        misses = 0;
@@ -996,6 +1003,7 @@ bool ArduControl::MfxSearchTask::pop_info(MfxInfo &info)
 
 
 ArduControl::MonitorTask::MonitorTask():
+       Task("Monitor"),
        voltage(0),
        current(0),
        base_level(0),
@@ -1005,17 +1013,13 @@ ArduControl::MonitorTask::MonitorTask():
 
 bool ArduControl::MonitorTask::get_work(PendingCommand &cmd)
 {
-       Time::TimeStamp t = Time::now();
-       if(t<next_poll)
-               return false;
-
        if(next_type==0)
                cmd.command[0] = READ_INPUT_VOLTAGE;
        else
                cmd.command[0] = READ_TRACK_CURRENT;
        cmd.length = 1;
 
-       next_poll = t+200*Time::msec;
+       sleep(200*Time::msec);
        next_type = (next_type+1)%5;
 
        return true;
@@ -1168,9 +1172,33 @@ void ArduControl::ControlThread::init_baud_rate()
 
 bool ArduControl::ControlThread::get_work(PendingCommand &cmd)
 {
-       for(vector<Task *>::iterator i=tasks.begin(); i!=tasks.end(); ++i)
-               if((*i)->get_work(cmd))
+       Time::TimeStamp t = Time::now();
+
+       unsigned count = 0;
+       for(; (count<tasks.size() && tasks[count]->get_sleep_timeout()<=t); ++count) ;
+
+       for(; count>0; --count)
+       {
+               unsigned i = 0;
+               for(unsigned j=1; j<count; ++j)
+                       if(tasks[j]->get_priority()<tasks[i]->get_priority())
+                               i = j;
+
+               Task *task = tasks[i];
+               bool result = task->get_work(cmd);
+
+               Time::TimeStamp st = max(task->get_sleep_timeout(), t);
+               for(; (i+1<tasks.size() && tasks[i+1]->get_sleep_timeout()<=st); ++i)
+                       tasks[i] = tasks[i+1];
+               tasks[i] = task;
+
+               if(result)
+               {
+                       if(control.debug>=2)
+                               IO::print("Scheduled task %s\n", task->get_name());
                        return true;
+               }
+       }
 
        // As fallback, send an idle packet for the MM protocol
        cmd.command[0] = MOTOROLA_SPEED;

diff --git a/source/libr2c2/arducontrol.h b/source/libr2c2/arducontrol.h
index 4373a84f707713e6b7d80c1e927875719b6cad0a..c170e4203c9c2eb3de864de49d958a0a10b6e794 100644 (file)
--- a/source/libr2c2/arducontrol.h
+++ b/source/libr2c2/arducontrol.h
@@ -226,12 +226,23 @@ private:
        class Task
        {
        protected:
-               Task() { }
+               std::string name;
+               unsigned priority;
+               Msp::Time::TimeStamp sleep_timeout;
+
+               Task(const std::string &, unsigned = 0);
        public:
                virtual ~Task() { }
 
+               const std::string &get_name() const { return name; }
+
                virtual bool get_work(PendingCommand &) = 0;
                virtual void process_reply(const char *, unsigned) { }
+
+               unsigned get_priority() const { return priority; }
+               const Msp::Time::TimeStamp &get_sleep_timeout() const { return sleep_timeout; }
+       protected:
+               void sleep(const Msp::Time::TimeDelta &);
        };
 
        class CommandQueueTask: public Task
@@ -240,6 +251,8 @@ private:
                Queue<PendingCommand> queue;
 
        public:
+               CommandQueueTask();
+
                virtual bool get_work(PendingCommand &);
 
                void push(const PendingCommand &);
@@ -276,7 +289,6 @@ private:
                ArduControl &control;
                unsigned n_octets;
                unsigned octets_remaining;
-               unsigned delay;
 
        public:
                S88Task(ArduControl &);
@@ -292,7 +304,6 @@ private:
        {
        private:
                unsigned serial;
-               Msp::Time::TimeStamp next;
 
        public:
                MfxAnnounceTask();
@@ -308,7 +319,6 @@ private:
        private:
                ArduControl &control;
                unsigned next_address;
-               Msp::Time::TimeStamp next;
                unsigned size;
                unsigned bits;
                unsigned misses;
@@ -331,7 +341,6 @@ private:
                float current;
                float base_level;
                float peak_level;
-               Msp::Time::TimeStamp next_poll;
                unsigned next_type;
 
        public: