source/libmarklin/intellibox.cpp

   1 /* $Id$
   2
   3 This file is part of the MSP Märklin suite
   4 Copyright © 2010  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 <msp/io/print.h>
  12 #include <msp/time/units.h>
  13 #include <msp/time/utils.h>
  14 #include "intellibox.h"
  15
  16 using namespace std;
  17 using namespace Msp;
  18
  19 namespace Marklin {
  20
  21 Intellibox::Intellibox(const string &dev):
  22         power(false),
  23         update_sensors(false),
  24         command_sent(false)
  25 {
  26         serial_fd = ::open(dev.c_str(), O_RDWR);
  27         if(serial_fd<0)
  28                 throw Exception("Couldn't open serial port\n");
  29
  30         static unsigned baud[]=
  31         {
  32                  2400, B2400,
  33                  4800, B4800,
  34                  9600, B9600,
  35                 19200, B19200,
  36                 0
  37         };
  38
  39         termios attr;
  40         tcgetattr(serial_fd, &attr);
  41         cfmakeraw(&attr);
  42         attr.c_cflag |= CSTOPB;
  43
  44         bool ok = false;
  45         bool p50 = false;
  46         for(unsigned i=0; baud[i]; i+=2)
  47         {
  48                 cfsetospeed(&attr, baud[i+1]);
  49                 tcsetattr(serial_fd, TCSADRAIN, &attr);
  50
  51                 write(serial_fd, "\xC4", 1);
  52
  53                 pollfd pfd = { serial_fd, POLLIN, 0 };
  54                 if(poll(&pfd, 1, 500)>0)
  55                 {
  56                         IO::print("IB detected at %d bits/s\n", baud[i]);
  57                         char buf[2];
  58                         p50 = (read(serial_fd, buf, 2)==2);
  59                         ok = true;
  60                         break;
  61                 }
  62         }
  63
  64         if(!ok)
  65                 throw Exception("IB not detected");
  66
  67         if(p50)
  68                 write(serial_fd, "xZzA1\r", 6);
  69
  70         command(CMD_STATUS);
  71 }
  72
  73 void Intellibox::set_power(bool p)
  74 {
  75         power = p;
  76         if(power)
  77                 command(CMD_POWER_ON);
  78         else
  79                 command(CMD_POWER_OFF);
  80         signal_power.emit(power);
  81 }
  82
  83 void Intellibox::add_loco(unsigned addr)
  84 {
  85         if(!locos.count(addr))
  86         {
  87                 locos[addr];
  88
  89                 unsigned char data[2];
  90                 data[0] = addr&0xFF;
  91                 data[1] = (addr>>8)&0xFF;
  92                 command(CMD_LOK_STATUS, addr, data, 2);
  93         }
  94 }
  95
  96 void Intellibox::set_loco_speed(unsigned addr, unsigned speed)
  97 {
  98         Locomotive &loco = locos[addr];
  99         if(speed==loco.speed)
 100                 return;
 101
 102         loco.speed = speed;
 103         loco_command(addr, speed, loco.reverse, loco.funcs|0x100);
 104 }
 105
 106 void Intellibox::set_loco_reverse(unsigned addr, bool rev)
 107 {
 108         Locomotive &loco = locos[addr];
 109         if(rev==loco.reverse)
 110                 return;
 111
 112         loco.reverse = rev;
 113         loco_command(addr, loco.speed, rev, loco.funcs|0x100);
 114 }
 115
 116 void Intellibox::set_loco_function(unsigned addr, unsigned func, bool state)
 117 {
 118         Locomotive &loco = locos[addr];
 119         if(state)
 120                 loco.funcs |= 1<<func;
 121         else
 122                 loco.funcs &= ~(1<<func);
 123         loco_command(addr, loco.speed, loco.reverse, loco.funcs);
 124         signal_loco_function.emit(addr, func, state);
 125 }
 126
 127 void Intellibox::add_turnout(unsigned addr)
 128 {
 129         if(!turnouts.count(addr))
 130         {
 131                 turnouts[addr];
 132
 133                 unsigned char data[2];
 134                 data[0] = addr&0xFF;
 135                 data[1] = (addr>>8)&0xFF;
 136                 command(CMD_TURNOUT_STATUS, addr, data, 2);
 137         }
 138 }
 139
 140 void Intellibox::set_turnout(unsigned addr, bool state)
 141 {
 142         Turnout &turnout = turnouts[addr];
 143         if(state==turnout.state || state==turnout.pending)
 144                 return;
 145
 146         turnout.pending = state;
 147         turnout.active = true;
 148
 149         turnout_command(addr, state, true);
 150 }
 151
 152 bool Intellibox::get_turnout(unsigned addr) const
 153 {
 154         map<unsigned, Turnout>::const_iterator i = turnouts.find(addr);
 155         if(i!=turnouts.end())
 156                 return i->second.state;
 157         return false;
 158 }
 159
 160 void Intellibox::add_sensor(unsigned addr)
 161 {
 162         if(!sensors.count(addr))
 163         {
 164                 sensors[addr];
 165                 update_sensors = true;
 166         }
 167 }
 168
 169 bool Intellibox::get_sensor(unsigned addr) const
 170 {
 171         map<unsigned, Sensor>::const_iterator i = sensors.find(addr);
 172         if(i!=sensors.end())
 173                 return i->second.state;
 174         return false;
 175 }
 176
 177 void Intellibox::tick()
 178 {
 179         const Time::TimeStamp t = Time::now();
 180
 181         if(t>next_event_query)
 182         {
 183                 next_event_query = t+200*Time::msec;
 184                 command(CMD_EVENT);
 185         }
 186
 187         for(map<unsigned, Turnout>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
 188                 if(i->second.active && i->second.off_timeout && t>i->second.off_timeout)
 189                 {
 190                         i->second.active = false;
 191                         i->second.off_timeout = Time::TimeStamp();
 192                         turnout_command(i->first, i->second.state, false);
 193                 }
 194
 195         for(map<unsigned, Sensor>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
 196                 if(i->second.off_timeout && t>i->second.off_timeout)
 197                 {
 198                         i->second.state = false;
 199                         i->second.off_timeout = Time::TimeStamp();
 200                         signal_sensor.emit(i->first, false);
 201                 }
 202
 203         if(update_sensors)
 204         {
 205                 unsigned max_addr = (--sensors.end())->first;
 206                 unsigned char data[2];
 207                 data[0] = 0;
 208                 data[1] = (max_addr+7)/8;
 209                 command(CMD_SENSOR_PARAM_SET, data, 2);
 210                 command(CMD_SENSOR_REPORT);
 211                 update_sensors = false;
 212         }
 213
 214         if(!queue.empty() && command_sent)
 215         {
 216                 pollfd pfd = { serial_fd, POLLIN, 0 };
 217                 if(poll(&pfd, 1, 0)>0)
 218                 {
 219                         process_reply(t);
 220                         queue.erase(queue.begin());
 221                         command_sent = false;
 222                 }
 223                 else
 224                         return;
 225         }
 226
 227         if(!queue.empty())
 228         {
 229                 const CommandSlot &slot = queue.front();
 230                 write(serial_fd, slot.data, slot.length);
 231                 command_sent = true;
 232         }
 233 }
 234
 235 void Intellibox::flush()
 236 {
 237         Time::TimeStamp t = Time::now();
 238         for(list<CommandSlot>::iterator i=queue.begin(); i!=queue.end(); ++i)
 239         {
 240                 write(serial_fd, i->data, i->length);
 241                 pollfd pfd = { serial_fd, POLLIN, 0 };
 242                 bool first = true;
 243                 while(poll(&pfd, 1, (first ? -1 : 0))>0)
 244                 {
 245                         char data[16];
 246                         read(serial_fd, data, 16);
 247                         first = false;
 248                 }
 249         }
 250
 251         queue.clear();
 252         command_sent = false;
 253 }
 254
 255 void Intellibox::command(Command cmd)
 256 {
 257         command(cmd, 0, 0);
 258 }
 259
 260 void Intellibox::command(Command cmd, const unsigned char *data, unsigned len)
 261 {
 262         command(cmd, 0, data, len);
 263 }
 264
 265 void Intellibox::command(Command cmd, unsigned addr, const unsigned char *data, unsigned len)
 266 {
 267         CommandSlot slot;
 268         slot.cmd = cmd;
 269         slot.addr = addr;
 270         slot.data[0] = cmd;
 271         copy(data, data+len, slot.data+1);
 272         slot.length = 1+len;
 273         queue.push_back(slot);
 274 }
 275
 276 void Intellibox::loco_command(unsigned addr, unsigned speed, bool rev, unsigned funcs)
 277 {
 278         unsigned char data[4];
 279         data[0] = addr&0xFF;
 280         data[1] = (addr>>8)&0xFF;
 281
 282         if(speed==0)
 283                 data[2] = 0;
 284         else if(speed==1)
 285                 data[2] = 2;
 286         else
 287                 data[2] = (speed*19-18)/2;
 288
 289         data[3] = (rev ? 0 : 0x20) | ((funcs&1) ? 0x10 : 0);
 290
 291         if(!(funcs&0x100))
 292                 data[3] |= 0x80 | ((funcs>>1)&0xF);
 293
 294         command(CMD_LOK, addr, data, 4);
 295 }
 296
 297 void Intellibox::turnout_command(unsigned addr, bool state, bool active)
 298 {
 299         unsigned char data[2];
 300         data[0] = addr&0xFF;
 301         data[1] = ((addr>>8)&0x7) | (active ? 0x40 : 0) | (state ? 0x80 : 0);
 302         command(CMD_TURNOUT, addr, data, 2);
 303 }
 304
 305 void Intellibox::process_reply(const Time::TimeStamp &t)
 306 {
 307         Command cmd = queue.front().cmd;
 308
 309         if(cmd==CMD_STATUS)
 310         {
 311                 unsigned char status;
 312                 read_all(&status, 1);
 313                 power = status&0x08;
 314                 signal_power.emit(power);
 315         }
 316         else if(cmd==CMD_EVENT)
 317         {
 318                 for(unsigned i=0;; ++i)
 319                 {
 320                         unsigned char byte;
 321                         read_all(&byte, 1);
 322
 323                         if(i==0)
 324                         {
 325                                 if(byte&0x01)
 326                                         command(CMD_EVENT_LOK);
 327                                 if(byte&0x20)
 328                                         command(CMD_EVENT_TURNOUT);
 329                                 if(byte&0x04)
 330                                         command(CMD_EVENT_SENSOR);
 331                         }
 332                         else if(i==1)
 333                         {
 334                                 if(byte&0x40)
 335                                         command(CMD_STATUS);
 336                         }
 337
 338                         if(!(byte&0x80))
 339                                 break;
 340                 }
 341         }
 342         else if(cmd==CMD_EVENT_LOK)
 343         {
 344                 while(1)
 345                 {
 346                         unsigned char data[5];
 347                         read_all(data, 1);
 348                         if(data[0]==0x80)
 349                                 break;
 350                         read_all(data+1, 4);
 351                 }
 352         }
 353         else if(cmd==CMD_EVENT_TURNOUT)
 354         {
 355                 unsigned char count;
 356                 read_all(&count, 1);
 357                 for(unsigned i=0; i<count; ++i)
 358                 {
 359                         unsigned char data[2];
 360                         read_all(data, 2);
 361
 362                         unsigned addr = data[0]+((data[1]&7)<<8);
 363                         signal_turnout.emit(addr, (data[1]&0x80)!=0);
 364                 }
 365         }
 366         else if(cmd==CMD_EVENT_SENSOR)
 367         {
 368                 while(1)
 369                 {
 370                         unsigned char mod;
 371                         read_all(&mod, 1);
 372                         if(!mod)
 373                                 break;
 374
 375                         unsigned char data[2];
 376                         read_all(data, 2);
 377                         for(unsigned i=0; i<16; ++i)
 378                         {
 379                                 unsigned addr = mod*16+i-15;
 380                                 bool state = (data[i/8]>>(7-i%8))&1;
 381
 382                                 Sensor &sensor = sensors[addr];
 383                                 if(state)
 384                                 {
 385                                         sensor.off_timeout = Time::TimeStamp();
 386                                         if(!sensor.state)
 387                                         {
 388                                                 sensor.state = state;
 389                                                 signal_sensor(addr, state);
 390                                         }
 391                                 }
 392                                 else if(sensor.state)
 393                                         sensor.off_timeout = t+700*Time::msec;
 394                         }
 395                 }
 396         }
 397         else if(cmd==CMD_LOK)
 398         {
 399                 unsigned char err;
 400                 read_all(&err, 1);
 401
 402                 if(err==ERR_NO_ERROR)
 403                 {
 404                         unsigned addr = queue.front().addr;
 405                         Locomotive &loco = locos[addr];
 406                         signal_loco_speed.emit(addr, loco.speed, loco.reverse);
 407                 }
 408         }
 409         else if(cmd==CMD_TURNOUT)
 410         {
 411                 unsigned char err;
 412                 read_all(&err, 1);
 413
 414                 if(err==ERR_NO_ERROR)
 415                 {
 416                         unsigned addr = queue.front().addr;
 417                         Turnout &turnout = turnouts[addr];
 418                         turnout.state = turnout.pending;
 419                         if(turnout.active)
 420                         {
 421                                 signal_turnout.emit(addr, turnout.state);
 422                                 turnout.off_timeout = t+500*Time::msec;
 423                         }
 424                 }
 425                 else if(err==ERR_NO_I2C_SPACE)
 426                         queue.push_back(queue.front());
 427         }
 428         else if(cmd==CMD_TURNOUT_STATUS)
 429         {
 430                 unsigned char err;
 431                 read_all(&err, 1);
 432
 433                 if(err==ERR_NO_ERROR)
 434                 {
 435                         unsigned char data;
 436                         read_all(&data, 1);
 437
 438                         unsigned addr = queue.front().addr;
 439                         bool state = data&0x04;
 440
 441                         Turnout &turnout = turnouts[addr];
 442                         if(state!=turnout.state)
 443                         {
 444                                 turnout.state = state;
 445                                 signal_turnout.emit(addr, turnout.state);
 446                         }
 447                 }
 448         }
 449         else if(cmd==CMD_LOK_STATUS)
 450         {
 451                 unsigned char err;
 452                 read_all(&err, 1);
 453
 454                 if(err==ERR_NO_ERROR)
 455                 {
 456                         unsigned char data[3];
 457                         read_all(data, 3);
 458
 459                         unsigned addr = queue.front().addr;
 460                         Locomotive &loco = locos[addr];
 461
 462                         unsigned speed = (data[0]<=1 ? 0 : data[0]*2/19+1);
 463                         bool reverse = !(data[1]&0x20);
 464                         if(speed!=loco.speed || reverse!=loco.reverse)
 465                         {
 466                                 loco.speed = speed;
 467                                 loco.reverse = reverse;
 468                                 signal_loco_speed.emit(addr, loco.speed, loco.reverse);
 469                         }
 470
 471                         unsigned funcs = (data[1]&0xF)<<1;
 472                         if(data[1]&0x10)
 473                                 funcs |= 1;
 474                         if(funcs!=loco.funcs)
 475                         {
 476                                 unsigned changed = loco.funcs^funcs;
 477                                 loco.funcs = funcs;
 478                                 for(unsigned i=0; i<5; ++i)
 479                                         if(changed&(1<<i))
 480                                                 signal_loco_function.emit(addr, i, loco.funcs&(1<<i));
 481                         }
 482                 }
 483         }
 484         else
 485         {
 486                 unsigned expected_bytes = 0;
 487                 if(cmd==CMD_FUNC_STATUS)
 488                         expected_bytes = 1;
 489                 if(cmd==CMD_TURNOUT_GROUP_STATUS)
 490                         expected_bytes = 2;
 491                 if(cmd==CMD_LOK_CONFIG)
 492                         expected_bytes = 4;
 493
 494                 unsigned char err;
 495                 read_all(&err, 1);
 496
 497                 if(err==ERR_NO_ERROR)
 498                 {
 499                         unsigned char data[8];
 500                         read_all(data, expected_bytes);
 501                 }
 502         }
 503 }
 504
 505 unsigned Intellibox::read_all(unsigned char *buf, unsigned len)
 506 {
 507         unsigned pos = 0;
 508         while(pos<len)
 509                 pos += read(serial_fd, buf+pos, len-pos);
 510
 511         return pos;
 512 }
 513
 514
 515 Intellibox::Locomotive::Locomotive():
 516         speed(0),
 517         reverse(false),
 518         funcs(0)
 519 { }
 520
 521
 522 Intellibox::Turnout::Turnout():
 523         state(false),
 524         active(false)
 525 { }
 526
 527
 528 Intellibox::Sensor::Sensor():
 529         state(false)
 530 { }
 531
 532 } // namespace Marklin