Halt all trains in various unexpected situations

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

diff --git a/source/libmarklin/intellibox.cpp b/source/libmarklin/intellibox.cpp
index ace23e8f6166762926782fbd2a9dbcb01e290656..70372f4f10dae20b930f091affe7cc80416249a4 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))
@@ -98,6 +112,8 @@ 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);
@@ -411,10 +427,11 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
                unsigned char err;
                read_all(&err, 1);
 
+               unsigned addr = queue.front().addr;
+               Turnout &turnout = turnouts[addr];
+
                if(err==ERR_NO_ERROR)
                {
-                       unsigned addr = queue.front().addr;
-                       Turnout &turnout = turnouts[addr];
                        turnout.state = turnout.pending;
                        if(turnout.active)
                        {
@@ -424,6 +441,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)
        {