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