Only signal turnout path change when the command is successfully completed

Call update_speed directly from reserve_more if blocks were reserved
Do not reserve turnouts with wrong path

diff --git a/source/libmarklin/train.cpp b/source/libmarklin/train.cpp
index 7af10280228fa751970c4df739f1506400dc7271..ef38416e98454ab4f0d3f3aaba00a6da07d05850 100644 (file)
--- a/source/libmarklin/train.cpp
+++ b/source/libmarklin/train.cpp
@@ -24,6 +24,7 @@ namespace Marklin {
 Train::Train(TrafficManager &tm, Locomotive &l):
        trfc_mgr(tm),
        loco(l),
+       pending_block(0),
        target_speed(0),
        status("Unplaced"),
        travel_dist(0),
@@ -140,10 +141,7 @@ bool Train::free_block(Block *block)
 void Train::tick(const Time::TimeStamp &t, const Time::TimeDelta &dt)
 {
        if(try_reserve && t>try_reserve)
-       {
                reserve_more();
-               update_speed();
-       }
 
        if(cur_track)
        {
@@ -189,7 +187,6 @@ void Train::locomotive_reverse_changed(bool)
        for(list<BlockRef>::iterator i=cur_blocks.begin(); i!=cur_blocks.end(); ++i)
                i->entry = i->block->traverse(i->entry);
        reserve_more();
-       update_speed();
 
        if(cur_track)
        {
@@ -240,11 +237,8 @@ void Train::sensor_event(bool state, Sensor *sensor)
                        if(i->block->get_sensor_id()==addr)
                                set_position(i->block->get_endpoints()[i->entry]);
 
-               if(target_speed)
-               {
-                       reserve_more();
+               if(target_speed && reserve_more()<2)
                        update_speed();
-               }
        }
        else
        {
@@ -261,7 +255,7 @@ void Train::sensor_event(bool state, Sensor *sensor)
                                break;
                        }
 
-               if(target_speed)
+               if(target_speed && pending_block && addr==pending_block->get_sensor_id())
                        reserve_more();
        }
 }
@@ -299,9 +293,11 @@ void Train::turnout_path_changed(unsigned, Turnout *turnout)
                                i = rsv_blocks.erase(i);
                        }
                        reserve_more();
-                       update_speed();
                        return;
                }
+
+       if(pending_block && tid==pending_block->get_turnout_id())
+               reserve_more();
 }
 
 unsigned Train::reserve_more()
@@ -314,13 +310,15 @@ unsigned Train::reserve_more()
        if(!last)
                return 0;
 
+       pending_block = 0;
+
        unsigned nsens = 0;
        for(list<BlockRef>::const_iterator i=rsv_blocks.begin(); i!=rsv_blocks.end(); ++i)
                if(i->block->get_sensor_id())
                        ++nsens;
 
-       bool result = false;
-       while(nsens<2)
+       bool got_more = false;
+       while(nsens<3)
        {
                int exit = last->block->traverse(last->entry);
                if(exit<0)
@@ -328,20 +326,30 @@ unsigned Train::reserve_more()
 
                Block *link = last->block->get_link(exit);
                if(!link || !link->reserve(this))
+               {
+                       pending_block = link;
                        break;
+               }
 
                if(route && link->get_turnout_id())
                {
                        int path = route->get_turnout(link->get_turnout_id());
-                       if(path>=0)
-                               trfc_mgr.get_control().get_turnout(link->get_turnout_id()).set_path(path);
+                       Turnout &turnout = trfc_mgr.get_control().get_turnout(link->get_turnout_id());
+                       if(path>=0 && path!=turnout.get_path())
+                       {
+                               link->reserve(0);
+                               pending_block = link;
+                               turnout.set_path(path);
+                               break;
+                       }
                }
+
                rsv_blocks.push_back(BlockRef(link, link->get_endpoint_by_link(*last->block)));
                last = &rsv_blocks.back();
                if(last->block->get_sensor_id())
                {
                        ++nsens;
-                       result = true;
+                       got_more = true;
                }
        }
 
@@ -355,6 +363,9 @@ unsigned Train::reserve_more()
                        last = 0;
        }
 
+       if(got_more)
+               update_speed();
+
        return nsens;
 }
 

diff --git a/source/libmarklin/train.h b/source/libmarklin/train.h
index 90627ae4561d021dc372ac8fe3090a1a94794677..bacb246f4dfd5f68a4dee346aa92abf5a85bebcf 100644 (file)
--- a/source/libmarklin/train.h
+++ b/source/libmarklin/train.h
@@ -60,6 +60,7 @@ private:
        Locomotive &loco;
        std::list<BlockRef> cur_blocks;
        std::list<BlockRef> rsv_blocks;
+       Block *pending_block;
        unsigned target_speed;
        const Route *route;
        Msp::Time::TimeStamp try_reserve;

diff --git a/source/libmarklin/turnout.cpp b/source/libmarklin/turnout.cpp
index d5f96179be74de4dc71f49129340f9f9bb05a06d..c022e05ea903ab5049eaf192e9860c19a8c5c7c0 100644 (file)
--- a/source/libmarklin/turnout.cpp
+++ b/source/libmarklin/turnout.cpp
@@ -21,7 +21,10 @@ Turnout::Turnout(Control &c, unsigned a, bool d):
        control(c),
        addr(a),
        path(0),
-       dual(d)
+       pending_path(0),
+       pending_cmds(0),
+       dual(d),
+       on(false)
 {
        control.add_turnout(*this);
 
@@ -30,56 +33,75 @@ Turnout::Turnout(Control &c, unsigned a, bool d):
        unsigned char data[2];
        data[0] = addr&0xFF;
        data[1] = (addr>>8)&0xFF;
-       control.command(CMD_TURNOUT_STATUS, data, 2).signal_done.connect(sigc::bind(sigc::mem_fun(this, &Turnout::status_reply), false));
+       control.command(CMD_TURNOUT_STATUS, data, 2).signal_done.connect(sigc::bind(sigc::mem_fun(this, &Turnout::status_reply), 1));
        if(dual)
        {
                data[0] = (addr+1)&0xFF;
                data[1] = ((addr+1)>>8)&0xFF;
-               control.command(CMD_TURNOUT_STATUS, data, 2).signal_done.connect(sigc::bind(sigc::mem_fun(this, &Turnout::status_reply), true));
+               control.command(CMD_TURNOUT_STATUS, data, 2).signal_done.connect(sigc::bind(sigc::mem_fun(this, &Turnout::status_reply), 2));
        }
 }
 
-void Turnout::set_path(unsigned p)
+void Turnout::set_path(unsigned char p)
 {
-       signal_path_changing.emit(p);
+       if(path==p || pending_cmds)
+               return;
 
-       path = p;
-       command(true);
-       control.set_timer(200*Time::msec).signal_timeout.connect(sigc::mem_fun(this, &Turnout::switch_timeout));
+       signal_path_changing.emit(p);
 
-       signal_path_changed.emit(path);
+       pending_path = p;
+       on = true;
+       command(3);
 }
 
-void Turnout::command(bool on)
+void Turnout::command(unsigned char mask)
 {
        unsigned char data[2];
-       data[0] = addr&0xFF;
-       data[1] = ((addr>>8)&0x7) | (on ? 0x40 : 0) | (path&1 ? 0 : 0x80);
-       control.command(CMD_TURNOUT, data, 2);
-       if(dual)
+       if(mask&1)
+       {
+               data[0] = addr&0xFF;
+               data[1] = ((addr>>8)&0x7) | (on ? 0x40 : 0) | (pending_path&1 ? 0 : 0x80);
+               control.command(CMD_TURNOUT, data, 2).signal_done.connect(sigc::bind(sigc::mem_fun(this, &Turnout::command_reply), 1));
+               pending_cmds |= 1;
+       }
+       if(dual && (mask&2))
        {
                data[0] = (addr+1)&0xFF;
-               data[1] = (((addr+1)>>8)&0x7) | (on ? 0x40 : 0) | (path&2 ? 0 : 0x80);
-               control.command(CMD_TURNOUT, data, 2);
+               data[1] = (((addr+1)>>8)&0x7) | (on ? 0x40 : 0) | (pending_path&2 ? 0 : 0x80);
+               control.command(CMD_TURNOUT, data, 2).signal_done.connect(sigc::bind(sigc::mem_fun(this, &Turnout::command_reply), 2));
+               pending_cmds |= 2;
        }
 }
 
-void Turnout::status_reply(const Reply &reply, bool high)
+void Turnout::command_reply(const Reply &reply, unsigned char bit)
 {
+       pending_cmds &= ~bit;
        if(reply.get_error()==ERR_NO_ERROR)
        {
-               bool v = !(reply.get_data()[0]&0x04);
-               unsigned b = (high?2:1);
-               path = (path&~b)|(v?b:0);
-               signal_path_changed.emit(path);
+               if(on && !pending_cmds)
+               {
+                       path = pending_path;
+                       on = false;
+                       control.set_timer(500*Time::msec).signal_timeout.connect(
+                               sigc::bind_return(sigc::bind(sigc::mem_fun(this, &Turnout::command), 3), false));
+                       signal_path_changed.emit(path);
+               }
+       }
+       else if(reply.get_error()==ERR_NO_I2C_SPACE)
+       {
+               control.set_timer(100*Time::msec).signal_timeout.connect(
+                       sigc::bind_return(sigc::bind(sigc::mem_fun(this, &Turnout::command), bit), false));
        }
 }
 
-bool Turnout::switch_timeout()
+void Turnout::status_reply(const Reply &reply, unsigned char bit)
 {
-       command(false);
-
-       return false;
+       if(reply.get_error()==ERR_NO_ERROR)
+       {
+               bool v = !(reply.get_data()[0]&0x04);
+               path = (path&~bit)|(v?bit:0);
+               signal_path_changed.emit(path);
+       }
 }
 
 void Turnout::turnout_event(unsigned a, bool p)

diff --git a/source/libmarklin/turnout.h b/source/libmarklin/turnout.h
index 01d37377dd8603a695c16e8151b94562ea77aa88..c7a603c78e53db9224b75951aa3474d107c62338 100644 (file)
--- a/source/libmarklin/turnout.h
+++ b/source/libmarklin/turnout.h
@@ -28,19 +28,22 @@ public:
 private:
        Control &control;
        unsigned addr;
-       unsigned path;
+       unsigned char path;
+       unsigned char pending_path;
+       unsigned char pending_cmds;
        bool dual;
+       bool on;
 
 public:
        Turnout(Control &, unsigned, bool =false);
 
-       void set_path(unsigned);
+       void set_path(unsigned char);
        unsigned get_address() const { return addr; }
-       unsigned get_path() const { return path; }
+       unsigned char get_path() const { return path; }
 private:
-       void command(bool);
-       void status_reply(const Reply &, bool);
-       bool switch_timeout();
+       void command(unsigned char);
+       void command_reply(const Reply &, unsigned char);
+       void status_reply(const Reply &, unsigned char);
        void turnout_event(unsigned, bool);
 };