From 26fbc0321526ccc41a81a332f0d60b7cc42b6567 Mon Sep 17 00:00:00 2001 From: Mikko Rasa Date: Mon, 2 Feb 2015 19:21:53 +0200 Subject: [PATCH] Avoid invalid states when setting multi-bit accessories --- source/libr2c2/arducontrol.cpp | 55 +++++++++++++++++++++++----------- source/libr2c2/arducontrol.h | 6 ++-- 2 files changed, 42 insertions(+), 19 deletions(-) diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp index 1d1ece9..0065c4d 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,18 @@ 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)); + 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 +414,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 @@ -454,7 +471,10 @@ void ArduControl::tick() { 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,10 +585,11 @@ 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< state; unsigned uncertain; unsigned target; Msp::Time::TimeDelta active_time; - Accessory(Kind, unsigned, unsigned); + Accessory(Kind, unsigned, unsigned, unsigned); unsigned create_state_command(unsigned, bool, char *) const; }; @@ -433,10 +434,11 @@ public: virtual unsigned get_signal(unsigned) const; private: - unsigned add_accessory(Accessory::Kind, unsigned, unsigned); + unsigned add_accessory(Accessory::Kind, unsigned, unsigned, unsigned); void remove_accessory(Accessory::Kind, unsigned); void set_accessory(Accessory::Kind, unsigned, unsigned); unsigned get_accessory(Accessory::Kind, unsigned) const; + void activate_accessory_by_mask(Accessory &, unsigned); public: virtual unsigned add_sensor(unsigned); -- 2.45.2