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         if(t>next_event_query)
 142         {
 143                 next_event_query=t+300*Time::msec;
 144                 command(string(1, CMD_EVENT)).signal_done.connect(sigc::mem_fun(this, &Control::event_query_done));
 145         }
 146
 147         if(poll_sensors)
 148         {
 149                 unsigned max_addr=(--sensors.end())->second->get_address();
 150                 string cmd(3, 0);
 151                 cmd[0]=CMD_SENSOR_PARAM_SET;
 152                 cmd[1]=0;
 153                 cmd[2]=(max_addr+7)/8;
 154                 command(cmd);
 155                 command(string(1, CMD_SENSOR_REPORT));
 156                 poll_sensors=false;
 157         }
 158
 159         if(queue.size() && queue.front().get_sent())
 160         {
 161                 pollfd pfd={serial_fd, POLLIN, 0};
 162                 if(poll(&pfd, 1, 0)>0)
 163                 {
 164                         string resp=read_reply((Cmd)(unsigned char)queue.front().get_command()[0]);
 165                         if(debug)
 166                         {
 167                                 printf("read:  ");
 168                                 for(unsigned i=0; i<resp.size(); ++i)
 169                                         printf("%02X ", (unsigned char)resp[i]);
 170                                 printf("(%d bytes)\n", resp.size());
 171                         }
 172
 173                         queue.front().signal_done.emit((Error)resp[0], resp.substr(1));
 174                         queue.erase(queue.begin());
 175                 }
 176                 else
 177                         return;
 178         }
 179
 180         if(queue.size())
 181         {
 182                 string cmd=queue.front().get_command();
 183
 184                 if(p50_enabled)
 185                 {
 186                         if(cmd[0]&0x80)
 187                                 cmd="X"+cmd;
 188                         else
 189                                 cmd="x"+cmd;
 190                 }
 191
 192                 if(debug)
 193                 {
 194                         printf("write: ");
 195                         for(unsigned i=0; i<cmd.size(); ++i)
 196                                 printf("%02X ",(unsigned char)cmd[i]);
 197                         printf("(%d bytes)\n",cmd.size());
 198                 }
 199
 200                 write(serial_fd, cmd.data(), cmd.size());
 201                 queue.front().set_sent(true);
 202         }
 203 }
 204
 205 Time::Timer::Slot &Control::set_timer(const Time::TimeDelta &dt)
 206 {
 207         return timer.add(dt);
 208 }
 209
 210 Control::~Control()
 211 {
 212         for(map<unsigned, Sensor *>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
 213                 delete i->second;
 214         for(map<unsigned, Turnout *>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
 215                 delete i->second;
 216         for(map<unsigned, Locomotive *>::iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
 217                 delete i->second;
 218         close(serial_fd);
 219 }
 220
 221 /*** private ***/
 222
 223 void Control::read_all(int fd, char *buf, int size)
 224 {
 225         int pos=0;
 226         while(pos<size)
 227         {
 228                 int len=read(fd, buf+pos, size-pos);
 229                 pos+=len;
 230         }
 231 }
 232
 233 string Control::read_reply(Cmd cmd)
 234 {
 235         string result;
 236         if(cmd==CMD_EVENT)
 237         {
 238                 result+=ERR_NO_ERROR;
 239                 for(unsigned i=0; i<3; ++i)
 240                 {
 241                         char c;
 242                         read(serial_fd, &c, 1);
 243                         result+=c;
 244                         if(!(c&0x80)) break;
 245                 }
 246         }
 247         else if(cmd==CMD_EVENT_LOK)
 248         {
 249                 result+=ERR_NO_ERROR;
 250                 char c[5];
 251                 read(serial_fd, c+4, 1);
 252                 result+=c[4];
 253                 while(c[4]&0x80)
 254                 {
 255                         read_all(serial_fd, c, 5);
 256                         result.append(c, 5);
 257                 }
 258         }
 259         else if(cmd==CMD_EVENT_TURNOUT)
 260         {
 261                 result+=ERR_NO_ERROR;
 262                 char c[511];
 263                 read(serial_fd, c, 1);
 264                 read_all(serial_fd, c+1, c[0]*2);
 265                 result.append(c, c[0]*2+1);
 266         }
 267         else if(cmd==CMD_EVENT_SENSOR)
 268         {
 269                 result+=ERR_NO_ERROR;
 270                 char c[3];
 271                 read(serial_fd, c+2, 1);
 272                 result+=c[2];
 273                 while(c[2])
 274                 {
 275                         read_all(serial_fd, c, 3);
 276                         result.append(c, 3);
 277                 }
 278         }
 279         else
 280         {
 281                 unsigned expected_bytes=1;
 282                 if(cmd==CMD_FUNC_STATUS || cmd==CMD_TURNOUT_STATUS)
 283                         expected_bytes=2;
 284                 if(cmd==CMD_SENSOR_STATUS || cmd==CMD_TURNOUT_GROUP_STATUS)
 285                         expected_bytes=3;
 286                 if(cmd==CMD_LOK_STATUS)
 287                         expected_bytes=4;
 288                 if(cmd==CMD_LOK_CONFIG)
 289                         expected_bytes=5;
 290                 char c[5];
 291                 read_all(serial_fd, c, 1);
 292                 result+=c[0];
 293                 if(!c[0])
 294                 {
 295                         read_all(serial_fd, c+1, expected_bytes-1);
 296                         result.append(c+1, expected_bytes-1);
 297                 }
 298         }
 299
 300         return result;
 301 }
 302
 303 void Control::event_query_done(Error, const string &resp)
 304 {
 305         if(resp[0]&0x20)
 306                 command(string(1, CMD_EVENT_TURNOUT)).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
 307         if(resp[0]&0x04)
 308                 command(string(1, CMD_EVENT_SENSOR)).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
 309 }
 310
 311 void Control::turnout_event_done(Error, const string &resp)
 312 {
 313         unsigned count=resp[0];
 314         for(unsigned i=0; i<count; ++i)
 315         {
 316                 unsigned addr=(unsigned char)resp[i*2+1]+((resp[i*2+2]&7)<<8);
 317                 bool status=!(resp[i*2+2]&0x80);
 318                 cout<<"Turnout "<<addr<<", status "<<status<<'\n';
 319                 signal_turnout_event.emit(addr, status);
 320         }
 321 }
 322
 323 void Control::sensor_event_done(Error, const string &resp)
 324 {
 325         for(unsigned i=0; resp[i]; i+=3)
 326         {
 327                 unsigned module=(unsigned char)resp[i];
 328                 cout<<"S88 module "<<module<<", status ";
 329                 for(unsigned j=0; j<16; ++j)
 330                 {
 331                         bool status=(resp[i+1+j/8]>>(7-j%8))&1;
 332                         cout<<status;
 333                         signal_sensor_event.emit(module*16+j-15, status);
 334                 }
 335                 cout<<'\n';
 336         }
 337 }
 338
 339 } // namespace Marklin