From dda1e6c8716b6ac70d63a3f6ff95474a8b8b7336 Mon Sep 17 00:00:00 2001 From: Mikko Rasa Date: Mon, 3 Jun 2013 21:31:12 +0300 Subject: [PATCH] Improve the interface and algorithms of BlockAllocator --- source/libr2c2/blockallocator.cpp | 141 ++++++++++++++++++++---------- source/libr2c2/blockallocator.h | 14 ++- source/libr2c2/signal.cpp | 4 +- source/libr2c2/train.cpp | 87 ++++++++---------- source/libr2c2/train.h | 2 +- source/libr2c2/trainrouter.cpp | 13 ++- 6 files changed, 153 insertions(+), 108 deletions(-) diff --git a/source/libr2c2/blockallocator.cpp b/source/libr2c2/blockallocator.cpp index c9cc5b4..55f932e 100644 --- a/source/libr2c2/blockallocator.cpp +++ b/source/libr2c2/blockallocator.cpp @@ -35,19 +35,13 @@ void BlockAllocator::start_from(const BlockIter &block) if(!block) throw invalid_argument("BlockAllocator::start_from"); - release_blocks(blocks.begin(), blocks.end()); - - blocks.push_back(block); - if(!block->reserve(&train)) - { - blocks.pop_back(); - return; - } + clear(); + reserve_block(block); } void BlockAllocator::clear() { - release_blocks(blocks.begin(), blocks.end()); + release_blocks_begin(blocks.end()); pending_block = 0; stop_at_block = 0; } @@ -84,12 +78,29 @@ const BlockIter &BlockAllocator::last_current() const return *--i; } -int BlockAllocator::get_entry_to_block(const Block &block) const +const BlockIter &BlockAllocator::iter_for(const Block &block) const { - for(BlockList::const_iterator i=blocks.begin(); i!=blocks.end(); ++i) - if(i->block()==&block) - return i->entry(); - return -1; + BlockList::const_iterator i = find_block(blocks.begin(), blocks.end(), block); + if(i==blocks.end()) + throw key_error(&block); + return *i; +} + +bool BlockAllocator::has_block(const Block &block) const +{ + return find_block(blocks.begin(), blocks.end(), block)!=blocks.end(); +} + +bool BlockAllocator::is_block_current(const Block &block) const +{ + return find_block(blocks.begin(), cur_blocks_end, block)!=cur_blocks_end; +} + +BlockAllocator::BlockList::const_iterator BlockAllocator::find_block(const BlockList::const_iterator &begin, const BlockList::const_iterator &end, const Block &block) const +{ + BlockList::const_iterator i; + for(i=begin; (i!=end && &**i!=&block); ++i) ; + return i; } void BlockAllocator::reserve_more() @@ -143,13 +154,8 @@ void BlockAllocator::reserve_more() break; } - blocks.push_back(block); - if(!block->reserve(&train)) - { - blocks.pop_back(); - pending_block = &*block; + if(!reserve_block(block)) break; - } if(cur_blocks_end==blocks.end()) --cur_blocks_end; @@ -199,13 +205,35 @@ void BlockAllocator::reserve_more() ++cur_blocks_end; } +bool BlockAllocator::reserve_block(const BlockIter &block) +{ + /* Add it to the list first to present a consistent state in block_reserved + signal. */ + blocks.push_back(block); + try + { + if(!block->reserve(&train)) + { + blocks.pop_back(); + return false; + } + + return true; + } + catch(...) + { + blocks.pop_back(); + throw; + } +} + void BlockAllocator::release_until(const Block &block) { for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i) if(i->block()==&block) { if(++i!=cur_blocks_end) - release_blocks(blocks.begin(), i); + release_blocks_begin(i); return; } } @@ -218,7 +246,7 @@ bool BlockAllocator::release_from(const Block &block) if(i->block()==&block) { if(have_sensor) - release_blocks(i, blocks.end()); + release_blocks_end(i); return have_sensor; } else if((*i)->get_sensor_id()) @@ -230,22 +258,43 @@ bool BlockAllocator::release_from(const Block &block) void BlockAllocator::release_noncurrent() { - release_blocks(cur_blocks_end, blocks.end()); + release_blocks_end(cur_blocks_end); } -void BlockAllocator::release_blocks(const BlockList::iterator &b, const BlockList::iterator &e) +void BlockAllocator::release_blocks_begin(const BlockList::iterator &end) { - for(BlockList::iterator i=b; i!=e; ) - { - if(cur_blocks_end==i) - cur_blocks_end = e; + for(BlockList::iterator i=blocks.begin(); i!=end; ) + release_block(i++); +} - Block &block = **i; - blocks.erase(i++); - block.reserve(0); +void BlockAllocator::release_blocks_end(const BlockList::iterator &begin) +{ + // Guard against decrementing blocks.begin() + if(begin==blocks.begin()) + return release_blocks_begin(blocks.end()); + + /* Release the blocks in reverse order so that a consistent state is + presented in block_reserved signal. */ + bool done = false; + for(BlockList::iterator i=blocks.end(); !done; ) + { + done = (i==begin); + release_block(i--); } } +void BlockAllocator::release_block(const BlockList::iterator &i) +{ + if(i==cur_blocks_end) + ++cur_blocks_end; + if(&**i==pending_block) + pending_block = 0; + + Block &block = **i; + blocks.erase(i); + block.reserve(0); +} + void BlockAllocator::reverse() { release_noncurrent(); @@ -256,9 +305,8 @@ void BlockAllocator::reverse() void BlockAllocator::turnout_path_changed(Track &track) { - for(list::iterator i=blocks.begin(); i!=blocks.end(); ++i) - if((*i)->get_turnout_id()==track.get_turnout_id() && !reserving && &**i==pending_block) - reserve_more(); + if(&track.get_block()==pending_block && !reserving) + reserve_more(); } void BlockAllocator::block_reserved(Block &block, const Train *tr) @@ -269,6 +317,9 @@ void BlockAllocator::block_reserved(Block &block, const Train *tr) void BlockAllocator::block_state_changed(Block &block, Block::State state) { + if(block.get_train()!=&train) + return; + if(state==Block::MAYBE_ACTIVE) { // Find the first sensor block from our reserved blocks that isn't this sensor @@ -302,29 +353,31 @@ void BlockAllocator::block_state_changed(Block &block, Block::State state) } else if(state==Block::INACTIVE) { - const Vehicle &veh = train.get_controller().get_reverse() ? train.get_vehicle(0) : train.get_vehicle(train.get_n_vehicles()-1); + const Vehicle &veh = train.get_vehicle(train.get_controller().get_reverse() ? 0 : train.get_n_vehicles()-1); + const Block &veh_block = veh.get_track()->get_block(); + const Driver &driver = train.get_layout().get_driver(); - // Find the first sensor in our current blocks that's still active - BlockList::iterator end = blocks.begin(); + /* Sensors aren't guaranteed to be detriggered in order. Go through the + block list and locate the first sensor that's still active. */ + BlockList::iterator end = blocks.end(); for(BlockList::iterator i=blocks.begin(); i!=cur_blocks_end; ++i) { - if((*i)->has_track(*veh.get_track())) + // Avoid freeing blocks that still hold the train's vehicles + if(&**i==&veh_block) break; + if((*i)->get_sensor_id()) { - if(train.get_layout().get_driver().get_sensor((*i)->get_sensor_id())) + if(driver.get_sensor((*i)->get_sensor_id())) break; else - { end = i; - ++end; - } } } - if(end!=blocks.begin() && end!=cur_blocks_end) + if(end!=blocks.end()) // Free blocks up to the last inactive sensor - release_blocks(blocks.begin(), end); + release_blocks_begin(++end); } } diff --git a/source/libr2c2/blockallocator.h b/source/libr2c2/blockallocator.h index 7ff5d77..c90b450 100644 --- a/source/libr2c2/blockallocator.h +++ b/source/libr2c2/blockallocator.h @@ -48,15 +48,25 @@ public: const BlockIter &first() const; const BlockIter &last() const; const BlockIter &last_current() const; + const BlockIter &iter_for(const Block &) const; - int get_entry_to_block(const Block &) const; + bool has_block(const Block &) const; + bool is_block_current(const Block &) const; +private: + BlockList::const_iterator find_block(const BlockList::const_iterator &, const BlockList::const_iterator &, const Block &) const; +public: void reserve_more(); +private: + bool reserve_block(const BlockIter &); +public: void release_until(const Block &); bool release_from(const Block &); void release_noncurrent(); private: - void release_blocks(const BlockList::iterator &, const BlockList::iterator &); + void release_blocks_begin(const BlockList::iterator &); + void release_blocks_end(const BlockList::iterator &); + void release_block(const BlockList::iterator &); public: void reverse(); diff --git a/source/libr2c2/signal.cpp b/source/libr2c2/signal.cpp index 7ccb238..a858e0d 100644 --- a/source/libr2c2/signal.cpp +++ b/source/libr2c2/signal.cpp @@ -163,8 +163,8 @@ void Signal::block_reserved(const Block &b, Train *t) { if(t) { - int train_entry = t->get_block_allocator().get_entry_to_block(*block); - if(train_entry>=0 && static_cast(train_entry)==entry) + const BlockIter &b_iter = t->get_block_allocator().iter_for(b); + if(b_iter && b_iter.entry()==entry) { if(train_conn) train_conn.disconnect(); diff --git a/source/libr2c2/train.cpp b/source/libr2c2/train.cpp index 72714d7..b7dc247 100644 --- a/source/libr2c2/train.cpp +++ b/source/libr2c2/train.cpp @@ -40,7 +40,6 @@ Train::Train(Layout &l, const VehicleType &t, unsigned a, const string &p): speed_changing(false), reverse(false), functions(0), - travel_dist(0), pure_speed(false), speed_quantizer(0), accurate_position(false), @@ -370,20 +369,9 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt) set_active(true); Vehicle &vehicle = *(reverse ? vehicles.back() : vehicles.front()); - Track *track = vehicle.get_track(); - - BlockIter first = allocator.first(); - BlockIter last_current = allocator.last_current(); - bool ok = false; - for(BlockIter i=first; !ok; i=i.next()) - { - ok = i->has_track(*track); - if(i==last_current) - break; - } float d = speed*(dt/Time::sec); - if(ok) + if(allocator.is_block_current(vehicle.get_track()->get_block())) { SetFlag setf(advancing); vehicle.advance(reverse ? -d : d); @@ -482,52 +470,47 @@ void Train::loco_func_event(unsigned addr, unsigned func, bool state) void Train::block_state_changed(Block &block, Block::State state) { - if(state==Block::MAYBE_ACTIVE) + if(block.get_train()==this && state==Block::MAYBE_ACTIVE) { - const BlockIter &first = allocator.first(); - const BlockIter &last_cur = allocator.last_current(); - bool valid = false; - for(BlockIter i=first; !valid; i=i.next()) + if(last_entry_block) { - if(&*i==&block) - valid = true; - if(&*i==&*last_cur) - break; - } + float travel_distance = -1; + if(pure_speed && speed_quantizer && current_speed_step>0) + travel_distance = 0; - if(valid) - { - // Compute speed and update related state - float travel_time_secs = (Time::now()-last_entry_time)/Time::sec; + for(BlockIter i=last_entry_block; &*i!=█ i=i.next()) + { + if(i->get_sensor_id()) + return; + if(travel_distance>=0) + travel_distance += i->get_path_length(i.entry()); + } - if(pure_speed && speed_quantizer && current_speed_step>0 && travel_time_secs>=2) - speed_quantizer->learn(current_speed_step, travel_dist/travel_time_secs, travel_time_secs); + if(travel_distance>0) + { + float travel_time_secs = (Time::now()-last_entry_time)/Time::sec; + + if(travel_time_secs>=2) + speed_quantizer->learn(current_speed_step, travel_distance/travel_time_secs, travel_time_secs); + } + } - travel_dist = 0; - unsigned entry = allocator.get_entry_to_block(block); - for(BlockIter i(&block, entry);; i=i.next()) + last_entry_block = allocator.iter_for(block); + last_entry_time = Time::now(); + pure_speed = true; + accurate_position = true; + overshoot_dist = 0; + + if(!advancing && vehicles.front()->get_track()) + { + TrackIter track = last_entry_block.track_iter(); + if(reverse) { - travel_dist += i->get_path_length(i.entry()); - - if(&*i==&block && !advancing && vehicles.front()->get_track()) - { - TrackIter track = i.track_iter(); - if(reverse) - { - track = track.flip(); - vehicles.back()->place(track, 0, Vehicle::BACK_AXLE); - } - else - vehicles.front()->place(track, 0, Vehicle::FRONT_AXLE); - } - - if(i==last_cur) - break; + track = track.flip(); + vehicles.back()->place(track, 0, Vehicle::BACK_AXLE); } - last_entry_time = Time::now(); - pure_speed = true; - accurate_position = true; - overshoot_dist = 0; + else + vehicles.front()->place(track, 0, Vehicle::FRONT_AXLE); } } } diff --git a/source/libr2c2/train.h b/source/libr2c2/train.h index 6b384f8..03b8b66 100644 --- a/source/libr2c2/train.h +++ b/source/libr2c2/train.h @@ -63,8 +63,8 @@ private: Msp::Time::TimeStamp stop_timeout; unsigned functions; + BlockIter last_entry_block; Msp::Time::TimeStamp last_entry_time; - float travel_dist; bool pure_speed; SpeedQuantizer *speed_quantizer; bool accurate_position; diff --git a/source/libr2c2/trainrouter.cpp b/source/libr2c2/trainrouter.cpp index 8cfedee..d29438f 100644 --- a/source/libr2c2/trainrouter.cpp +++ b/source/libr2c2/trainrouter.cpp @@ -176,7 +176,7 @@ void TrainRouter::block_reserved(Block &block, Train *t) return; } - BlockIter b_iter(&block, t->get_block_allocator().get_entry_to_block(block)); + BlockIter b_iter = t->get_block_allocator().iter_for(block); RouteList::iterator route = routes.begin(); if(advance_route(route, block)) @@ -206,13 +206,13 @@ void TrainRouter::block_reserved(Block &block, Train *t) void TrainRouter::train_advanced(Block &block) { + BlockIter b_iter = train.get_block_allocator().iter_for(block); + // Check if we've reached the next route if(routes.size()>1) { - unsigned entry = train.get_block_allocator().get_entry_to_block(block); - Track &track = *block.get_endpoint(entry).track; const Route &route = **++routes.begin(); - if(route.has_track(track)) + if(route.has_track(*b_iter.endpoint().track)) { routes.pop_front(); // XXX Exceptions? @@ -222,9 +222,8 @@ void TrainRouter::train_advanced(Block &block) if(!routes.empty()) { - BlockIter iter(&block, train.get_block_allocator().get_entry_to_block(block)); - iter = iter.next(); - if(iter && !is_on_route(*iter)) + b_iter = b_iter.next(); + if(b_iter && !is_on_route(*b_iter)) arriving = true; } } -- 2.45.2