X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Farducontrol.cpp;h=8446294f8562619b416ecd439bcc2cd5d8acfbd3;hb=25aacc9bfe66f1a2bffd196c735d3444adac6fe3;hp=8e28ea66c9ce275142c0dbb9f59f42001e0acfd5;hpb=83d2cda02939e450f858456915210625d634946d;p=r2c2.git diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp index 8e28ea6..8446294 100644 --- a/source/libr2c2/arducontrol.cpp +++ b/source/libr2c2/arducontrol.cpp @@ -18,6 +18,14 @@ ArduControl::ProtocolInfo ArduControl::protocol_info[2] = { 0x3FFF, 126, 15 } // MFX }; +Driver::TelemetryInfo ArduControl::telemetry_info[4] = +{ + { "voltage", "Voltage", "V", 1 }, + { "current", "Current", "A", 2 }, + { "cmd-queue-depth", "Cmd queue", "", 0 }, + { "s88-latency", "S88 latency", "ms", 0 } +}; + ArduControl::ArduControl(const Options &opts): serial(opts.get(string(), "ttyUSB0")), debug(opts.get("debug")), @@ -327,6 +335,28 @@ bool ArduControl::get_sensor(unsigned addr) const return get_item(sensors, addr).state; } +const Driver::TelemetryInfo *ArduControl::enumerate_telemetry(unsigned i) const +{ + if(i<4) + return telemetry_info+i; + else + return 0; +} + +float ArduControl::get_telemetry_value(const string &name) const +{ + if(name==telemetry_info[0].name) + return monitor.get_voltage(); + else if(name==telemetry_info[1].name) + return monitor.get_current(); + else if(name==telemetry_info[2].name) + return command_queue.size(); + else if(name==telemetry_info[3].name) + return s88.get_latency()/Time::msec; + else + throw key_error(name); +} + void ArduControl::tick() { Tag tag; @@ -459,8 +489,9 @@ void ArduControl::tick() if(active_accessory && off_timeout) { + bool success = (monitor.get_peak()>0.35f && monitor.get_current()off_timeout) + if(t>off_timeout || success) { Accessory &acc = *active_accessory; @@ -469,7 +500,7 @@ void ArduControl::tick() // Assume success if we were uncertain of the physical setting if(acc.uncertain&bit) acc.uncertain &= ~bit; - else if(acc.kind==Accessory::TURNOUT && monitor.get_peak()<0.5f) + else if(acc.kind==Accessory::TURNOUT && !success) { if(debug>=1) IO::print("Peak current only %.2f A\n", monitor.get_peak()); @@ -597,7 +628,7 @@ ArduControl::Accessory::Accessory(Kind k, unsigned a, unsigned b, unsigned s): state(0), uncertain((1<::pop(T &item) return true; } +template +unsigned ArduControl::Queue::size() const +{ + return items.size(); +} + template bool ArduControl::Queue::empty() const { @@ -701,7 +738,35 @@ bool ArduControl::Queue::empty() const } +bool ArduControl::CommandQueueTask::get_work(PendingCommand &cmd) +{ + return queue.pop(cmd); +} + +void ArduControl::CommandQueueTask::push(const PendingCommand &cmd) +{ + queue.push(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), @@ -795,28 +860,28 @@ 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; + Time::TimeStamp t = Time::now(); + if(last_poll) + latency = t-last_poll; + last_poll = t; + octets_remaining = n_octets; cmd.command[0] = S88_READ; cmd.command[1] = octets_remaining; cmd.length = 2; - delay = 4; + sleep(100*Time::msec); return true; } @@ -865,20 +930,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; } @@ -890,6 +953,7 @@ void ArduControl::MfxAnnounceTask::set_serial(unsigned s) ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c): + Task("MfxSearch", 1), control(c), next_address(1), size(0), @@ -929,17 +993,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); @@ -964,7 +1024,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; @@ -984,6 +1044,7 @@ bool ArduControl::MfxSearchTask::pop_info(MfxInfo &info) ArduControl::MonitorTask::MonitorTask(): + Task("Monitor"), voltage(0), current(0), base_level(0), @@ -993,17 +1054,13 @@ ArduControl::MonitorTask::MonitorTask(): bool ArduControl::MonitorTask::get_work(PendingCommand &cmd) { - Time::TimeStamp t = Time::now(); - if(tget_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); - for(vector::iterator i=tasks.begin(); i!=tasks.end(); ++i) - if((*i)->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;