X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Farducontrol.cpp;h=40b3d35fd9f84693f233ad9490eb5013fa7bab57;hb=23e1a988da2efae47e39d93babe7e37b1c0ca554;hp=1d1ece9adb4eb4fb5e347c373298f8d1a64f869e;hpb=fe8b45dc60ed7828f0b4af9daa9b4cd0649415ca;p=r2c2.git diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp index 1d1ece9..40b3d35 100644 --- a/source/libr2c2/arducontrol.cpp +++ b/source/libr2c2/arducontrol.cpp @@ -207,7 +207,7 @@ unsigned ArduControl::add_turnout(unsigned addr, const TrackType &type) if(!addr || !type.is_turnout()) throw invalid_argument("ArduControl::add_turnout"); - return add_accessory(Accessory::TURNOUT, addr, type.get_state_bits()); + return add_accessory(Accessory::TURNOUT, addr, type.get_state_bits(), type.get_paths()); } void ArduControl::remove_turnout(unsigned addr) @@ -227,7 +227,7 @@ unsigned ArduControl::get_turnout(unsigned addr) const unsigned ArduControl::add_signal(unsigned addr, const SignalType &) { - return add_accessory(Accessory::SIGNAL, addr, 1); + return add_accessory(Accessory::SIGNAL, addr, 1, 3); } void ArduControl::remove_signal(unsigned addr) @@ -245,7 +245,7 @@ unsigned ArduControl::get_signal(unsigned addr) const return get_accessory(Accessory::SIGNAL, addr); } -unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits) +unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits, unsigned states) { AccessoryMap::iterator i = accessories.lower_bound(addr); AccessoryMap::iterator j = accessories.upper_bound(addr+bits-1); @@ -258,7 +258,7 @@ unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigne throw key_error(addr); } - insert_unique(accessories, addr, Accessory(kind, addr, bits)); + insert_unique(accessories, addr, Accessory(kind, addr, bits, states)); return addr; } @@ -291,6 +291,20 @@ unsigned ArduControl::get_accessory(Accessory::Kind kind, unsigned addr) const return acc.state; } +void ArduControl::activate_accessory_by_mask(Accessory &acc, unsigned mask) +{ + unsigned bit = mask&~(mask-1); + for(active_index=0; (bit>>active_index)>1; ++active_index) ; + acc.state.set((acc.state&~bit)|(acc.target&bit)); + if(debug>=1) + IO::print("Setting accessory %d bit %d, state=%d\n", acc.address, active_index, acc.state.pending); + PendingCommand cmd(acc, Accessory::ACTIVATE, active_index); + command_queue.push(cmd); + active_accessory = &acc; + + monitor.reset_peak(); +} + unsigned ArduControl::add_sensor(unsigned addr) { if(!addr) @@ -402,11 +416,18 @@ void ArduControl::tick() { Accessory &acc = *accessory_queue.front(); - if(acc.state!=acc.target || acc.uncertain) + if(acc.uncertain) + { + unsigned zeroes = acc.uncertain&~acc.target; + if(zeroes) + activate_accessory_by_mask(acc, zeroes); + else + activate_accessory_by_mask(acc, acc.uncertain); + } + else if(acc.state!=acc.target) { unsigned changes = acc.state^acc.target; - unsigned lowest_bit = changes&~(changes-1); - if(lowest_bit>>acc.bits) + if(!(changes&((1<>active_index)>1; ++active_index) ; - acc.state.set(acc.state^lowest_bit); - PendingCommand cmd(acc, Accessory::ACTIVATE, active_index); - command_queue.push(cmd); - active_accessory = &acc; - - monitor.reset_peak(); + unsigned toggle_bit = 0; + for(unsigned bit=1; (!toggle_bit && bit<=changes); bit<<=1) + if((changes&bit) && (acc.valid_states&(1<<(acc.state^bit)))) + toggle_bit = bit; + + activate_accessory_by_mask(acc, toggle_bit); } } else @@ -440,8 +459,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; @@ -450,11 +470,16 @@ 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()); signal_turnout_failed.emit(acc.address); acc.state.rollback(); - acc.target ^= bit; + if(acc.valid_states&(1<<(acc.target^bit))) + acc.target ^= bit; + else + acc.target = acc.state; } off_timeout = Time::TimeStamp(); @@ -565,14 +590,15 @@ unsigned ArduControl::Locomotive::create_speed_func_command(unsigned f, char *bu } -ArduControl::Accessory::Accessory(Kind k, unsigned a, unsigned b): +ArduControl::Accessory::Accessory(Kind k, unsigned a, unsigned b, unsigned s): kind(k), address(a), bits(b), + valid_states(s), state(0), uncertain((1<::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), @@ -770,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; @@ -791,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; } @@ -840,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; } @@ -865,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), @@ -904,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); @@ -939,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; @@ -959,6 +1003,7 @@ bool ArduControl::MfxSearchTask::pop_info(MfxInfo &info) ArduControl::MonitorTask::MonitorTask(): + Task("Monitor"), voltage(0), current(0), base_level(0), @@ -968,17 +1013,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; - for(vector::iterator i=tasks.begin(); i!=tasks.end(); ++i) - if((*i)->get_work(cmd)) + 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;