source/libmarklin/control.cpp

   1 #include <fcntl.h>
   2 #include <termios.h>
   3 #include <sys/poll.h>
   4 #include <iostream>
   5 #include <msp/core/error.h>
   6 #include <msp/time/units.h>
   7 #include <msp/time/utils.h>
   8 #include "command.h"
   9 #include "control.h"
  10
  11 using namespace std;
  12 using namespace Msp;
  13
  14 namespace Marklin {
  15
  16 Control::Control():
  17         serial_fd(-1),
  18         p50_enabled(false),
  19         power(true),
  20         poll_sensors(false),
  21         debug(false)
  22 { }
  23
  24 void Control::set_power(bool p)
  25 {
  26         power=p;
  27         if(power)
  28                 command(string(1, CMD_POWER_ON));
  29         else
  30                 command(string(1, CMD_POWER_OFF));
  31 }
  32
  33 Turnout *Control::get_turnout(unsigned id) const
  34 {
  35         TurnoutMap::const_iterator i=turnouts.find(id);
  36         if(i!=turnouts.end())
  37                 return i->second;
  38
  39         return 0;
  40 }
  41
  42 Locomotive *Control::get_locomotive(unsigned id) const
  43 {
  44         for(LocomotiveSeq::const_iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
  45                 if((*i)->get_address()==id)
  46                         return *i;
  47         return 0;
  48 }
  49
  50 Sensor *Control::get_sensor(unsigned id) const
  51 {
  52         SensorMap::const_iterator i=sensors.find(id);
  53         if(i!=sensors.end())
  54                 return i->second;
  55
  56         return 0;
  57 }
  58
  59 void Control::open(const string &dev)
  60 {
  61         serial_fd=::open(dev.c_str(), O_RDWR);
  62         if(serial_fd<0)
  63                 throw Exception("Couldn't open serial port\n");
  64
  65         static unsigned baud[]=
  66         {
  67                  2400, B2400,
  68                  4800, B4800,
  69                  9600, B9600,
  70                 19200, B19200,
  71                 0
  72         };
  73
  74         termios attr;
  75         tcgetattr(serial_fd, &attr);
  76         cfmakeraw(&attr);
  77         attr.c_cflag|=CSTOPB;
  78
  79         bool ok=false;
  80         for(unsigned i=0; baud[i]; i+=2)
  81         {
  82                 cfsetospeed(&attr, baud[i+1]);
  83                 tcsetattr(serial_fd, TCSADRAIN, &attr);
  84
  85                 write(serial_fd, "\xC4", 1);
  86
  87                 pollfd pfd={serial_fd, POLLIN, 0};
  88                 if(poll(&pfd, 1, 500)>0)
  89                 {
  90                         cout<<"IB detected at "<<baud[i]<<" bits/s, P50 is ";
  91                         char buf[2];
  92                         if(read(serial_fd, buf, 2)==2)
  93                         {
  94                                 p50_enabled=true;
  95                                 cout<<"enabled";
  96                         }
  97                         else
  98                         {
  99                                 p50_enabled=false;
 100                                 cout<<"disabled";
 101                         }
 102                         cout<<'\n';
 103                         ok=true;
 104                         break;
 105                 }
 106         }
 107         if(!ok)
 108                 throw Exception("IB not detected");
 109 }
 110
 111 Command &Control::command(const string &cmd)
 112 {
 113         queue.push_back(Command(cmd));
 114         return queue.back();
 115 }
 116
 117 void Control::add_turnout(Turnout *t)
 118 {
 119         if(turnouts.count(t->get_address())==0)
 120                 turnouts.insert(TurnoutMap::value_type(t->get_address(), t));
 121 }
 122
 123 void Control::add_locomotive(Locomotive *l)
 124 {
 125         if(find(locomotives.begin(), locomotives.end(), l)==locomotives.end())
 126                 locomotives.push_back(l);
 127 }
 128
 129 void Control::add_sensor(Sensor *s)
 130 {
 131         if(sensors.count(s->get_address())==0)
 132         {
 133                 sensors.insert(SensorMap::value_type(s->get_address(), s));
 134                 poll_sensors=true;
 135         }
 136 }
 137
 138 void Control::tick()
 139 {
 140         const Time::TimeStamp t=Time::now();
 141
 142         if(t>next_event_query)
 143         {
 144                 next_event_query=t+300*Time::msec;
 145                 command(string(1, CMD_EVENT)).signal_done.connect(sigc::mem_fun(this, &Control::event_query_done));
 146         }
 147
 148         if(poll_sensors)
 149         {
 150                 unsigned max_addr=(--sensors.end())->second->get_address();
 151                 string cmd(3, 0);
 152                 cmd[0]=CMD_SENSOR_PARAM_SET;
 153                 cmd[1]=0;
 154                 cmd[2]=(max_addr+7)/8;
 155                 command(cmd);
 156                 command(string(1, CMD_SENSOR_REPORT));
 157                 poll_sensors=false;
 158         }
 159
 160         if(queue.size() && queue.front().get_sent())
 161         {
 162                 pollfd pfd={serial_fd, POLLIN, 0};
 163                 if(poll(&pfd, 1, 0)>0)
 164                 {
 165                         string resp=read_reply((Cmd)(unsigned char)queue.front().get_command()[0]);
 166                         if(debug)
 167                         {
 168                                 printf("read:  ");
 169                                 for(unsigned i=0; i<resp.size(); ++i)
 170                                         printf("%02X ", (unsigned char)resp[i]);
 171                                 printf("(%d bytes)\n", resp.size());
 172                         }
 173
 174                         queue.front().signal_done.emit((Error)resp[0], resp.substr(1));
 175                         queue.erase(queue.begin());
 176                 }
 177                 else
 178                         return;
 179         }
 180
 181         if(queue.size())
 182         {
 183                 string cmd=queue.front().get_command();
 184
 185                 if(p50_enabled)
 186                 {
 187                         if(cmd[0]&0x80)
 188                                 cmd="X"+cmd;
 189                         else
 190                                 cmd="x"+cmd;
 191                 }
 192
 193                 if(debug)
 194                 {
 195                         printf("write: ");
 196                         for(unsigned i=0; i<cmd.size(); ++i)
 197                                 printf("%02X ",(unsigned char)cmd[i]);
 198                         printf("(%d bytes)\n",cmd.size());
 199                 }
 200
 201                 write(serial_fd, cmd.data(), cmd.size());
 202                 queue.front().set_sent(true);
 203         }
 204 }
 205
 206 Control::~Control()
 207 {
 208         for(SensorMap::iterator i=sensors.begin(); i!=sensors.end(); ++i)
 209                 delete i->second;
 210         for(TurnoutMap::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
 211                 delete i->second;
 212         for(LocomotiveSeq::iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
 213                 delete *i;
 214         close(serial_fd);
 215 }
 216
 217 /*** private ***/
 218
 219 void Control::read_all(int fd, char *buf, int size)
 220 {
 221         int pos=0;
 222         while(pos<size)
 223         {
 224                 int len=read(fd, buf+pos, size-pos);
 225                 pos+=len;
 226         }
 227 }
 228
 229 string Control::read_reply(Cmd cmd)
 230 {
 231         string result;
 232         if(cmd==CMD_EVENT)
 233         {
 234                 result+=ERR_NO_ERROR;
 235                 for(unsigned i=0; i<3; ++i)
 236                 {
 237                         char c;
 238                         read(serial_fd, &c, 1);
 239                         result+=c;
 240                         if(!(c&0x80)) break;
 241                 }
 242         }
 243         else if(cmd==CMD_EVENT_LOK)
 244         {
 245                 result+=ERR_NO_ERROR;
 246                 char c[5];
 247                 read(serial_fd, c+4, 1);
 248                 result+=c[4];
 249                 while(c[4]&0x80)
 250                 {
 251                         read_all(serial_fd, c, 5);
 252                         result.append(c, 5);
 253                 }
 254         }
 255         else if(cmd==CMD_EVENT_TURNOUT)
 256         {
 257                 result+=ERR_NO_ERROR;
 258                 char c[511];
 259                 read(serial_fd, c, 1);
 260                 read_all(serial_fd, c+1, c[0]*2);
 261                 result.append(c, c[0]*2+1);
 262         }
 263         else if(cmd==CMD_EVENT_SENSOR)
 264         {
 265                 result+=ERR_NO_ERROR;
 266                 char c[3];
 267                 read(serial_fd, c+2, 1);
 268                 result+=c[2];
 269                 while(c[2])
 270                 {
 271                         read_all(serial_fd, c, 3);
 272                         result.append(c, 3);
 273                 }
 274         }
 275         else
 276         {
 277                 unsigned expected_bytes=1;
 278                 if(cmd==CMD_FUNC_STATUS || cmd==CMD_TURNOUT_STATUS)
 279                         expected_bytes=2;
 280                 if(cmd==CMD_SENSOR_STATUS || cmd==CMD_TURNOUT_GROUP_STATUS)
 281                         expected_bytes=3;
 282                 if(cmd==CMD_LOK_STATUS)
 283                         expected_bytes=4;
 284                 if(cmd==CMD_LOK_CONFIG)
 285                         expected_bytes=5;
 286                 char c[5];
 287                 read_all(serial_fd, c, 1);
 288                 result+=c[0];
 289                 if(!c[0])
 290                 {
 291                         read_all(serial_fd, c+1, expected_bytes-1);
 292                         result.append(c+1, expected_bytes-1);
 293                 }
 294         }
 295
 296         return result;
 297 }
 298
 299 void Control::event_query_done(Error, const string &resp)
 300 {
 301         if(resp[0]&0x20)
 302                 command(string(1, CMD_EVENT_TURNOUT)).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
 303         if(resp[0]&0x04)
 304                 command(string(1, CMD_EVENT_SENSOR)).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
 305 }
 306
 307 void Control::turnout_event_done(Error, const string &resp)
 308 {
 309         unsigned count=resp[0];
 310         for(unsigned i=0; i<count; ++i)
 311         {
 312                 unsigned addr=(unsigned char)resp[i*2+1]+((resp[i*2+2]&7)<<8);
 313                 bool status=!(resp[i*2+2]&0x80);
 314                 cout<<"Turnout "<<addr<<", status "<<status<<'\n';
 315                 signal_turnout_event.emit(addr, status);
 316         }
 317 }
 318
 319 void Control::sensor_event_done(Error, const string &resp)
 320 {
 321         for(unsigned i=0; resp[i]; i+=3)
 322         {
 323                 unsigned module=(unsigned char)resp[i];
 324                 cout<<"S88 module "<<module<<", status ";
 325                 for(unsigned j=0; j<16; ++j)
 326                 {
 327                         bool status=(resp[i+1+j/8]>>(7-j%8))&1;
 328                         cout<<status;
 329                         signal_sensor_event.emit(module*16+j-15, status);
 330                 }
 331                 cout<<'\n';
 332         }
 333 }
 334
 335 } // namespace Marklin