Release non-sensor blocks when the tail end of the train has passed far enough

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

diff --git a/source/libmarklin/intellibox.cpp b/source/libmarklin/intellibox.cpp
index 35880610145de2d426c4c860bbf4ba9a56e43652..6efe8750a188e7dce26efeb4585d46fd438c1ba9 100644 (file)
--- a/source/libmarklin/intellibox.cpp
+++ b/source/libmarklin/intellibox.cpp
@@ -20,6 +20,7 @@ namespace Marklin {
 
 Intellibox::Intellibox(const string &dev):
        power(false),
+       halted(false),
        update_sensors(false),
        command_sent(false)
 {
@@ -80,6 +81,19 @@ void Intellibox::set_power(bool p)
        signal_power.emit(power);
 }
 
+void Intellibox::halt(bool h)
+{
+       halted = h;
+       if(halted)
+       {
+               for(map<unsigned, Locomotive>::iterator i=locos.begin(); i!=locos.end(); ++i)
+                       if(i->second.speed)
+                               set_loco_speed(i->first, 0);
+       }
+
+       signal_halt.emit(halted);
+}
+
 void Intellibox::add_loco(unsigned addr)
 {
        if(!locos.count(addr))
@@ -96,17 +110,23 @@ void Intellibox::add_loco(unsigned addr)
 void Intellibox::set_loco_speed(unsigned addr, unsigned speed)
 {
        Locomotive &loco = locos[addr];
+       if(speed==loco.speed)
+               return;
+       if(speed && halted)
+               return;
+
        loco.speed = speed;
        loco_command(addr, speed, loco.reverse, loco.funcs|0x100);
-       signal_loco_speed.emit(addr, speed, loco.reverse);
 }
 
 void Intellibox::set_loco_reverse(unsigned addr, bool rev)
 {
        Locomotive &loco = locos[addr];
+       if(rev==loco.reverse)
+               return;
+
        loco.reverse = rev;
        loco_command(addr, loco.speed, rev, loco.funcs|0x100);
-       signal_loco_speed.emit(addr, loco.speed, rev);
 }
 
 void Intellibox::set_loco_function(unsigned addr, unsigned func, bool state)
@@ -141,6 +161,7 @@ void Intellibox::set_turnout(unsigned addr, bool state)
 
        turnout.pending = state;
        turnout.active = true;
+       turnout.off_timeout = Time::TimeStamp();
 
        turnout_command(addr, state, true);
 }
@@ -230,8 +251,22 @@ void Intellibox::tick()
 
 void Intellibox::flush()
 {
+       Time::TimeStamp t = Time::now();
        for(list<CommandSlot>::iterator i=queue.begin(); i!=queue.end(); ++i)
+       {
                write(serial_fd, i->data, i->length);
+               pollfd pfd = { serial_fd, POLLIN, 0 };
+               bool first = true;
+               while(poll(&pfd, 1, (first ? -1 : 0))>0)
+               {
+                       char data[16];
+                       read(serial_fd, data, 16);
+                       first = false;
+               }
+       }
+
+       queue.clear();
+       command_sent = false;
 }
 
 void Intellibox::command(Command cmd)
@@ -342,7 +377,10 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
                        read_all(data, 2);
 
                        unsigned addr = data[0]+((data[1]&7)<<8);
-                       signal_turnout.emit(addr, (data[1]&0x80)!=0);
+                       Turnout &turnout = turnouts[addr];
+                       turnout.state = (data[1]&0x80)!=0;
+                       turnout.pending = turnout.state;
+                       signal_turnout.emit(addr, turnout.state);
                }
        }
        else if(cmd==CMD_EVENT_SENSOR)
@@ -376,7 +414,7 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
                        }
                }
        }
-       else if(cmd==CMD_TURNOUT)
+       else if(cmd==CMD_LOK)
        {
                unsigned char err;
                read_all(&err, 1);
@@ -384,7 +422,20 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
                if(err==ERR_NO_ERROR)
                {
                        unsigned addr = queue.front().addr;
-                       Turnout &turnout = turnouts[addr];
+                       Locomotive &loco = locos[addr];
+                       signal_loco_speed.emit(addr, loco.speed, loco.reverse);
+               }
+       }
+       else if(cmd==CMD_TURNOUT)
+       {
+               unsigned char err;
+               read_all(&err, 1);
+
+               unsigned addr = queue.front().addr;
+               Turnout &turnout = turnouts[addr];
+
+               if(err==ERR_NO_ERROR)
+               {
                        turnout.state = turnout.pending;
                        if(turnout.active)
                        {
@@ -394,6 +445,8 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
                }
                else if(err==ERR_NO_I2C_SPACE)
                        queue.push_back(queue.front());
+               else
+                       turnout.pending = turnout.state;
        }
        else if(cmd==CMD_TURNOUT_STATUS)
        {
@@ -412,6 +465,7 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
                        if(state!=turnout.state)
                        {
                                turnout.state = state;
+                               turnout.pending = state;
                                signal_turnout.emit(addr, turnout.state);
                        }
                }
@@ -491,7 +545,8 @@ Intellibox::Locomotive::Locomotive():
 
 Intellibox::Turnout::Turnout():
        state(false),
-       active(false)
+       active(false),
+       pending(false)
 { }