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