Forgot to add the new files

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

diff --git a/source/libmarklin/locomotive.cpp b/source/libmarklin/locomotive.cpp
index 3088d4655ba3d100b2c64d2c704782e4e3c04e57..5fe5ca5f863d07ca489cdea2bae773eb46ba9d8e 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 "locotype.h"
 #include "reply.h"
 
 using namespace std;
@@ -34,6 +35,8 @@ Locomotive::Locomotive(const LocoType &t, Control &c, unsigned a):
 void Locomotive::set_speed(unsigned spd)
 {
        spd = min(spd, 14U);
+       if(spd==speed)
+               return;
        signal_speed_changing.emit(spd);
 
        speed = spd;
@@ -103,6 +106,18 @@ void Locomotive::send_command(bool setf)
                                data[3] |= (1<<i);
        }
        control.command(CMD_LOK, data, 4);
+
+       if(setf && type.get_max_function()>4)
+       {
+               if(!++data[0])
+                       ++data[1];
+               data[2] = 0;
+               data[3] = 0xA0;
+               for(unsigned i=0; i<4; ++i)
+                       if((funcs>>i)&32)
+                               data[3] |= (1<<i);
+               control.command(CMD_LOK, data, 4);
+       }
 }
 
 void Locomotive::status_reply(const Reply &reply)