source/libmarklin/control.cpp

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