From 23e1a988da2efae47e39d93babe7e37b1c0ca554 Mon Sep 17 00:00:00 2001 From: Mikko Rasa Date: Thu, 19 Feb 2015 11:18:02 +0200 Subject: [PATCH] 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. --- source/libr2c2/arducontrol.cpp | 80 +++++++++++++++++++++++----------- source/libr2c2/arducontrol.h | 19 +++++--- 2 files changed, 68 insertions(+), 31 deletions(-) diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp index 4acfc60..40b3d35 100644 --- 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>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>(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::iterator i=tasks.begin(); i!=tasks.end(); ++i) - if((*i)->get_work(cmd)) + Time::TimeStamp t = Time::now(); + + unsigned count = 0; + for(; (countget_sleep_timeout()<=t); ++count) ; + + for(; count>0; --count) + { + unsigned i = 0; + for(unsigned j=1; jget_priority()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+1get_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 4373a84..c170e42 100644 --- 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 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: -- 2.45.2