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