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