X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Farducontrol.cpp;h=4acfc60e0e761a0c5aaa7d4c260a0023988a8034;hb=aa48746158b19ad795a3cc656c4dfe17044baf54;hp=1d1ece9adb4eb4fb5e347c373298f8d1a64f869e;hpb=fe8b45dc60ed7828f0b4af9daa9b4cd0649415ca;p=r2c2.git diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp index 1d1ece9..4acfc60 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::RefreshTask::RefreshTask(): next(cycle.end()), round(0), @@ -1009,6 +1046,7 @@ ArduControl::ControlThread::ControlThread(ArduControl &c): control(c), done(false) { + tasks.push_back(&control.command_queue); tasks.push_back(&control.monitor); tasks.push_back(&control.mfx_announce); tasks.push_back(&control.mfx_search); @@ -1130,9 +1168,6 @@ void ArduControl::ControlThread::init_baud_rate() bool ArduControl::ControlThread::get_work(PendingCommand &cmd) { - if(control.command_queue.pop(cmd)) - return true; - for(vector::iterator i=tasks.begin(); i!=tasks.end(); ++i) if((*i)->get_work(cmd)) return true;