source/libr2c2/arducontrol.cpp

   1 #include <msp/core/maputils.h>
   2 #include <msp/io/print.h>
   3 #include <msp/time/utils.h>
   4 #include "arducontrol.h"
   5 #include "tracktype.h"
   6
   7 using namespace std;
   8 using namespace Msp;
   9
  10 namespace R2C2 {
  11
  12 ArduControl::ArduControl(const string &dev):
  13         serial(dev),
  14         debug(true),
  15         power(false),
  16         next_refresh(refresh_cycle.end()),
  17         refresh_counter(0),
  18         active_accessory(0),
  19         n_s88_octets(0),
  20         thread(*this)
  21 {
  22 }
  23
  24 ArduControl::~ArduControl()
  25 {
  26         thread.exit();
  27 }
  28
  29 void ArduControl::set_power(bool p)
  30 {
  31         if(p==power.pending)
  32                 return;
  33
  34         power.pending = p;
  35         ++power.serial;
  36
  37         QueuedCommand cmd(POWER);
  38         cmd.tag.serial = power.serial;
  39         cmd.command[0] = (p ? POWER_ON : POWER_OFF);
  40         cmd.length = 1;
  41         push_command(cmd);
  42 }
  43
  44 void ArduControl::halt(bool)
  45 {
  46 }
  47
  48 const char *ArduControl::enumerate_protocols(unsigned i) const
  49 {
  50         if(i==0)
  51                 return "MM";
  52         else
  53                 return 0;
  54 }
  55
  56 ArduControl::Protocol ArduControl::map_protocol(const string &proto_name)
  57 {
  58         if(proto_name=="MM")
  59                 return MM;
  60         else
  61                 throw invalid_argument("ArduControl::map_protocol");
  62 }
  63
  64 unsigned ArduControl::get_protocol_speed_steps(const string &proto_name) const
  65 {
  66         Protocol proto = map_protocol(proto_name);
  67         if(proto==MM)
  68                 return 14;
  69         else
  70                 return 0;
  71 }
  72
  73 unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
  74 {
  75         if(!addr)
  76                 throw invalid_argument("ArduControl::add_loco");
  77
  78         Protocol proto = map_protocol(proto_name);
  79
  80         if(proto==MM)
  81         {
  82                 if(addr>=80)
  83                         throw invalid_argument("ArduControl::add_loco");
  84         }
  85
  86         Locomotive loco(proto, addr);
  87         insert_unique(locomotives, loco.id, loco);
  88
  89         return loco.id;
  90 }
  91
  92 void ArduControl::remove_loco(unsigned id)
  93 {
  94         Locomotive &loco = get_item(locomotives, id);
  95         remove_loco_from_refresh(loco);
  96         locomotives.erase(id);
  97 }
  98
  99 void ArduControl::set_loco_speed(unsigned id, unsigned speed)
 100 {
 101         Locomotive &loco = get_item(locomotives, id);
 102         if(loco.speed.set(speed))
 103         {
 104                 QueuedCommand cmd(loco, Locomotive::SPEED);
 105                 push_command(cmd);
 106
 107                 add_loco_to_refresh(loco);
 108         }
 109 }
 110
 111 void ArduControl::set_loco_reverse(unsigned id, bool rev)
 112 {
 113         Locomotive &loco = get_item(locomotives, id);
 114         if(loco.reverse.set(rev))
 115         {
 116                 QueuedCommand cmd(loco, Locomotive::REVERSE);
 117                 push_command(cmd);
 118
 119                 add_loco_to_refresh(loco);
 120         }
 121 }
 122
 123 void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
 124 {
 125         Locomotive &loco = get_item(locomotives, id);
 126         unsigned mask = 1<<func;
 127         if(loco.funcs.set((loco.funcs&~mask)|(mask*state)))
 128         {
 129                 if(func>0)
 130                 {
 131                         QueuedCommand cmd(loco, Locomotive::FUNCTIONS, func);
 132                         push_command(cmd);
 133                 }
 134
 135                 add_loco_to_refresh(loco);
 136         }
 137 }
 138
 139 void ArduControl::add_loco_to_refresh(Locomotive &loco)
 140 {
 141         MutexLock lock(mutex);
 142         refresh_cycle.push_back(&loco);
 143         if(refresh_cycle.size()>15)
 144         {
 145                 LocomotivePtrList::iterator oldest = refresh_cycle.begin();
 146                 for(LocomotivePtrList::iterator i=refresh_cycle.begin(); ++i!=refresh_cycle.end(); )
 147                         if((*i)->last_change_age>(*oldest)->last_change_age)
 148                                 oldest = i;
 149                 if(oldest==next_refresh)
 150                         advance_next_refresh();
 151                 refresh_cycle.erase(oldest);
 152         }
 153         if(next_refresh==refresh_cycle.end())
 154                 next_refresh = refresh_cycle.begin();
 155 }
 156
 157 void ArduControl::remove_loco_from_refresh(Locomotive &loco)
 158 {
 159         MutexLock lock(mutex);
 160         for(LocomotivePtrList::iterator i=refresh_cycle.begin(); i!=refresh_cycle.end(); ++i)
 161                 if(*i==&loco)
 162                 {
 163                         if(i==next_refresh)
 164                         {
 165                                 if(refresh_cycle.size()>1)
 166                                         advance_next_refresh();
 167                                 else
 168                                         next_refresh = refresh_cycle.end();
 169                         }
 170                         refresh_cycle.erase(i);
 171                         return;
 172                 }
 173 }
 174
 175 ArduControl::Locomotive *ArduControl::get_loco_to_refresh()
 176 {
 177         MutexLock lock(mutex);
 178         if(refresh_cycle.empty())
 179                 return 0;
 180
 181         Locomotive *loco = *next_refresh;
 182         advance_next_refresh();
 183         return loco;
 184 }
 185
 186 void ArduControl::advance_next_refresh()
 187 {
 188         ++next_refresh;
 189         if(next_refresh==refresh_cycle.end())
 190         {
 191                 next_refresh = refresh_cycle.begin();
 192                 ++refresh_counter;
 193         }
 194 }
 195
 196 unsigned ArduControl::add_turnout(unsigned addr, const TrackType &type)
 197 {
 198         if(!addr || !type.is_turnout())
 199                 throw invalid_argument("ArduControl::add_turnout");
 200
 201         return add_accessory(Accessory::TURNOUT, addr, type.get_state_bits());
 202 }
 203
 204 void ArduControl::remove_turnout(unsigned addr)
 205 {
 206         remove_accessory(Accessory::TURNOUT, addr);
 207 }
 208
 209 void ArduControl::set_turnout(unsigned addr, unsigned state)
 210 {
 211         set_accessory(Accessory::TURNOUT, addr, state);
 212 }
 213
 214 unsigned ArduControl::get_turnout(unsigned addr) const
 215 {
 216         return get_accessory(Accessory::TURNOUT, addr);
 217 }
 218
 219 unsigned ArduControl::add_signal(unsigned addr, const SignalType &)
 220 {
 221         return add_accessory(Accessory::SIGNAL, addr, 1);
 222 }
 223
 224 void ArduControl::remove_signal(unsigned addr)
 225 {
 226         remove_accessory(Accessory::SIGNAL, addr);
 227 }
 228
 229 void ArduControl::set_signal(unsigned addr, unsigned state)
 230 {
 231         set_accessory(Accessory::SIGNAL, addr, state);
 232 }
 233
 234 unsigned ArduControl::get_signal(unsigned addr) const
 235 {
 236         return get_accessory(Accessory::SIGNAL, addr);
 237 }
 238
 239 unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits)
 240 {
 241         AccessoryMap::iterator i = accessories.lower_bound(addr);
 242         AccessoryMap::iterator j = accessories.upper_bound(addr+bits-1);
 243         if(i!=j)
 244                 throw key_error(addr);
 245         if(i!=accessories.begin())
 246         {
 247                 --i;
 248                 if(i->first+i->second.bits>addr)
 249                         throw key_error(addr);
 250         }
 251
 252         insert_unique(accessories, addr, Accessory(kind, addr, bits));
 253         return addr;
 254 }
 255
 256 void ArduControl::remove_accessory(Accessory::Kind kind, unsigned addr)
 257 {
 258         Accessory &acc = get_item(accessories, addr);
 259         if(acc.kind!=kind)
 260                 throw key_error(addr);
 261         accessories.erase(addr);
 262 }
 263
 264 void ArduControl::set_accessory(Accessory::Kind kind, unsigned addr, unsigned state)
 265 {
 266         Accessory &acc = get_item(accessories, addr);
 267         if(acc.kind!=kind)
 268                 throw key_error(addr);
 269
 270         if(state!=acc.target)
 271         {
 272                 acc.target = state;
 273                 accessory_queue.push_back(&acc);
 274         }
 275 }
 276
 277 unsigned ArduControl::get_accessory(Accessory::Kind kind, unsigned addr) const
 278 {
 279         const Accessory &acc = get_item(accessories, addr);
 280         if(acc.kind!=kind)
 281                 throw key_error(addr);
 282         return acc.state;
 283 }
 284
 285 unsigned ArduControl::add_sensor(unsigned addr)
 286 {
 287         if(!addr)
 288                 throw invalid_argument("ArduControl::add_sensor");
 289
 290         insert_unique(sensors, addr, Sensor(addr));
 291         unsigned octet_index = (addr-1)/8;
 292         if(octet_index>=n_s88_octets)
 293                 n_s88_octets = octet_index+1;
 294
 295         return addr;
 296 }
 297
 298 void ArduControl::remove_sensor(unsigned addr)
 299 {
 300         remove_existing(sensors, addr);
 301         // TODO update n_s88_octets
 302 }
 303
 304 bool ArduControl::get_sensor(unsigned addr) const
 305 {
 306         return get_item(sensors, addr).state;
 307 }
 308
 309 void ArduControl::tick()
 310 {
 311         while(Tag tag = pop_completed_tag())
 312         {
 313                 if(tag.type==Tag::GENERAL)
 314                 {
 315                         if(tag.command==POWER)
 316                         {
 317                                 if(power.commit(tag.serial))
 318                                         signal_power.emit(power.current);
 319                         }
 320                 }
 321                 else if(tag.type==Tag::LOCOMOTIVE)
 322                 {
 323                         LocomotiveMap::iterator i = locomotives.find(tag.id);
 324                         if(i==locomotives.end())
 325                                 continue;
 326
 327                         Locomotive &loco = i->second;
 328                         if(tag.command==Locomotive::SPEED)
 329                         {
 330                                 if(loco.speed.commit(tag.serial))
 331                                         signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
 332                         }
 333                         else if(tag.command==Locomotive::REVERSE)
 334                         {
 335                                 if(loco.reverse.commit(tag.serial))
 336                                         signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
 337                         }
 338                         else if(tag.command==Locomotive::FUNCTIONS)
 339                         {
 340                                 unsigned old = loco.funcs;
 341                                 if(loco.funcs.commit(tag.serial))
 342                                 {
 343                                         unsigned changed = old^loco.funcs;
 344                                         for(unsigned j=0; changed>>j; ++j)
 345                                                 if((changed>>j)&1)
 346                                                         signal_loco_function.emit(loco.id, j, (loco.funcs>>j)&1);
 347                                 }
 348                         }
 349                 }
 350                 else if(tag.type==Tag::ACCESSORY)
 351                 {
 352                         AccessoryMap::iterator i = accessories.find(tag.id);
 353                         if(i==accessories.end())
 354                                 continue;
 355
 356                         Accessory &acc = i->second;
 357                         if(tag.command==Accessory::ACTIVATE)
 358                         {
 359                                 off_timeout = Time::now()+acc.active_time;
 360                         }
 361                         else if(tag.command==Accessory::DEACTIVATE)
 362                         {
 363                                 if(acc.state.commit(tag.serial))
 364                                 {
 365                                         if(acc.state==acc.target)
 366                                         {
 367                                                 if(acc.kind==Accessory::TURNOUT)
 368                                                         signal_turnout.emit(acc.address, acc.state);
 369                                                 else if(acc.kind==Accessory::SIGNAL)
 370                                                         signal_signal.emit(acc.address, acc.state);
 371                                         }
 372                                         if(&acc==active_accessory)
 373                                                 active_accessory = 0;
 374                                 }
 375                         }
 376                 }
 377                 else if(tag.type==Tag::SENSOR)
 378                 {
 379                         SensorMap::iterator i = sensors.find(tag.id);
 380                         if(i==sensors.end())
 381                                 continue;
 382
 383                         Sensor &sensor = i->second;
 384                         if(tag.command==Sensor::STATE)
 385                         {
 386                                 if(sensor.state.commit(tag.serial))
 387                                         signal_sensor.emit(sensor.address, sensor.state);
 388                         }
 389                 }
 390         }
 391
 392         while(!active_accessory && !accessory_queue.empty())
 393         {
 394                 Accessory &acc = *accessory_queue.front();
 395
 396                 if(acc.state!=acc.target)
 397                 {
 398                         active_accessory = &acc;
 399
 400                         unsigned changes = acc.state^acc.target;
 401                         unsigned lowest_bit = changes&~(changes-1);
 402                         unsigned i;
 403                         for(i=0; (lowest_bit>>i)>1; ++i) ;
 404                         acc.state.set(acc.state^lowest_bit);
 405                         QueuedCommand cmd(acc, Accessory::ACTIVATE, i);
 406                         push_command(cmd);
 407                 }
 408                 else
 409                         accessory_queue.pop_front();
 410         }
 411
 412         if(active_accessory && off_timeout)
 413         {
 414                 Time::TimeStamp t = Time::now();
 415                 if(t>off_timeout)
 416                 {
 417                         off_timeout = Time::TimeStamp();
 418                         QueuedCommand cmd(*active_accessory, Accessory::DEACTIVATE);
 419                         push_command(cmd);
 420                 }
 421         }
 422 }
 423
 424 void ArduControl::flush()
 425 {
 426 }
 427
 428 void ArduControl::push_command(const QueuedCommand &cmd)
 429 {
 430         MutexLock lock(mutex);
 431         command_queue.push_back(cmd);
 432 }
 433
 434 bool ArduControl::pop_command(QueuedCommand &cmd)
 435 {
 436         MutexLock lock(mutex);
 437         if(command_queue.empty())
 438                 return false;
 439         cmd = command_queue.front();
 440         command_queue.pop_front();
 441         return true;
 442 }
 443
 444 void ArduControl::push_completed_tag(const Tag &tag)
 445 {
 446         MutexLock lock(mutex);
 447         completed_commands.push_back(tag);
 448 }
 449
 450 ArduControl::Tag ArduControl::pop_completed_tag()
 451 {
 452         MutexLock lock(mutex);
 453         if(completed_commands.empty())
 454                 return Tag();
 455         Tag tag = completed_commands.front();
 456         completed_commands.pop_front();
 457         return tag;
 458 }
 459
 460
 461 ArduControl::Tag::Tag():
 462         type(NONE),
 463         command(0),
 464         serial(0),
 465         id(0)
 466 { }
 467
 468
 469 ArduControl::Locomotive::Locomotive(Protocol p, unsigned a):
 470         id((p<<16)|a),
 471         proto(p),
 472         address(a),
 473         speed(0),
 474         reverse(false),
 475         funcs(0),
 476         last_change_age(0)
 477 { }
 478
 479 unsigned ArduControl::Locomotive::create_speed_dir_command(char *buffer) const
 480 {
 481         if(proto==MM)
 482         {
 483                 buffer[0] = MOTOROLA_SPEED_DIRECTION;
 484                 buffer[1] = address;
 485                 buffer[2] = funcs.pending&1;
 486                 buffer[3] = speed.pending+reverse.pending*0x80;
 487                 return 4;
 488         }
 489         else
 490                 return 0;
 491 }
 492
 493 unsigned ArduControl::Locomotive::create_speed_func_command(unsigned f, char *buffer) const
 494 {
 495         if(proto==MM)
 496         {
 497                 if(f<1 || f>4)
 498                         throw invalid_argument("Locomotive::create_speed_func_command");
 499
 500                 buffer[0] = MOTOROLA_SPEED_FUNCTION;
 501                 buffer[1] = address;
 502                 buffer[2] = (f<<4)|(((funcs.pending>>f)&1)<<1)|(funcs.pending&1);
 503                 buffer[3] = speed.pending;
 504                 return 4;
 505         }
 506         else
 507                 return 0;
 508 }
 509
 510
 511 ArduControl::Accessory::Accessory(Kind k, unsigned a, unsigned b):
 512         kind(k),
 513         address(a),
 514         bits(b),
 515         state(0),
 516         active_time(500*Time::msec)
 517 { }
 518
 519 unsigned ArduControl::Accessory::create_state_command(unsigned b, bool c, char *buffer) const
 520 {
 521         if(b>=bits)
 522                 throw invalid_argument("Accessory::create_state_command");
 523
 524         unsigned a = (address+b+3)*2;
 525         if(!((state.pending>>b)&1))
 526                 ++a;
 527         buffer[0] = MOTOROLA_SOLENOID;
 528         buffer[1] = a>>3;
 529         buffer[2] = ((a&7)<<4)|c;
 530         return 3;
 531 }
 532
 533
 534 ArduControl::Sensor::Sensor(unsigned a):
 535         address(a),
 536         state(false)
 537 { }
 538
 539
 540 ArduControl::ControlThread::ControlThread(ArduControl &c):
 541         control(c),
 542         done(false)
 543 {
 544         launch();
 545 }
 546
 547 void ArduControl::ControlThread::exit()
 548 {
 549         done = true;
 550         join();
 551 }
 552
 553 void ArduControl::ControlThread::main()
 554 {
 555         char command[15];
 556         unsigned length = 0;
 557         unsigned repeat_count = 0;
 558         Tag tag;
 559         Locomotive *loco = 0;
 560         unsigned phase = 0;
 561         unsigned s88_octets_remaining = 0;
 562         while(!done)
 563         {
 564                 if(!repeat_count)
 565                 {
 566                         tag = Tag();
 567                         length = 0;
 568                         repeat_count = 1;
 569                         QueuedCommand qcmd;
 570                         if(control.pop_command(qcmd))
 571                         {
 572                                 length = qcmd.length;
 573                                 copy(qcmd.command, qcmd.command+length, command);
 574                                 if(qcmd.tag.type==Tag::LOCOMOTIVE)
 575                                         repeat_count = 8;
 576                                 tag = qcmd.tag;
 577                         }
 578                         else if(loco && phase==0)
 579                         {
 580                                 length = loco->create_speed_func_command(control.refresh_counter%4+1, command);
 581                                 repeat_count = 2;
 582                                 ++phase;
 583                         }
 584                         else if(!s88_octets_remaining && control.n_s88_octets)
 585                         {
 586                                 command[0] = S88_READ;
 587                                 command[1] = control.n_s88_octets;
 588                                 length = 2;
 589                                 s88_octets_remaining = control.n_s88_octets;
 590                         }
 591                         else if((loco = control.get_loco_to_refresh()))
 592                         {
 593                                 length = loco->create_speed_dir_command(command);
 594                                 repeat_count = 2;
 595                                 phase = 0;
 596                         }
 597                         else
 598                         {
 599                                 // Send an idle packet for the MM protocol
 600                                 command[0] = MOTOROLA_SPEED;
 601                                 command[1] = 80;
 602                                 command[2] = 0;
 603                                 command[3] = 0;
 604                                 length = 4;
 605                         }
 606                 }
 607
 608                 if(control.debug)
 609                 {
 610                         string cmd_hex;
 611                         for(unsigned i=0; i<length; ++i)
 612                                 cmd_hex += format(" %02X", static_cast<unsigned char>(command[i]));
 613                         IO::print("< %02X%s\n", length^0xFF, cmd_hex);
 614                 }
 615
 616                 control.serial.put(length^0xFF);
 617                 control.serial.write(command, length);
 618                 --repeat_count;
 619
 620                 bool got_reply = false;
 621                 bool got_data = false;
 622                 while(!got_reply || got_data)
 623                 {
 624                         if(got_reply)
 625                                 got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero);
 626                         else
 627                                 got_data = IO::poll(control.serial, IO::P_INPUT);
 628
 629                         if(got_data)
 630                         {
 631                                 unsigned rlength = control.serial.get()^0xFF;
 632                                 if(rlength>15)
 633                                 {
 634                                         IO::print("Invalid length %02X\n", rlength);
 635                                         continue;
 636                                 }
 637
 638                                 char reply[15];
 639                                 unsigned pos = 0;
 640                                 while(pos<rlength)
 641                                         pos += control.serial.read(reply+pos, rlength-pos);
 642
 643                                 if(control.debug)
 644                                 {
 645                                         string reply_hex;
 646                                         for(unsigned i=0; i<rlength; ++i)
 647                                                 reply_hex += format(" %02X", static_cast<unsigned char>(reply[i]));
 648                                         IO::print("> %02X%s\n", rlength^0xFF, reply_hex);
 649                                 }
 650
 651                                 unsigned char type = reply[0];
 652                                 if((type&0xE0)==0x80)
 653                                 {
 654                                         got_reply = true;
 655                                         if(type!=COMMAND_OK)
 656                                                 IO::print("Error %02X\n", type);
 657                                         else if(tag && !repeat_count)
 658                                                 control.push_completed_tag(tag);
 659                                 }
 660                                 else if(type==S88_DATA && rlength>2)
 661                                 {
 662                                         unsigned offset = static_cast<unsigned char>(reply[1]);
 663                                         unsigned count = rlength-2;
 664
 665                                         SensorMap::iterator begin = control.sensors.lower_bound(offset*8+1);
 666                                         SensorMap::iterator end = control.sensors.upper_bound((offset+count)*8);
 667                                         for(SensorMap::iterator i=begin; i!=end; ++i)
 668                                         {
 669                                                 unsigned bit_index = i->first-1-offset*8;
 670                                                 bool state = (reply[2+bit_index/8]>>(7-bit_index%8))&1;
 671                                                 i->second.state.set(state);
 672
 673                                                 Tag stag;
 674                                                 stag.type = Tag::SENSOR;
 675                                                 stag.command = Sensor::STATE;
 676                                                 stag.serial = i->second.state.serial;
 677                                                 stag.id = i->first;
 678                                                 control.push_completed_tag(stag);
 679                                         }
 680
 681                                         if(count>s88_octets_remaining)
 682                                                 s88_octets_remaining = 0;
 683                                         else
 684                                                 s88_octets_remaining -= count;
 685                                 }
 686                         }
 687                 }
 688         }
 689 }
 690
 691
 692 ArduControl::QueuedCommand::QueuedCommand():
 693         length(0)
 694 { }
 695
 696 ArduControl::QueuedCommand::QueuedCommand(GeneralCommand cmd):
 697         length(0)
 698 {
 699         tag.type = Tag::GENERAL;
 700         tag.command = cmd;
 701 }
 702
 703 ArduControl::QueuedCommand::QueuedCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index)
 704 {
 705         tag.type = Tag::LOCOMOTIVE;
 706         tag.command = cmd;
 707         tag.id = loco.id;
 708         if(cmd==Locomotive::SPEED)
 709         {
 710                 tag.serial = loco.speed.serial;
 711                 length = loco.create_speed_dir_command(command);
 712         }
 713         else if(cmd==Locomotive::REVERSE)
 714         {
 715                 tag.serial = loco.reverse.serial;
 716                 length = loco.create_speed_dir_command(command);
 717         }
 718         else if(cmd==Locomotive::FUNCTIONS)
 719         {
 720                 tag.serial = loco.funcs.serial;
 721                 length = loco.create_speed_func_command(index, command);
 722         }
 723         else
 724                 throw invalid_argument("QueuedCommand");
 725 }
 726
 727 ArduControl::QueuedCommand::QueuedCommand(Accessory &acc, Accessory::Command cmd, unsigned index)
 728 {
 729         tag.type = Tag::ACCESSORY;
 730         tag.command = cmd;
 731         tag.id = acc.address;
 732         if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE)
 733         {
 734                 tag.serial = acc.state.serial;
 735                 length = acc.create_state_command(index, (cmd==Accessory::ACTIVATE), command);
 736         }
 737         else
 738                 throw invalid_argument("QueuedCommand");
 739 }
 740
 741 } // namespace R2C2