Rewrite command/reply system

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

diff --git a/source/libmarklin/command.cpp b/source/libmarklin/command.cpp
index 9530cbabe78f22f40b3332bfbfe45053905445b4..dd1cdbc5d97b1137a30f162da4db7e6d438b832b 100644 (file)
--- a/source/libmarklin/command.cpp
+++ b/source/libmarklin/command.cpp
@@ -5,20 +5,41 @@ Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
+#include <cstring>
+#include <msp/strings/formatter.h>
 #include "command.h"
 
 using namespace std;
+using namespace Msp;
 
 namespace Marklin {
 
-Command::Command(const string &c):
+Command::Command(Cmd c, const unsigned char *d, unsigned l):
        cmd(c),
+       len(1),
        sent(false)
-{ }
+{
+       data[0]=cmd;
+       if(d)
+       {
+               memcpy(data+1, d, min(l, 127U));
+               len+=min(l, 127U);
+       }
+}
+
+void Command::send(int fd)
+{
+       write(fd, data, len);
+       sent=true;
+}
 
-void Command::set_sent(bool s)
+ostream &operator<<(ostream &out, const Command &cmd)
 {
-       sent=s;
+       out<<cmd.cmd;
+       for(unsigned i=1; i<cmd.len; ++i)
+               out<<format(" %02X", static_cast<int>(cmd.data[i]));
+
+       return out;
 }
 
 } // namespace Marklin