Move double-address logic to drivers

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

diff --git a/source/libr2c2/intellibox.cpp b/source/libr2c2/intellibox.cpp
index 9ac448e466108f3e964ffc6b6f6bbd9151566faf..64e785582dce0f8895deda28493af96cb5f7974c 100644 (file)
--- a/source/libr2c2/intellibox.cpp
+++ b/source/libr2c2/intellibox.cpp
@@ -12,6 +12,8 @@ Distributed under the GPL
 #include <msp/time/units.h>
 #include <msp/time/utils.h>
 #include "intellibox.h"
+#include "tracktype.h"
+#include "vehicletype.h"
 
 using namespace std;
 using namespace Msp;
@@ -96,6 +98,7 @@ void Intellibox::halt(bool h)
 
 const char *Intellibox::enumerate_protocols(unsigned i) const
 {
+       ++i;
        if(i==MM)
                return "MM";
        else if(i==MM_27)
@@ -113,13 +116,19 @@ unsigned Intellibox::get_protocol_speed_steps(const string &proto_name) const
        return 0;
 }
 
-void Intellibox::add_loco(unsigned addr, const string &proto_name)
+void Intellibox::add_loco(unsigned addr, const string &proto_name, const VehicleType &type)
 {
        Protocol proto = map_protocol(proto_name);
 
        if(!locos.count(addr))
        {
-               locos[addr].protocol = proto;
+               Locomotive &loco = locos[addr];
+               loco.protocol = proto;
+               if(type.get_max_function()>4)
+               {
+                       loco.ext_func = true;
+                       locos[addr+1].protocol = NONE;
+               }
 
                unsigned char data[2];
                data[0] = addr&0xFF;
@@ -131,6 +140,9 @@ void Intellibox::add_loco(unsigned addr, const string &proto_name)
 void Intellibox::set_loco_speed(unsigned addr, unsigned speed)
 {
        Locomotive &loco = locos[addr];
+       if(loco.protocol==NONE)
+               return;
+
        if(speed==loco.speed)
        {
                if(loco.pending_half_step)
@@ -163,14 +175,14 @@ void Intellibox::set_loco_speed(unsigned addr, unsigned speed)
                        loco.pending_half_step = 0;
                loco.half_step_delay = Time::TimeStamp();
 
-               loco_command(addr, (speed+1)/2, loco.reverse, loco.funcs|0x100);
+               loco_command(addr, (speed+1)/2, loco.reverse, loco.funcs, false);
        }
-       else
+       else if(loco.protocol==MM)
        {
                if(speed>14)
                        speed = 14;
 
-               loco_command(addr, speed, loco.reverse, loco.funcs|0x100);
+               loco_command(addr, speed, loco.reverse, loco.funcs, false);
        }
        loco.speed = speed;
 }
@@ -178,56 +190,82 @@ void Intellibox::set_loco_speed(unsigned addr, unsigned speed)
 void Intellibox::set_loco_reverse(unsigned addr, bool rev)
 {
        Locomotive &loco = locos[addr];
-       if(rev==loco.reverse)
+       if(loco.protocol==NONE || rev==loco.reverse)
                return;
 
+       loco.speed = 0;
        loco.reverse = rev;
-       loco_command(addr, loco.speed, rev, loco.funcs|0x100);
+       loco_command(addr, 0, rev, loco.funcs, false);
 }
 
 void Intellibox::set_loco_function(unsigned addr, unsigned func, bool state)
 {
        Locomotive &loco = locos[addr];
+       if(loco.protocol==NONE)
+               return;
+
        if(state)
                loco.funcs |= 1<<func;
        else
                loco.funcs &= ~(1<<func);
-       loco_command(addr, loco.speed, loco.reverse, loco.funcs);
+       if(func<=4)
+               loco_command(addr, loco.speed, loco.reverse, loco.funcs, true);
+       else if(loco.ext_func && func<=8)
+               loco_command(addr+1, 0, false, (loco.funcs>>4)&0x1E, true);
        signal_loco_function.emit(addr, func, state);
 }
 
-void Intellibox::add_turnout(unsigned addr)
+void Intellibox::add_turnout(unsigned addr, const TrackType &type)
 {
        if(!turnouts.count(addr))
        {
-               turnouts[addr];
+               Turnout &turnout = turnouts[addr];
+               turnout.bits = type.get_state_bits();
 
                unsigned char data[2];
                data[0] = addr&0xFF;
                data[1] = (addr>>8)&0xFF;
                command(CMD_TURNOUT_STATUS, addr, data, 2);
+               for(unsigned i=1; i<turnout.bits; ++i)
+               {
+                       turnouts[addr+i].bits = 0;
+
+                       ++data[0];
+                       if(!data[0])
+                               ++data[1];
+                       command(CMD_TURNOUT_STATUS, addr+i, data, 2);
+               }
        }
 }
 
-void Intellibox::set_turnout(unsigned addr, bool state)
+void Intellibox::set_turnout(unsigned addr, unsigned state)
 {
        Turnout &turnout = turnouts[addr];
-       if(state==turnout.state || state==turnout.pending)
+       unsigned mask = (1<<turnout.bits)-1;
+       if(((state^turnout.state)&mask)==0 || ((state^turnout.pending)&mask)==0)
+       {
+               turnout.state = state;
+               turnout.pending = state;
+               signal_turnout.emit(addr, state);
                return;
+       }
 
+       turnout.state = (turnout.state&mask) | (state&~mask);
        turnout.pending = state;
        turnout.active = true;
        turnout.off_timeout = Time::TimeStamp();
 
-       turnout_command(addr, state, true);
+       for(unsigned i=0; i<turnout.bits; ++i)
+               if((state^turnout.state)&(1<<i))
+                       turnout_command(addr+i, state&(1<<i), true);
 }
 
-bool Intellibox::get_turnout(unsigned addr) const
+unsigned Intellibox::get_turnout(unsigned addr) const
 {
        map<unsigned, Turnout>::const_iterator i = turnouts.find(addr);
        if(i!=turnouts.end())
                return i->second.state;
-       return false;
+       return 0;
 }
 
 void Intellibox::add_sensor(unsigned addr)
@@ -263,7 +301,7 @@ void Intellibox::tick()
                        i->second.speed += i->second.pending_half_step;
                        i->second.pending_half_step = 0;
                        i->second.half_step_delay = Time::TimeStamp();
-                       loco_command(i->first, (i->second.speed+1)/2, i->second.reverse, i->second.funcs|0x100);
+                       loco_command(i->first, (i->second.speed+1)/2, i->second.reverse, i->second.funcs, false);
                }
 
        for(map<unsigned, Turnout>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
@@ -365,7 +403,7 @@ void Intellibox::command(Command cmd, unsigned addr, const unsigned char *data,
        queue.push_back(slot);
 }
 
-void Intellibox::loco_command(unsigned addr, unsigned speed, bool rev, unsigned funcs)
+void Intellibox::loco_command(unsigned addr, unsigned speed, bool rev, unsigned funcs, bool setf)
 {
        unsigned char data[4];
        data[0] = addr&0xFF;
@@ -380,7 +418,7 @@ void Intellibox::loco_command(unsigned addr, unsigned speed, bool rev, unsigned
        
        data[3] = (rev ? 0 : 0x20) | ((funcs&1) ? 0x10 : 0);
 
-       if(!(funcs&0x100))
+       if(setf)
                data[3] |= 0x80 | ((funcs>>1)&0xF);
 
        command(CMD_LOK, addr, data, 4);
@@ -498,9 +536,12 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
                {
                        unsigned addr = queue.front().addr;
                        Locomotive &loco = locos[addr];
-                       signal_loco_speed.emit(addr, loco.speed+loco.pending_half_step, loco.reverse);
-                       if(loco.pending_half_step)
-                               loco.half_step_delay = Time::now()+500*Time::msec;
+                       if(loco.protocol)
+                       {
+                               signal_loco_speed.emit(addr, loco.speed+loco.pending_half_step, loco.reverse);
+                               if(loco.pending_half_step)
+                                       loco.half_step_delay = Time::now()+500*Time::msec;
+                       }
                }
                else
                        error(cmd, err);
@@ -511,14 +552,21 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
                read_status(&err);
 
                unsigned addr = queue.front().addr;
+               unsigned mask = 1;
+               while(!turnouts[addr].bits)
+               {
+                       --addr;
+                       mask <<= 1;
+               }
                Turnout &turnout = turnouts[addr];
 
                if(err==ERR_NO_ERROR)
                {
-                       turnout.state = turnout.pending;
+                       turnout.state = (turnout.state&~mask) | (turnout.pending&mask);
                        if(turnout.active)
                        {
-                               signal_turnout.emit(addr, turnout.state);
+                               if(turnout.state==turnout.pending)
+                                       signal_turnout.emit(addr, turnout.state);
                                turnout.off_timeout = t+500*Time::msec;
                        }
                }
@@ -526,7 +574,7 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
                        queue.push_back(queue.front());
                else
                {
-                       turnout.pending = turnout.state;
+                       turnout.pending = (turnout.pending&~mask) | (turnout.state&mask);
                        error(cmd, err);
                }
        }
@@ -541,13 +589,19 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
                        read_all(&data, 1);
 
                        unsigned addr = queue.front().addr;
-                       bool state = data&0x04;
-
+                       unsigned mask = 1;
+                       while(!turnouts[addr].bits)
+                       {
+                               --addr;
+                               mask <<= 1;
+                       }
                        Turnout &turnout = turnouts[addr];
-                       if(state!=turnout.state)
+
+                       bool state = data&0x04;
+                       if(state!=((turnout.state&mask)!=0))
                        {
-                               turnout.state = state;
-                               turnout.pending = state;
+                               turnout.state = (turnout.state&~mask) | (state ? mask : 0);
+                               turnout.pending = turnout.state;
                                signal_turnout.emit(addr, turnout.state);
                        }
                }
@@ -686,6 +740,7 @@ void Intellibox::error(Command cmd, Error err)
 
 
 Intellibox::Locomotive::Locomotive():
+       ext_func(false),
        speed(0),
        reverse(false),
        funcs(0)
@@ -693,7 +748,8 @@ Intellibox::Locomotive::Locomotive():
 
 
 Intellibox::Turnout::Turnout():
-       state(false),
+       bits(1),
+       state(0),
        active(false),
        pending(false)
 { }