source/libmarklin/locomotive.cpp

   1 #include <msp/time/timer.h>
   2 #include <msp/time/units.h>
   3 #include "command.h"
   4 #include "constants.h"
   5 #include "control.h"
   6 #include "locomotive.h"
   7
   8 using namespace std;
   9 using namespace Msp;
  10
  11 namespace Marklin {
  12
  13 Locomotive::Locomotive(Control &c, unsigned a):
  14         control(c),
  15         addr(a),
  16         speed(0),
  17         reverse(false),
  18         funcs(0)
  19 {
  20         control.add_locomotive(this);
  21
  22         refresh_status();
  23 }
  24
  25 void Locomotive::set_speed(unsigned spd)
  26 {
  27         speed=min(spd, 14U);
  28
  29         send_command(false);
  30 }
  31
  32 void Locomotive::set_reverse(bool rev)
  33 {
  34         if(rev==reverse)
  35                 return;
  36
  37         if(speed)
  38         {
  39                 (new Time::Timer((500+speed*150)*Time::msec))->signal_timeout.connect(sigc::mem_fun(this, &Locomotive::reverse_timeout));
  40                 set_speed(0);
  41         }
  42         else
  43         {
  44                 reverse=rev;
  45                 send_command(false);
  46         }
  47 }
  48
  49 void Locomotive::set_function(unsigned func, bool state)
  50 {
  51         if(state)
  52                 funcs|=1<<func;
  53         else
  54                 funcs&=~(1<<func);
  55
  56         send_command(true);
  57 }
  58
  59 void Locomotive::refresh_status()
  60 {
  61         char cmd[3];
  62         cmd[0]=CMD_LOK_STATUS;
  63         cmd[1]=addr&0xFF;
  64         cmd[2]=(addr>>8)&0xFF;
  65         control.command(string(cmd,3)).signal_done.connect(sigc::mem_fun(this,&Locomotive::status_reply));
  66 }
  67
  68 void Locomotive::send_command(bool setf)
  69 {
  70         char cmd[16];
  71         cmd[0]=CMD_LOK;
  72         cmd[1]=addr&0xFF;
  73         cmd[2]=(addr>>8)&0xFF;
  74
  75         if(speed==0)
  76                 cmd[3]=0;
  77         else if(speed==1)
  78                 cmd[3]=2;
  79         else
  80                 cmd[3]=(speed*19-18)/2;
  81
  82         cmd[4]=(reverse?0:0x20) | ((funcs&1)?0x10:0);
  83
  84         if(setf)
  85         {
  86                 cmd[4]|=0x80;
  87                 for(unsigned i=0; i<4; ++i)
  88                         if((funcs>>i)&2)
  89                                 cmd[4]|=(1<<i);
  90         }
  91         control.command(string(cmd,5));
  92 }
  93
  94 void Locomotive::status_reply(Error err, const string &reply)
  95 {
  96         if(err==ERR_NO_ERROR)
  97         {
  98                 if((unsigned char)reply[0]<=1)
  99                         speed=0;
 100                 else
 101                         speed=(unsigned char)reply[0]*2/19+1;
 102                 reverse=(reply[1]&0x20)?false:true;
 103                 funcs=(reply[1]&0xF)<<1;
 104                 if(reply[1]&0x10)
 105                         funcs|=1;
 106         }
 107 }
 108
 109 bool Locomotive::reverse_timeout()
 110 {
 111         reverse=!reverse;
 112         send_command(true);
 113         return false;
 114 }
 115
 116 } // namespace Marklin