Read power state from arducontrol device on startup

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

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index a1d238103004d1e95cba46ffd65fb57ef7b6caf7..e5d94ef9d5d180e0bf3f7f5a5cfdcd847f48ae77 100644 (file)
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -9,6 +9,11 @@ using namespace Msp;
 
 namespace R2C2 {
 
+ArduControl::ProtocolInfo ArduControl::protocol_info[2] =
+{
+       { 79, 14, 4 }  // MM
+};
+
 ArduControl::ArduControl(const string &dev):
        serial(dev),
        debug(true),
@@ -19,6 +24,10 @@ ArduControl::ArduControl(const string &dev):
        n_s88_octets(0),
        thread(*this)
 {
+       QueuedCommand cmd;
+       cmd.command[0] = READ_POWER_STATE;
+       cmd.length = 1;
+       push_command(cmd);
 }
 
 ArduControl::~ArduControl()
@@ -28,17 +37,14 @@ ArduControl::~ArduControl()
 
 void ArduControl::set_power(bool p)
 {
-       if(p==power.pending)
-               return;
-
-       power.pending = p;
-       ++power.serial;
-
-       QueuedCommand cmd(POWER);
-       cmd.tag.serial = power.serial;
-       cmd.command[0] = (p ? POWER_ON : POWER_OFF);
-       cmd.length = 1;
-       push_command(cmd);
+       if(power.set(p))
+       {
+               QueuedCommand cmd(POWER);
+               cmd.tag.serial = power.serial;
+               cmd.command[0] = (p ? POWER_ON : POWER_OFF);
+               cmd.length = 1;
+               push_command(cmd);
+       }
 }
 
 void ArduControl::halt(bool)
@@ -57,45 +63,45 @@ ArduControl::Protocol ArduControl::map_protocol(const string &proto_name)
 {
        if(proto_name=="MM")
                return MM;
+       else if(proto_name=="MFX")
+               return MFX;
        else
                throw invalid_argument("ArduControl::map_protocol");
 }
 
 unsigned ArduControl::get_protocol_speed_steps(const string &proto_name) const
 {
-       Protocol proto = map_protocol(proto_name);
-       if(proto==MM)
-               return 14;
-       else
-               return 0;
+       return protocol_info[map_protocol(proto_name)].max_speed;
 }
 
-void ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
+unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
 {
        if(!addr)
                throw invalid_argument("ArduControl::add_loco");
 
        Protocol proto = map_protocol(proto_name);
+       if(addr>protocol_info[proto].max_address)
+               throw invalid_argument("ArduControl::add_loco");
 
-       if(proto==MM)
-       {
-               if(addr>=80)
-                       throw invalid_argument("ArduControl::add_loco");
-       }
+       Locomotive loco(proto, addr);
+       insert_unique(locomotives, loco.id, loco);
 
-       insert_unique(locomotives, addr, Locomotive(proto, addr));
+       return loco.id;
 }
 
-void ArduControl::remove_loco(unsigned addr)
+void ArduControl::remove_loco(unsigned id)
 {
-       Locomotive &loco = get_item(locomotives, addr);
+       Locomotive &loco = get_item(locomotives, id);
        remove_loco_from_refresh(loco);
-       locomotives.erase(addr);
+       locomotives.erase(id);
 }
 
-void ArduControl::set_loco_speed(unsigned addr, unsigned speed)
+void ArduControl::set_loco_speed(unsigned id, unsigned speed)
 {
-       Locomotive &loco = get_item(locomotives, addr);
+       Locomotive &loco = get_item(locomotives, id);
+       if(speed>protocol_info[loco.proto].max_speed)
+               throw invalid_argument("ArduControl::set_loco_speed");
+
        if(loco.speed.set(speed))
        {
                QueuedCommand cmd(loco, Locomotive::SPEED);
@@ -105,9 +111,9 @@ void ArduControl::set_loco_speed(unsigned addr, unsigned speed)
        }
 }
 
-void ArduControl::set_loco_reverse(unsigned addr, bool rev)
+void ArduControl::set_loco_reverse(unsigned id, bool rev)
 {
-       Locomotive &loco = get_item(locomotives, addr);
+       Locomotive &loco = get_item(locomotives, id);
        if(loco.reverse.set(rev))
        {
                QueuedCommand cmd(loco, Locomotive::REVERSE);
@@ -117,9 +123,12 @@ void ArduControl::set_loco_reverse(unsigned addr, bool rev)
        }
 }
 
-void ArduControl::set_loco_function(unsigned addr, unsigned func, bool state)
+void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
 {
-       Locomotive &loco = get_item(locomotives, addr);
+       Locomotive &loco = get_item(locomotives, id);
+       if(func>protocol_info[loco.proto].max_func)
+               throw invalid_argument("ArduControl::set_loco_function");
+
        unsigned mask = 1<<func;
        if(loco.funcs.set((loco.funcs&~mask)|(mask*state)))
        {
@@ -190,12 +199,12 @@ void ArduControl::advance_next_refresh()
        }
 }
 
-void ArduControl::add_turnout(unsigned addr, const TrackType &type)
+unsigned ArduControl::add_turnout(unsigned addr, const TrackType &type)
 {
        if(!addr || !type.is_turnout())
                throw invalid_argument("ArduControl::add_turnout");
 
-       add_accessory(Accessory::TURNOUT, addr, type.get_state_bits());
+       return add_accessory(Accessory::TURNOUT, addr, type.get_state_bits());
 }
 
 void ArduControl::remove_turnout(unsigned addr)
@@ -213,9 +222,9 @@ unsigned ArduControl::get_turnout(unsigned addr) const
        return get_accessory(Accessory::TURNOUT, addr);
 }
 
-void ArduControl::add_signal(unsigned addr, const SignalType &)
+unsigned ArduControl::add_signal(unsigned addr, const SignalType &)
 {
-       add_accessory(Accessory::SIGNAL, addr, 1);
+       return add_accessory(Accessory::SIGNAL, addr, 1);
 }
 
 void ArduControl::remove_signal(unsigned addr)
@@ -233,7 +242,7 @@ unsigned ArduControl::get_signal(unsigned addr) const
        return get_accessory(Accessory::SIGNAL, addr);
 }
 
-void ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits)
+unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits)
 {
        AccessoryMap::iterator i = accessories.lower_bound(addr);
        AccessoryMap::iterator j = accessories.upper_bound(addr+bits-1);
@@ -247,6 +256,7 @@ void ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bi
        }
 
        insert_unique(accessories, addr, Accessory(kind, addr, bits));
+       return addr;
 }
 
 void ArduControl::remove_accessory(Accessory::Kind kind, unsigned addr)
@@ -278,7 +288,7 @@ unsigned ArduControl::get_accessory(Accessory::Kind kind, unsigned addr) const
        return acc.state;
 }
 
-void ArduControl::add_sensor(unsigned addr)
+unsigned ArduControl::add_sensor(unsigned addr)
 {
        if(!addr)
                throw invalid_argument("ArduControl::add_sensor");
@@ -287,6 +297,8 @@ void ArduControl::add_sensor(unsigned addr)
        unsigned octet_index = (addr-1)/8;
        if(octet_index>=n_s88_octets)
                n_s88_octets = octet_index+1;
+
+       return addr;
 }
 
 void ArduControl::remove_sensor(unsigned addr)
@@ -314,7 +326,7 @@ void ArduControl::tick()
                }
                else if(tag.type==Tag::LOCOMOTIVE)
                {
-                       LocomotiveMap::iterator i = locomotives.find(tag.address);
+                       LocomotiveMap::iterator i = locomotives.find(tag.id);
                        if(i==locomotives.end())
                                continue;
 
@@ -322,12 +334,12 @@ void ArduControl::tick()
                        if(tag.command==Locomotive::SPEED)
                        {
                                if(loco.speed.commit(tag.serial))
-                                       signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
+                                       signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
                        }
                        else if(tag.command==Locomotive::REVERSE)
                        {
                                if(loco.reverse.commit(tag.serial))
-                                       signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
+                                       signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
                        }
                        else if(tag.command==Locomotive::FUNCTIONS)
                        {
@@ -337,13 +349,13 @@ void ArduControl::tick()
                                        unsigned changed = old^loco.funcs;
                                        for(unsigned j=0; changed>>j; ++j)
                                                if((changed>>j)&1)
-                                                       signal_loco_function.emit(loco.address, j, (loco.funcs>>j)&1);
+                                                       signal_loco_function.emit(loco.id, j, (loco.funcs>>j)&1);
                                }
                        }
                }
                else if(tag.type==Tag::ACCESSORY)
                {
-                       AccessoryMap::iterator i = accessories.find(tag.address);
+                       AccessoryMap::iterator i = accessories.find(tag.id);
                        if(i==accessories.end())
                                continue;
 
@@ -370,7 +382,7 @@ void ArduControl::tick()
                }
                else if(tag.type==Tag::SENSOR)
                {
-                       SensorMap::iterator i = sensors.find(tag.address);
+                       SensorMap::iterator i = sensors.find(tag.id);
                        if(i==sensors.end())
                                continue;
 
@@ -456,11 +468,12 @@ ArduControl::Tag::Tag():
        type(NONE),
        command(0),
        serial(0),
-       address(0)
+       id(0)
 { }
 
 
 ArduControl::Locomotive::Locomotive(Protocol p, unsigned a):
+       id((p<<16)|a),
        proto(p),
        address(a),
        speed(0),
@@ -515,7 +528,7 @@ unsigned ArduControl::Accessory::create_state_command(unsigned b, bool c, char *
                throw invalid_argument("Accessory::create_state_command");
 
        unsigned a = (address+b+3)*2;
-       if((state.pending>>b)&1)
+       if(!((state.pending>>b)&1))
                ++a;
        buffer[0] = MOTOROLA_SOLENOID;
        buffer[1] = a>>3;
@@ -650,6 +663,16 @@ void ArduControl::ControlThread::main()
                                        else if(tag && !repeat_count)
                                                control.push_completed_tag(tag);
                                }
+                               else if(type==POWER_STATE && rlength==2)
+                               {
+                                       control.power.set(reply[1]);
+
+                                       Tag ptag;
+                                       ptag.type = Tag::GENERAL;
+                                       ptag.command = POWER;
+                                       ptag.serial = control.power.serial;
+                                       control.push_completed_tag(ptag);
+                               }
                                else if(type==S88_DATA && rlength>2)
                                {
                                        unsigned offset = static_cast<unsigned char>(reply[1]);
@@ -667,7 +690,7 @@ void ArduControl::ControlThread::main()
                                                stag.type = Tag::SENSOR;
                                                stag.command = Sensor::STATE;
                                                stag.serial = i->second.state.serial;
-                                               stag.address = i->first;
+                                               stag.id = i->first;
                                                control.push_completed_tag(stag);
                                        }
 
@@ -697,7 +720,7 @@ ArduControl::QueuedCommand::QueuedCommand(Locomotive &loco, Locomotive::Command
 {
        tag.type = Tag::LOCOMOTIVE;
        tag.command = cmd;
-       tag.address = loco.address;
+       tag.id = loco.id;
        if(cmd==Locomotive::SPEED)
        {
                tag.serial = loco.speed.serial;
@@ -721,7 +744,7 @@ ArduControl::QueuedCommand::QueuedCommand(Accessory &acc, Accessory::Command cmd
 {
        tag.type = Tag::ACCESSORY;
        tag.command = cmd;
-       tag.address = acc.address;
+       tag.id = acc.address;
        if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE)
        {
                tag.serial = acc.state.serial;