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