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