Handle reversing in a way that allows backing out of a dead end

[r2c2.git] / source / libmarklin / train.cpp

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index ed57dd7ee143c7f1ec8eb1a5b1081f85d9ad4881..1abf535c06b6d03a0649c95162b7264da1802c73 100644 (file)
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -60,6 +60,8 @@ Train::Train(Layout &l, const LocoType &t, unsigned a):
        layout.get_driver().signal_sensor.connect(sigc::mem_fun(this, &Train::sensor_event));
        layout.get_driver().signal_turnout.connect(sigc::mem_fun(this, &Train::turnout_event));
 
+       layout.get_driver().signal_halt.connect(sigc::mem_fun(this, &Train::halt_event));
+
        control->signal_control_changed.connect(signal_control_changed);
 }
 
@@ -320,10 +322,10 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
        float speed = control->get_speed();
        unsigned speed_notch = find_speed(abs(speed));
 
-       if(speed && (speed<0)!=reverse)
+       if(control->get_reverse()!=reverse)
        {
-               driver.set_loco_reverse(address, speed<0);
-               reverse = speed<0;
+               reverse = control->get_reverse();
+               driver.set_loco_reverse(address, reverse);
 
                release_blocks(rsv_blocks);
                reverse_blocks(cur_blocks);
@@ -362,11 +364,60 @@ void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
                {
                        overshoot_dist += d;
                        if(overshoot_dist>40*layout.get_catalogue().get_scale())
+                       {
                                layout.emergency(name+" has not arrived at sensor");
+                               accurate_position = false;
+                       }
                }
        }
        else if(end_of_route)
                set_route(0);
+
+       if(!cur_blocks.empty() && !cur_blocks.front().block->get_sensor_id())
+       {
+               Vehicle &veh = *(reverse ? vehicles.front() : vehicles.back());
+
+               list<BlockRef>::iterator i = cur_blocks.begin();
+               const Block::Endpoint &bep = i->block->get_endpoints()[i->entry];
+
+               Track *track = bep.track;
+               unsigned entry = bep.track_ep;
+
+               bool found = false;
+               float dist = veh.get_offset()-veh.get_type().get_length()/2;
+               while(1)
+               {
+                       if(track==veh.get_track())
+                       {
+                               found = true;
+                               break;
+                       }
+
+                       if(i!=cur_blocks.begin())
+                       {
+                               float path_len = track->get_type().get_path_length(track->get_active_path());
+                               dist += path_len;
+                       }
+
+                       unsigned exit = track->traverse(entry);
+                       Track *next = track->get_link(exit);
+                       entry = next->get_endpoint_by_link(*track);
+                       track = next;
+
+                       if(!i->block->get_tracks().count(track))
+                       {
+                               ++i;
+                               if(i==cur_blocks.end())
+                                       break;
+                       }
+               }
+
+               if(found && i!=cur_blocks.begin() && dist>10*layout.get_catalogue().get_scale())
+               {
+                       cur_blocks.front().block->reserve(0);
+                       cur_blocks.erase(cur_blocks.begin());
+               }
+       }
 }
 
 void Train::save(list<DataFile::Statement> &st) const
@@ -534,15 +585,15 @@ void Train::sensor_event(unsigned addr, bool state)
                                if(layout.get_driver().get_sensor(i->block->get_sensor_id()))
                                        break;
                                else
+                               {
                                        end = i;
+                                       ++end;
+                               }
                        }
                
                if(end!=cur_blocks.begin())
-               {
                        // Free blocks up to the last inactive sensor
-                       ++end;
                        release_blocks(cur_blocks, cur_blocks.begin(), end);
-               }
        }
 }
 
@@ -557,6 +608,12 @@ void Train::turnout_event(unsigned addr, bool)
        }
 }
 
+void Train::halt_event(bool h)
+{
+       if(h)
+               accurate_position = false;
+}
+
 void Train::block_reserved(const Block &block, const Train *train)
 {
        if(&block==pending_block && !train)