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