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         signal_speed_changed.emit(speed);
  32 }
  33
  34 void Locomotive::set_reverse(bool rev)
  35 {
  36         if(rev==reverse)
  37                 return;
  38
  39         if(speed)
  40         {
  41                 (new Time::Timer((500+speed*150)*Time::msec))->signal_timeout.connect(sigc::mem_fun(this, &Locomotive::reverse_timeout));
  42                 set_speed(0);
  43         }
  44         else
  45         {
  46                 reverse=rev;
  47                 send_command(false);
  48         }
  49 }
  50
  51 void Locomotive::set_function(unsigned func, bool state)
  52 {
  53         if(state)
  54                 funcs|=1<<func;
  55         else
  56                 funcs&=~(1<<func);
  57
  58         send_command(true);
  59 }
  60
  61 void Locomotive::refresh_status()
  62 {
  63         char cmd[3];
  64         cmd[0]=CMD_LOK_STATUS;
  65         cmd[1]=addr&0xFF;
  66         cmd[2]=(addr>>8)&0xFF;
  67         control.command(string(cmd, 3)).signal_done.connect(sigc::mem_fun(this, &Locomotive::status_reply));
  68 }
  69
  70 void Locomotive::send_command(bool setf)
  71 {
  72         char cmd[16];
  73         cmd[0]=CMD_LOK;
  74         cmd[1]=addr&0xFF;
  75         cmd[2]=(addr>>8)&0xFF;
  76
  77         if(speed==0)
  78                 cmd[3]=0;
  79         else if(speed==1)
  80                 cmd[3]=2;
  81         else
  82                 cmd[3]=(speed*19-18)/2;
  83
  84         cmd[4]=(reverse?0:0x20) | ((funcs&1)?0x10:0);
  85
  86         if(setf)
  87         {
  88                 cmd[4]|=0x80;
  89                 for(unsigned i=0; i<4; ++i)
  90                         if((funcs>>i)&2)
  91                                 cmd[4]|=(1<<i);
  92         }
  93         control.command(string(cmd,5));
  94 }
  95
  96 void Locomotive::status_reply(Error err, const string &reply)
  97 {
  98         if(err==ERR_NO_ERROR)
  99         {
 100                 if((unsigned char)reply[0]<=1)
 101                         speed=0;
 102                 else
 103                         speed=(unsigned char)reply[0]*2/19+1;
 104                 reverse=(reply[1]&0x20)?false:true;
 105                 funcs=(reply[1]&0xF)<<1;
 106                 if(reply[1]&0x10)
 107                         funcs|=1;
 108         }
 109 }
 110
 111 bool Locomotive::reverse_timeout()
 112 {
 113         reverse=!reverse;
 114         send_command(true);
 115         return false;
 116 }
 117
 118 } // namespace Marklin