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