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         PendingCommand 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                 PendingCommand 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                 PendingCommand 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                 PendingCommand 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                         PendingCommand 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                         PendingCommand 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                         PendingCommand 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 PendingCommand &cmd)
 448 {
 449         MutexLock lock(mutex);
 450         command_queue.push_back(cmd);
 451 }
 452
 453 bool ArduControl::pop_command(PendingCommand &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::PendingCommand::PendingCommand():
 587         length(0),
 588         repeat_count(1)
 589 { }
 590
 591 ArduControl::PendingCommand::PendingCommand(GeneralCommand cmd):
 592         length(0),
 593         repeat_count(1)
 594 {
 595         tag.type = Tag::GENERAL;
 596         tag.command = cmd;
 597 }
 598
 599 ArduControl::PendingCommand::PendingCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index):
 600         repeat_count(8)
 601 {
 602         tag.type = Tag::LOCOMOTIVE;
 603         tag.command = cmd;
 604         tag.id = loco.id;
 605         if(cmd==Locomotive::SPEED)
 606         {
 607                 tag.serial = loco.speed.serial;
 608                 length = loco.create_speed_dir_command(command);
 609         }
 610         else if(cmd==Locomotive::REVERSE)
 611         {
 612                 tag.serial = loco.reverse.serial;
 613                 length = loco.create_speed_dir_command(command);
 614         }
 615         else if(cmd==Locomotive::FUNCTIONS)
 616         {
 617                 tag.serial = loco.funcs.serial;
 618                 length = loco.create_speed_func_command(index, command);
 619         }
 620         else
 621                 throw invalid_argument("PendingCommand");
 622 }
 623
 624 ArduControl::PendingCommand::PendingCommand(Accessory &acc, Accessory::Command cmd, unsigned index):
 625         repeat_count(1)
 626 {
 627         tag.type = Tag::ACCESSORY;
 628         tag.command = cmd;
 629         tag.id = acc.address;
 630         if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE)
 631         {
 632                 tag.serial = acc.state.serial;
 633                 length = acc.create_state_command(index, (cmd==Accessory::ACTIVATE), command);
 634         }
 635         else
 636                 throw invalid_argument("PendingCommand");
 637 }
 638
 639
 640 ArduControl::RefreshTask::RefreshTask(ArduControl &c):
 641         control(c),
 642         loco(0),
 643         phase(0)
 644 { }
 645
 646 bool ArduControl::RefreshTask::get_work(PendingCommand &cmd)
 647 {
 648         if(loco && loco->proto==MM && phase==0)
 649         {
 650                 cmd.length = loco->create_speed_func_command(control.refresh_counter%4+1, cmd.command);
 651                 cmd.repeat_count = 2;
 652                 ++phase;
 653                 return true;
 654         }
 655
 656         loco = control.get_loco_to_refresh();
 657         if(!loco)
 658                 return false;
 659
 660         phase = 0;
 661         if(loco->proto==MM)
 662         {
 663                 cmd.length = loco->create_speed_dir_command(cmd.command);
 664                 cmd.repeat_count = 2;
 665         }
 666         else if(loco->proto==MFX)
 667                 cmd.length = loco->create_speed_func_command(0, cmd.command);
 668         else
 669                 return false;
 670
 671         return true;
 672 }
 673
 674
 675 ArduControl::S88Task::S88Task(ArduControl &c):
 676         control(c),
 677         octets_remaining(0)
 678 { }
 679
 680 bool ArduControl::S88Task::get_work(PendingCommand &cmd)
 681 {
 682         if(octets_remaining || !control.n_s88_octets)
 683                 return false;
 684
 685         cmd.command[0] = S88_READ;
 686         cmd.command[1] = control.n_s88_octets;
 687         cmd.length = 2;
 688         octets_remaining = control.n_s88_octets;
 689
 690         return true;
 691 }
 692
 693 void ArduControl::S88Task::process_reply(const char *reply, unsigned length)
 694 {
 695         unsigned char type = reply[0];
 696         if(type==S88_DATA && length>2)
 697         {
 698                 unsigned offset = static_cast<unsigned char>(reply[1]);
 699                 unsigned count = length-2;
 700
 701                 SensorMap::iterator begin = control.sensors.lower_bound(offset*8+1);
 702                 SensorMap::iterator end = control.sensors.upper_bound((offset+count)*8);
 703                 for(SensorMap::iterator i=begin; i!=end; ++i)
 704                 {
 705                         unsigned bit_index = i->first-1-offset*8;
 706                         bool state = (reply[2+bit_index/8]>>(7-bit_index%8))&1;
 707                         i->second.state.set(state);
 708
 709                         Tag tag;
 710                         tag.type = Tag::SENSOR;
 711                         tag.command = Sensor::STATE;
 712                         tag.serial = i->second.state.serial;
 713                         tag.id = i->first;
 714                         control.push_completed_tag(tag);
 715                 }
 716
 717                 if(count>octets_remaining)
 718                         octets_remaining = 0;
 719                 else
 720                         octets_remaining -= count;
 721         }
 722 }
 723
 724
 725 ArduControl::MfxAnnounceTask::MfxAnnounceTask(ArduControl &c):
 726         control(c)
 727 { }
 728
 729 bool ArduControl::MfxAnnounceTask::get_work(PendingCommand &cmd)
 730 {
 731         Time::TimeStamp t = Time::now();
 732         if(t<next)
 733                 return false;
 734
 735         cmd.command[0] = MFX_ANNOUNCE;
 736         cmd.command[1] = control.mfx_announce_serial>>8;
 737         cmd.command[2] = control.mfx_announce_serial;
 738         cmd.length = 3;
 739         next = t+400*Time::msec;
 740
 741         return true;
 742 }
 743
 744
 745 ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c):
 746         control(c),
 747         size(0),
 748         bits(0),
 749         pending(false)
 750 { }
 751
 752 bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
 753 {
 754         if(size==32)
 755         {
 756                 if(control.debug>=1)
 757                         IO::print("Assigning MFX address %d to decoder %08X\n", control.next_mfx_address, bits);
 758
 759                 cmd.command[0] = MFX_ASSIGN_ADDRESS;
 760                 cmd.command[1] = control.next_mfx_address>>8;
 761                 cmd.command[2] = control.next_mfx_address;
 762                 for(unsigned i=0; i<4; ++i)
 763                         cmd.command[3+i] = bits>>(24-i*8);
 764                 cmd.length = 7;
 765
 766                 size = 0;
 767                 bits = 0;
 768                 ++control.next_mfx_address;
 769
 770                 return true;
 771         }
 772
 773         Time::TimeStamp t = Time::now();
 774         if(t<next)
 775                 return false;
 776
 777         cmd.command[0] = MFX_SEARCH;
 778         for(unsigned i=0; i<4; ++i)
 779                 cmd.command[1+i] = bits>>(24-i*8);
 780         cmd.command[5] = size;
 781         cmd.length = 6;
 782
 783         next = t+200*Time::msec;
 784         pending = true;
 785
 786         if(control.debug>=1)
 787                 IO::print("Search %08X/%d\n", bits, size);
 788
 789         return true;
 790 }
 791
 792 void ArduControl::MfxSearchTask::process_reply(const char *reply, unsigned length)
 793 {
 794         unsigned char type = reply[0];
 795         if(type==MFX_FEEDBACK && length==2 && pending)
 796         {
 797                 pending = false;
 798                 bool finished = true;
 799                 if(reply[1])
 800                 {
 801                         if(size<32)
 802                                 ++size;
 803                         finished = false;
 804                 }
 805                 else if(size>0)
 806                 {
 807                         unsigned mask = 1<<(32-size);
 808                         if(!(bits&mask))
 809                         {
 810                                 bits |= mask;
 811                                 finished = false;
 812                         }
 813                 }
 814
 815                 if(finished)
 816                 {
 817                         next = Time::now()+2*Time::sec;
 818                         bits = 0;
 819                         size = 0;
 820                 }
 821         }
 822 }
 823
 824
 825 ArduControl::ControlThread::ControlThread(ArduControl &c):
 826         control(c),
 827         done(false)
 828 {
 829         tasks.push_back(new MfxAnnounceTask(control));
 830         tasks.push_back(new MfxSearchTask(control));
 831         tasks.push_back(new S88Task(control));
 832         tasks.push_back(new RefreshTask(control));
 833
 834         launch();
 835 }
 836
 837 void ArduControl::ControlThread::exit()
 838 {
 839         done = true;
 840         join();
 841 }
 842
 843 void ArduControl::ControlThread::main()
 844 {
 845         while(!done)
 846         {
 847                 PendingCommand cmd;
 848                 if(get_work(cmd))
 849                 {
 850                         bool success = true;
 851                         for(unsigned i=0; (success && i<cmd.repeat_count); ++i)
 852                                 success = (do_command(cmd)==COMMAND_OK);
 853                         if(success && cmd.tag)
 854                                 control.push_completed_tag(cmd.tag);
 855                 }
 856                 else
 857                         Time::sleep(10*Time::msec);
 858         }
 859 }
 860
 861 bool ArduControl::ControlThread::get_work(PendingCommand &cmd)
 862 {
 863         if(control.pop_command(cmd))
 864                 return true;
 865
 866         for(vector<Task *>::iterator i=tasks.begin(); i!=tasks.end(); ++i)
 867                 if((*i)->get_work(cmd))
 868                         return true;
 869
 870         // As fallback, send an idle packet for the MM protocol
 871         cmd.command[0] = MOTOROLA_SPEED;
 872         cmd.command[1] = 80;
 873         cmd.command[2] = 0;
 874         cmd.command[3] = 0;
 875         cmd.length = 4;
 876
 877         return true;
 878 }
 879
 880 unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd)
 881 {
 882         if(control.debug>=2)
 883         {
 884                 string cmd_hex;
 885                 for(unsigned i=0; i<cmd.length; ++i)
 886                         cmd_hex += format(" %02X", static_cast<unsigned char>(cmd.command[i]));
 887                 IO::print("< %02X%s\n", cmd.length^0xFF, cmd_hex);
 888         }
 889
 890         control.serial.put(cmd.length^0xFF);
 891         control.serial.write(cmd.command, cmd.length);
 892
 893         unsigned result = 0;
 894         while(1)
 895         {
 896                 bool got_data;
 897                 if(result)
 898                         got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero);
 899                 else
 900                         got_data = IO::poll(control.serial, IO::P_INPUT);
 901
 902                 if(!got_data)
 903                         break;
 904
 905                 unsigned rlength = control.serial.get()^0xFF;
 906                 if(rlength>15)
 907                 {
 908                         IO::print("Invalid length %02X\n", rlength);
 909                         continue;
 910                 }
 911
 912                 char reply[15];
 913                 unsigned pos = 0;
 914                 while(pos<rlength)
 915                         pos += control.serial.read(reply+pos, rlength-pos);
 916
 917                 if(control.debug>=2)
 918                 {
 919                         string reply_hex;
 920                         for(unsigned i=0; i<rlength; ++i)
 921                                 reply_hex += format(" %02X", static_cast<unsigned char>(reply[i]));
 922                         IO::print("> %02X%s\n", rlength^0xFF, reply_hex);
 923                 }
 924
 925                 unsigned r = process_reply(reply, rlength);
 926                 if(r && !result)
 927                         result = r;
 928         }
 929
 930         return result;
 931 }
 932
 933 unsigned ArduControl::ControlThread::process_reply(const char *reply, unsigned rlength)
 934 {
 935         unsigned char type = reply[0];
 936         if((type&0xE0)==0x80)
 937         {
 938                 if(type!=COMMAND_OK)
 939                         IO::print("Error %02X\n", type);
 940                 return type;
 941         }
 942         else if(type==POWER_STATE && rlength==2)
 943         {
 944                 control.power.set(reply[1]);
 945
 946                 Tag tag;
 947                 tag.type = Tag::GENERAL;
 948                 tag.command = POWER;
 949                 tag.serial = control.power.serial;
 950                 control.push_completed_tag(tag);
 951         }
 952         else
 953         {
 954                 for(vector<Task *>::iterator i=tasks.begin(); i!=tasks.end(); ++i)
 955                         (*i)->process_reply(reply, rlength);
 956         }
 957
 958         return 0;
 959 }
 960
 961 } // namespace R2C2