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