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 9cf8383bf1eadd997f5b58d4b08a68ee782e6a76..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,17 +63,15 @@ 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;
 }
 
 unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
@@ -76,12 +80,8 @@ unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const Ve
                throw invalid_argument("ArduControl::add_loco");
 
        Protocol proto = map_protocol(proto_name);
-
-       if(proto==MM)
-       {
-               if(addr>=80)
-                       throw invalid_argument("ArduControl::add_loco");
-       }
+       if(addr>protocol_info[proto].max_address)
+               throw invalid_argument("ArduControl::add_loco");
 
        Locomotive loco(proto, addr);
        insert_unique(locomotives, loco.id, loco);
@@ -99,6 +99,9 @@ void ArduControl::remove_loco(unsigned id)
 void ArduControl::set_loco_speed(unsigned id, unsigned speed)
 {
        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);
@@ -123,6 +126,9 @@ void ArduControl::set_loco_reverse(unsigned id, bool rev)
 void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
 {
        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)))
        {
@@ -657,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]);