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         loco.speed = speed;
 100         loco_command(addr, speed, loco.reverse, loco.funcs|0x100);
 101         signal_loco_speed.emit(addr, speed, loco.reverse);
 102 }
 103
 104 void Intellibox::set_loco_reverse(unsigned addr, bool rev)
 105 {
 106         Locomotive &loco = locos[addr];
 107         loco.reverse = rev;
 108         loco_command(addr, loco.speed, rev, loco.funcs|0x100);
 109         signal_loco_speed.emit(addr, loco.speed, rev);
 110 }
 111
 112 void Intellibox::set_loco_function(unsigned addr, unsigned func, bool state)
 113 {
 114         Locomotive &loco = locos[addr];
 115         if(state)
 116                 loco.funcs |= 1<<func;
 117         else
 118                 loco.funcs &= ~(1<<func);
 119         loco_command(addr, loco.speed, loco.reverse, loco.funcs);
 120         signal_loco_function.emit(addr, func, state);
 121 }
 122
 123 void Intellibox::add_turnout(unsigned addr)
 124 {
 125         if(!turnouts.count(addr))
 126         {
 127                 turnouts[addr];
 128
 129                 unsigned char data[2];
 130                 data[0] = addr&0xFF;
 131                 data[1] = (addr>>8)&0xFF;
 132                 command(CMD_TURNOUT_STATUS, addr, data, 2);
 133         }
 134 }
 135
 136 void Intellibox::set_turnout(unsigned addr, bool state)
 137 {
 138         Turnout &turnout = turnouts[addr];
 139         if(state==turnout.state || state==turnout.pending)
 140                 return;
 141
 142         turnout.pending = state;
 143         turnout.active = true;
 144
 145         turnout_command(addr, state, true);
 146 }
 147
 148 bool Intellibox::get_turnout(unsigned addr) const
 149 {
 150         map<unsigned, Turnout>::const_iterator i = turnouts.find(addr);
 151         if(i!=turnouts.end())
 152                 return i->second.state;
 153         return false;
 154 }
 155
 156 void Intellibox::add_sensor(unsigned addr)
 157 {
 158         if(!sensors.count(addr))
 159         {
 160                 sensors[addr];
 161                 update_sensors = true;
 162         }
 163 }
 164
 165 bool Intellibox::get_sensor(unsigned addr) const
 166 {
 167         map<unsigned, Sensor>::const_iterator i = sensors.find(addr);
 168         if(i!=sensors.end())
 169                 return i->second.state;
 170         return false;
 171 }
 172
 173 void Intellibox::tick()
 174 {
 175         const Time::TimeStamp t = Time::now();
 176
 177         if(t>next_event_query)
 178         {
 179                 next_event_query = t+200*Time::msec;
 180                 command(CMD_EVENT);
 181         }
 182
 183         for(map<unsigned, Turnout>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
 184                 if(i->second.active && i->second.off_timeout && t>i->second.off_timeout)
 185                 {
 186                         i->second.active = false;
 187                         i->second.off_timeout = Time::TimeStamp();
 188                         turnout_command(i->first, i->second.state, false);
 189                 }
 190
 191         for(map<unsigned, Sensor>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
 192                 if(i->second.off_timeout && t>i->second.off_timeout)
 193                 {
 194                         i->second.state = false;
 195                         i->second.off_timeout = Time::TimeStamp();
 196                         signal_sensor.emit(i->first, false);
 197                 }
 198
 199         if(update_sensors)
 200         {
 201                 unsigned max_addr = (--sensors.end())->first;
 202                 unsigned char data[2];
 203                 data[0] = 0;
 204                 data[1] = (max_addr+7)/8;
 205                 command(CMD_SENSOR_PARAM_SET, data, 2);
 206                 command(CMD_SENSOR_REPORT);
 207                 update_sensors = false;
 208         }
 209
 210         if(!queue.empty() && command_sent)
 211         {
 212                 pollfd pfd = { serial_fd, POLLIN, 0 };
 213                 if(poll(&pfd, 1, 0)>0)
 214                 {
 215                         process_reply(t);
 216                         queue.erase(queue.begin());
 217                         command_sent = false;
 218                 }
 219                 else
 220                         return;
 221         }
 222
 223         if(!queue.empty())
 224         {
 225                 const CommandSlot &slot = queue.front();
 226                 write(serial_fd, slot.data, slot.length);
 227                 command_sent = true;
 228         }
 229 }
 230
 231 void Intellibox::flush()
 232 {
 233         for(list<CommandSlot>::iterator i=queue.begin(); i!=queue.end(); ++i)
 234                 write(serial_fd, i->data, i->length);
 235 }
 236
 237 void Intellibox::command(Command cmd)
 238 {
 239         command(cmd, 0, 0);
 240 }
 241
 242 void Intellibox::command(Command cmd, const unsigned char *data, unsigned len)
 243 {
 244         command(cmd, 0, data, len);
 245 }
 246
 247 void Intellibox::command(Command cmd, unsigned addr, const unsigned char *data, unsigned len)
 248 {
 249         CommandSlot slot;
 250         slot.cmd = cmd;
 251         slot.addr = addr;
 252         slot.data[0] = cmd;
 253         copy(data, data+len, slot.data+1);
 254         slot.length = 1+len;
 255         queue.push_back(slot);
 256 }
 257
 258 void Intellibox::loco_command(unsigned addr, unsigned speed, bool rev, unsigned funcs)
 259 {
 260         unsigned char data[4];
 261         data[0] = addr&0xFF;
 262         data[1] = (addr>>8)&0xFF;
 263
 264         if(speed==0)
 265                 data[2] = 0;
 266         else if(speed==1)
 267                 data[2] = 2;
 268         else
 269                 data[2] = (speed*19-18)/2;
 270
 271         data[3] = (rev ? 0 : 0x20) | ((funcs&1) ? 0x10 : 0);
 272
 273         if(!(funcs&0x100))
 274                 data[3] |= 0x80 | ((funcs>>1)&0xF);
 275
 276         command(CMD_LOK, addr, data, 4);
 277 }
 278
 279 void Intellibox::turnout_command(unsigned addr, bool state, bool active)
 280 {
 281         unsigned char data[2];
 282         data[0] = addr&0xFF;
 283         data[1] = ((addr>>8)&0x7) | (active ? 0x40 : 0) | (state ? 0x80 : 0);
 284         command(CMD_TURNOUT, addr, data, 2);
 285 }
 286
 287 void Intellibox::process_reply(const Time::TimeStamp &t)
 288 {
 289         Command cmd = queue.front().cmd;
 290
 291         if(cmd==CMD_STATUS)
 292         {
 293                 unsigned char status;
 294                 read_all(&status, 1);
 295                 power = status&0x08;
 296                 signal_power.emit(power);
 297         }
 298         else if(cmd==CMD_EVENT)
 299         {
 300                 for(unsigned i=0;; ++i)
 301                 {
 302                         unsigned char byte;
 303                         read_all(&byte, 1);
 304
 305                         if(i==0)
 306                         {
 307                                 if(byte&0x01)
 308                                         command(CMD_EVENT_LOK);
 309                                 if(byte&0x20)
 310                                         command(CMD_EVENT_TURNOUT);
 311                                 if(byte&0x04)
 312                                         command(CMD_EVENT_SENSOR);
 313                         }
 314                         else if(i==1)
 315                         {
 316                                 if(byte&0x40)
 317                                         command(CMD_STATUS);
 318                         }
 319
 320                         if(!(byte&0x80))
 321                                 break;
 322                 }
 323         }
 324         else if(cmd==CMD_EVENT_LOK)
 325         {
 326                 while(1)
 327                 {
 328                         unsigned char data[5];
 329                         read_all(data, 1);
 330                         if(data[0]==0x80)
 331                                 break;
 332                         read_all(data+1, 4);
 333                 }
 334         }
 335         else if(cmd==CMD_EVENT_TURNOUT)
 336         {
 337                 unsigned char count;
 338                 read_all(&count, 1);
 339                 for(unsigned i=0; i<count; ++i)
 340                 {
 341                         unsigned char data[2];
 342                         read_all(data, 2);
 343
 344                         unsigned addr = data[0]+((data[1]&7)<<8);
 345                         signal_turnout.emit(addr, (data[1]&0x80)!=0);
 346                 }
 347         }
 348         else if(cmd==CMD_EVENT_SENSOR)
 349         {
 350                 while(1)
 351                 {
 352                         unsigned char mod;
 353                         read_all(&mod, 1);
 354                         if(!mod)
 355                                 break;
 356
 357                         unsigned char data[2];
 358                         read_all(data, 2);
 359                         for(unsigned i=0; i<16; ++i)
 360                         {
 361                                 unsigned addr = mod*16+i-15;
 362                                 bool state = (data[i/8]>>(7-i%8))&1;
 363
 364                                 Sensor &sensor = sensors[addr];
 365                                 if(state)
 366                                 {
 367                                         sensor.off_timeout = Time::TimeStamp();
 368                                         if(!sensor.state)
 369                                         {
 370                                                 sensor.state = state;
 371                                                 signal_sensor(addr, state);
 372                                         }
 373                                 }
 374                                 else if(sensor.state)
 375                                         sensor.off_timeout = t+700*Time::msec;
 376                         }
 377                 }
 378         }
 379         else if(cmd==CMD_TURNOUT)
 380         {
 381                 unsigned char err;
 382                 read_all(&err, 1);
 383
 384                 if(err==ERR_NO_ERROR)
 385                 {
 386                         unsigned addr = queue.front().addr;
 387                         Turnout &turnout = turnouts[addr];
 388                         turnout.state = turnout.pending;
 389                         if(turnout.active)
 390                         {
 391                                 signal_turnout.emit(addr, turnout.state);
 392                                 turnout.off_timeout = t+500*Time::msec;
 393                         }
 394                 }
 395                 else if(err==ERR_NO_I2C_SPACE)
 396                         queue.push_back(queue.front());
 397         }
 398         else if(cmd==CMD_TURNOUT_STATUS)
 399         {
 400                 unsigned char err;
 401                 read_all(&err, 1);
 402
 403                 if(err==ERR_NO_ERROR)
 404                 {
 405                         unsigned char data;
 406                         read_all(&data, 1);
 407
 408                         unsigned addr = queue.front().addr;
 409                         bool state = data&0x04;
 410
 411                         Turnout &turnout = turnouts[addr];
 412                         if(state!=turnout.state)
 413                         {
 414                                 turnout.state = state;
 415                                 signal_turnout.emit(addr, turnout.state);
 416                         }
 417                 }
 418         }
 419         else if(cmd==CMD_LOK_STATUS)
 420         {
 421                 unsigned char err;
 422                 read_all(&err, 1);
 423
 424                 if(err==ERR_NO_ERROR)
 425                 {
 426                         unsigned char data[3];
 427                         read_all(data, 3);
 428
 429                         unsigned addr = queue.front().addr;
 430                         Locomotive &loco = locos[addr];
 431
 432                         unsigned speed = (data[0]<=1 ? 0 : data[0]*2/19+1);
 433                         bool reverse = !(data[1]&0x20);
 434                         if(speed!=loco.speed || reverse!=loco.reverse)
 435                         {
 436                                 loco.speed = speed;
 437                                 loco.reverse = reverse;
 438                                 signal_loco_speed.emit(addr, loco.speed, loco.reverse);
 439                         }
 440
 441                         unsigned funcs = (data[1]&0xF)<<1;
 442                         if(data[1]&0x10)
 443                                 funcs |= 1;
 444                         if(funcs!=loco.funcs)
 445                         {
 446                                 unsigned changed = loco.funcs^funcs;
 447                                 loco.funcs = funcs;
 448                                 for(unsigned i=0; i<5; ++i)
 449                                         if(changed&(1<<i))
 450                                                 signal_loco_function.emit(addr, i, loco.funcs&(1<<i));
 451                         }
 452                 }
 453         }
 454         else
 455         {
 456                 unsigned expected_bytes = 0;
 457                 if(cmd==CMD_FUNC_STATUS)
 458                         expected_bytes = 1;
 459                 if(cmd==CMD_TURNOUT_GROUP_STATUS)
 460                         expected_bytes = 2;
 461                 if(cmd==CMD_LOK_CONFIG)
 462                         expected_bytes = 4;
 463
 464                 unsigned char err;
 465                 read_all(&err, 1);
 466
 467                 if(err==ERR_NO_ERROR)
 468                 {
 469                         unsigned char data[8];
 470                         read_all(data, expected_bytes);
 471                 }
 472         }
 473 }
 474
 475 unsigned Intellibox::read_all(unsigned char *buf, unsigned len)
 476 {
 477         unsigned pos = 0;
 478         while(pos<len)
 479                 pos += read(serial_fd, buf+pos, len-pos);
 480
 481         return pos;
 482 }
 483
 484
 485 Intellibox::Locomotive::Locomotive():
 486         speed(0),
 487         reverse(false),
 488         funcs(0)
 489 { }
 490
 491
 492 Intellibox::Turnout::Turnout():
 493         state(false),
 494         active(false)
 495 { }
 496
 497
 498 Intellibox::Sensor::Sensor():
 499         state(false)
 500 { }
 501
 502 } // namespace Marklin