Rewrite command/reply system

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

diff --git a/source/libmarklin/locomotive.cpp b/source/libmarklin/locomotive.cpp
index 87effa04e8e35175152bc5de2e88950532a5d55c..a42d8d7820f0c4b3ce18e3c0bf7bfc19a7a6e8f4 100644 (file)
--- a/source/libmarklin/locomotive.cpp
+++ b/source/libmarklin/locomotive.cpp
@@ -11,6 +11,7 @@ Distributed under the GPL
 #include "constants.h"
 #include "control.h"
 #include "locomotive.h"
+#include "reply.h"
 
 using namespace std;
 using namespace Msp;
@@ -70,50 +71,51 @@ void Locomotive::set_function(unsigned func, bool state)
 
 void Locomotive::refresh_status()
 {
-       char cmd[3];
-       cmd[0]=CMD_LOK_STATUS;
-       cmd[1]=addr&0xFF;
-       cmd[2]=(addr>>8)&0xFF;
-       control.command(string(cmd, 3)).signal_done.connect(sigc::mem_fun(this, &Locomotive::status_reply));
+       unsigned char data[2];
+       data[0]=addr&0xFF;
+       data[1]=(addr>>8)&0xFF;
+       control.command(CMD_LOK_STATUS, data, 2).signal_done.connect(sigc::mem_fun(this, &Locomotive::status_reply));
 }
 
 void Locomotive::send_command(bool setf)
 {
-       char cmd[16];
-       cmd[0]=CMD_LOK;
-       cmd[1]=addr&0xFF;
-       cmd[2]=(addr>>8)&0xFF;
+       unsigned char data[4];
+       data[0]=addr&0xFF;
+       data[1]=(addr>>8)&0xFF;
 
        if(speed==0)
-               cmd[3]=0;
+               data[2]=0;
        else if(speed==1)
-               cmd[3]=2;
+               data[2]=2;
        else
-               cmd[3]=(speed*19-18)/2;
+               data[2]=(speed*19-18)/2;
        
-       cmd[4]=(reverse?0:0x20) | ((funcs&1)?0x10:0);
+       data[3]=(reverse ? 0 : 0x20) | ((funcs&1) ? 0x10 : 0);
 
        if(setf)
        {
-               cmd[4]|=0x80;
+               data[3]|=0x80;
                for(unsigned i=0; i<4; ++i)
                        if((funcs>>i)&2)
-                               cmd[4]|=(1<<i);
+                               data[3]|=(1<<i);
        }
-       control.command(string(cmd, 5));
+       control.command(CMD_LOK, data, 4);
 }
 
-void Locomotive::status_reply(Error err, const string &reply)
+void Locomotive::status_reply(const Reply &reply)
 {
-       if(err==ERR_NO_ERROR)
+       if(reply.get_error()==ERR_NO_ERROR)
        {
-               if(static_cast<unsigned char>(reply[0])<=1)
+               const unsigned char *data=reply.get_data();
+
+               if(data[0]<=1)
                        speed=0;
                else
-                       speed=static_cast<unsigned char>(reply[0])*2/19+1;
-               reverse=(reply[1]&0x20)?false:true;
-               funcs=(reply[1]&0xF)<<1;
-               if(reply[1]&0x10)
+                       speed=data[0]*2/19+1;
+
+               reverse=(data[1]&0x20) ? false : true;
+               funcs=(data[1]&0xF)<<1;
+               if(data[1]&0x10)
                        funcs|=1;
 
                for(unsigned i=0; i<5; ++i)