source/libmarklin/control.cpp

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