source/libr2c2/arducontrol.cpp

   1 #include <msp/core/maputils.h>
   2 #include <msp/datafile/writer.h>
   3 #include <msp/fs/redirectedpath.h>
   4 #include <msp/fs/stat.h>
   5 #include <msp/io/print.h>
   6 #include <msp/time/utils.h>
   7 #include "arducontrol.h"
   8 #include "tracktype.h"
   9
  10 using namespace std;
  11 using namespace Msp;
  12
  13 namespace R2C2 {
  14
  15 ArduControl::ProtocolInfo ArduControl::protocol_info[2] =
  16 {
  17         { 79, 14, 4 },       // MM
  18         { 0x3FFF, 126, 15 }  // MFX
  19 };
  20
  21 Driver::TelemetryInfo ArduControl::telemetry_info[4] =
  22 {
  23         { "voltage", "Voltage", "V", 1 },
  24         { "current", "Current", "A", 2 },
  25         { "cmd-queue-depth", "Cmd queue", "", 0 },
  26         { "s88-latency", "S88 latency", "ms", 0 }
  27 };
  28
  29 ArduControl::ArduControl(const Options &opts):
  30         serial(opts.get<string>(string(), "ttyUSB0")),
  31         debug(opts.get<unsigned>("debug")),
  32         state_file("arducontrol.state"),
  33         power(false),
  34         halted(false),
  35         active_accessory(0),
  36         command_timeout(200*Time::msec),
  37         s88(*this),
  38         mfx_search(*this),
  39         thread(*this)
  40 {
  41         if(FS::exists(state_file))
  42                 DataFile::load(*this, state_file.str());
  43
  44         unsigned max_address = 0;
  45         for(MfxInfoArray::const_iterator i=mfx_info.begin(); i!=mfx_info.end(); ++i)
  46                 max_address = max(max_address, i->address);
  47         mfx_search.set_next_address(max_address+1);
  48
  49         PendingCommand cmd;
  50         cmd.command[0] = READ_POWER_STATE;
  51         cmd.length = 1;
  52         command_queue.push(cmd);
  53
  54         cmd.command[0] = MFX_SET_STATION_ID;
  55         cmd.command[1] = 'R';
  56         cmd.command[2] = '2';
  57         cmd.command[3] = 'C';
  58         cmd.command[4] = '2';
  59         cmd.length = 5;
  60         command_queue.push(cmd);
  61 }
  62
  63 ArduControl::~ArduControl()
  64 {
  65         thread.exit();
  66 }
  67
  68 void ArduControl::set_power(bool p)
  69 {
  70         if(power.set(p))
  71         {
  72                 PendingCommand cmd(POWER);
  73                 cmd.tag.serial = power.serial;
  74                 cmd.command[0] = (p ? POWER_ON : POWER_OFF);
  75                 cmd.length = 1;
  76                 command_queue.push(cmd);
  77         }
  78 }
  79
  80 void ArduControl::halt(bool h)
  81 {
  82         if(h==halted)
  83                 return;
  84
  85         halted = h;
  86         if(halted)
  87         {
  88                 for(LocomotiveMap::const_iterator i=locomotives.begin(); i!=locomotives.end(); ++i)
  89                         set_loco_speed(i->first, 0);
  90         }
  91
  92         signal_halt.emit(halted);
  93 }
  94
  95 const char *ArduControl::enumerate_protocols(unsigned i) const
  96 {
  97         if(i==0)
  98                 return "MM";
  99         else if(i==1)
 100                 return "MFX";
 101         else
 102                 return 0;
 103 }
 104
 105 ArduControl::Protocol ArduControl::map_protocol(const string &proto_name)
 106 {
 107         if(proto_name=="MM")
 108                 return MM;
 109         else if(proto_name=="MFX")
 110                 return MFX;
 111         else
 112                 throw invalid_argument("ArduControl::map_protocol");
 113 }
 114
 115 unsigned ArduControl::get_protocol_speed_steps(const string &proto_name) const
 116 {
 117         return protocol_info[map_protocol(proto_name)].max_speed;
 118 }
 119
 120 const Driver::DetectedLocomotive *ArduControl::enumerate_detected_locos(unsigned i) const
 121 {
 122         if(i>=mfx_info.size())
 123                 return 0;
 124
 125         return &mfx_info[i];
 126 }
 127
 128 unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
 129 {
 130         if(!addr)
 131                 throw invalid_argument("ArduControl::add_loco");
 132
 133         Protocol proto = map_protocol(proto_name);
 134         if(addr>protocol_info[proto].max_address)
 135                 throw invalid_argument("ArduControl::add_loco");
 136
 137         Locomotive loco(proto, addr);
 138         insert_unique(locomotives, loco.id, loco);
 139
 140         return loco.id;
 141 }
 142
 143 ArduControl::MfxInfoArray::iterator ArduControl::add_mfx_info(const MfxInfo &info)
 144 {
 145         MfxInfoArray::iterator i;
 146         for(i=mfx_info.begin(); (i!=mfx_info.end() && i->id!=info.id); ++i) ;
 147         if(i==mfx_info.end())
 148         {
 149                 mfx_info.push_back(info);
 150                 i = --mfx_info.end();
 151         }
 152         else
 153                 *i = info;
 154         return i;
 155 }
 156
 157 ArduControl::MfxInfo *ArduControl::find_mfx_info(unsigned id)
 158 {
 159         for(MfxInfoArray::iterator i=mfx_info.begin(); i!=mfx_info.end(); ++i)
 160                 if(i->id==id)
 161                         return &*i;
 162         return 0;
 163 }
 164
 165 void ArduControl::remove_loco(unsigned id)
 166 {
 167         Locomotive &loco = get_item(locomotives, id);
 168         refresh.remove_loco(loco);
 169         locomotives.erase(id);
 170 }
 171
 172 void ArduControl::set_loco_speed(unsigned id, unsigned speed)
 173 {
 174         Locomotive &loco = get_item(locomotives, id);
 175         if(speed>protocol_info[loco.proto].max_speed)
 176                 throw invalid_argument("ArduControl::set_loco_speed");
 177
 178         if(speed && halted)
 179                 return;
 180
 181         if(loco.speed.set(speed))
 182         {
 183                 PendingCommand cmd(loco, Locomotive::SPEED);
 184                 command_queue.push(cmd);
 185
 186                 refresh.add_loco(loco);
 187         }
 188 }
 189
 190 void ArduControl::set_loco_reverse(unsigned id, bool rev)
 191 {
 192         Locomotive &loco = get_item(locomotives, id);
 193         if(loco.reverse.set(rev))
 194         {
 195                 PendingCommand cmd(loco, Locomotive::REVERSE);
 196                 command_queue.push(cmd);
 197
 198                 refresh.add_loco(loco);
 199         }
 200 }
 201
 202 void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
 203 {
 204         Locomotive &loco = get_item(locomotives, id);
 205         if(func>protocol_info[loco.proto].max_func)
 206                 throw invalid_argument("ArduControl::set_loco_function");
 207
 208         unsigned mask = 1<<func;
 209         if(loco.funcs.set((loco.funcs&~mask)|(mask*state)))
 210         {
 211                 if(func>0 || loco.proto!=MM)
 212                 {
 213                         PendingCommand cmd(loco, Locomotive::FUNCTIONS, func);
 214                         command_queue.push(cmd);
 215                 }
 216
 217                 refresh.add_loco(loco);
 218         }
 219 }
 220
 221 unsigned ArduControl::add_turnout(unsigned addr, const TrackType &type)
 222 {
 223         if(!addr || !type.is_turnout())
 224                 throw invalid_argument("ArduControl::add_turnout");
 225
 226         return add_accessory(Accessory::TURNOUT, addr, type.get_state_bits(), type.get_paths());
 227 }
 228
 229 void ArduControl::remove_turnout(unsigned addr)
 230 {
 231         remove_accessory(Accessory::TURNOUT, addr);
 232 }
 233
 234 void ArduControl::set_turnout(unsigned addr, unsigned state)
 235 {
 236         set_accessory(Accessory::TURNOUT, addr, state);
 237 }
 238
 239 unsigned ArduControl::get_turnout(unsigned addr) const
 240 {
 241         return get_accessory(Accessory::TURNOUT, addr);
 242 }
 243
 244 unsigned ArduControl::add_signal(unsigned addr, const SignalType &)
 245 {
 246         return add_accessory(Accessory::SIGNAL, addr, 1, 3);
 247 }
 248
 249 void ArduControl::remove_signal(unsigned addr)
 250 {
 251         remove_accessory(Accessory::SIGNAL, addr);
 252 }
 253
 254 void ArduControl::set_signal(unsigned addr, unsigned state)
 255 {
 256         set_accessory(Accessory::SIGNAL, addr, state);
 257 }
 258
 259 unsigned ArduControl::get_signal(unsigned addr) const
 260 {
 261         return get_accessory(Accessory::SIGNAL, addr);
 262 }
 263
 264 unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits, unsigned states)
 265 {
 266         AccessoryMap::iterator i = accessories.lower_bound(addr);
 267         AccessoryMap::iterator j = accessories.upper_bound(addr+bits-1);
 268         if(i!=j)
 269                 throw key_error(addr);
 270         if(i!=accessories.begin())
 271         {
 272                 --i;
 273                 if(i->first+i->second.bits>addr)
 274                         throw key_error(addr);
 275         }
 276
 277         insert_unique(accessories, addr, Accessory(kind, addr, bits, states));
 278         return addr;
 279 }
 280
 281 void ArduControl::remove_accessory(Accessory::Kind kind, unsigned addr)
 282 {
 283         Accessory &acc = get_item(accessories, addr);
 284         if(acc.kind!=kind)
 285                 throw key_error(addr);
 286         accessories.erase(addr);
 287 }
 288
 289 void ArduControl::set_accessory(Accessory::Kind kind, unsigned addr, unsigned state)
 290 {
 291         Accessory &acc = get_item(accessories, addr);
 292         if(acc.kind!=kind)
 293                 throw key_error(addr);
 294
 295         if(state!=acc.target || acc.uncertain)
 296         {
 297                 acc.target = state;
 298                 accessory_queue.push_back(&acc);
 299         }
 300 }
 301
 302 unsigned ArduControl::get_accessory(Accessory::Kind kind, unsigned addr) const
 303 {
 304         const Accessory &acc = get_item(accessories, addr);
 305         if(acc.kind!=kind)
 306                 throw key_error(addr);
 307         return acc.state;
 308 }
 309
 310 void ArduControl::activate_accessory_by_mask(Accessory &acc, unsigned mask)
 311 {
 312         unsigned bit = mask&~(mask-1);
 313         for(active_index=0; (bit>>active_index)>1; ++active_index) ;
 314         acc.state.set((acc.state&~bit)|(acc.target&bit));
 315         if(debug>=1)
 316                 IO::print("Setting accessory %d bit %d, state=%d\n", acc.address, active_index, acc.state.pending);
 317         PendingCommand cmd(acc, Accessory::ACTIVATE, active_index);
 318         command_queue.push(cmd);
 319         active_accessory = &acc;
 320
 321         monitor.reset_peak();
 322 }
 323
 324 unsigned ArduControl::add_sensor(unsigned addr)
 325 {
 326         if(!addr)
 327                 throw invalid_argument("ArduControl::add_sensor");
 328
 329         insert_unique(sensors, addr, Sensor(addr));
 330         s88.grow_n_octets((addr+7)/8);
 331
 332         return addr;
 333 }
 334
 335 void ArduControl::remove_sensor(unsigned addr)
 336 {
 337         remove_existing(sensors, addr);
 338         // TODO update s88.n_octets
 339 }
 340
 341 bool ArduControl::get_sensor(unsigned addr) const
 342 {
 343         return get_item(sensors, addr).state;
 344 }
 345
 346 const Driver::TelemetryInfo *ArduControl::enumerate_telemetry(unsigned i) const
 347 {
 348         if(i<4)
 349                 return telemetry_info+i;
 350         else
 351                 return 0;
 352 }
 353
 354 float ArduControl::get_telemetry_value(const string &name) const
 355 {
 356         if(name==telemetry_info[0].name)
 357                 return monitor.get_voltage();
 358         else if(name==telemetry_info[1].name)
 359                 return monitor.get_current();
 360         else if(name==telemetry_info[2].name)
 361                 return command_queue.size();
 362         else if(name==telemetry_info[3].name)
 363                 return s88.get_latency()/Time::msec;
 364         else
 365                 throw key_error(name);
 366 }
 367
 368 void ArduControl::tick()
 369 {
 370         Tag tag;
 371         while(completed_commands.pop(tag))
 372         {
 373                 if(tag.type==Tag::GENERAL)
 374                 {
 375                         if(tag.command==POWER)
 376                         {
 377                                 if(power.commit(tag.serial))
 378                                         signal_power.emit(power.current);
 379                         }
 380                         else if(tag.command==NEW_LOCO)
 381                         {
 382                                 MfxInfo info;
 383                                 if(mfx_search.pop_info(info))
 384                                 {
 385                                         MfxInfoArray::iterator i = add_mfx_info(info);
 386                                         save_state();
 387                                         signal_locomotive_detected.emit(*i);
 388                                 }
 389                         }
 390                 }
 391                 else if(tag.type==Tag::LOCOMOTIVE)
 392                 {
 393                         LocomotiveMap::iterator i = locomotives.find(tag.id);
 394                         if(i==locomotives.end())
 395                                 continue;
 396
 397                         Locomotive &loco = i->second;
 398                         if(tag.command==Locomotive::SPEED)
 399                         {
 400                                 if(loco.speed.commit(tag.serial))
 401                                         signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
 402                         }
 403                         else if(tag.command==Locomotive::REVERSE)
 404                         {
 405                                 if(loco.reverse.commit(tag.serial))
 406                                         signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
 407                         }
 408                         else if(tag.command==Locomotive::FUNCTIONS)
 409                         {
 410                                 unsigned old = loco.funcs;
 411                                 if(loco.funcs.commit(tag.serial))
 412                                 {
 413                                         unsigned changed = old^loco.funcs;
 414                                         for(unsigned j=0; changed>>j; ++j)
 415                                                 if((changed>>j)&1)
 416                                                         signal_loco_function.emit(loco.id, j, (loco.funcs>>j)&1);
 417                                 }
 418                         }
 419                 }
 420                 else if(tag.type==Tag::ACCESSORY)
 421                 {
 422                         AccessoryMap::iterator i = accessories.find(tag.id);
 423                         if(i==accessories.end())
 424                                 continue;
 425
 426                         Accessory &acc = i->second;
 427                         if(tag.command==Accessory::ACTIVATE)
 428                                 off_timeout = Time::now()+acc.active_time;
 429                         else if(tag.command==Accessory::DEACTIVATE)
 430                         {
 431                                 if(acc.state.commit(tag.serial))
 432                                 {
 433                                         if(&acc==active_accessory)
 434                                                 active_accessory = 0;
 435                                 }
 436                         }
 437                 }
 438                 else if(tag.type==Tag::SENSOR)
 439                 {
 440                         SensorMap::iterator i = sensors.find(tag.id);
 441                         if(i==sensors.end())
 442                                 continue;
 443
 444                         Sensor &sensor = i->second;
 445                         if(tag.command==Sensor::STATE)
 446                         {
 447                                 if(sensor.state.commit(tag.serial))
 448                                         signal_sensor.emit(sensor.address, sensor.state);
 449                         }
 450                 }
 451         }
 452
 453         while(power && !active_accessory && !accessory_queue.empty())
 454         {
 455                 Accessory &acc = *accessory_queue.front();
 456
 457                 if(acc.uncertain)
 458                 {
 459                         unsigned zeroes = acc.uncertain&~acc.target;
 460                         if(zeroes)
 461                                 activate_accessory_by_mask(acc, zeroes);
 462                         else
 463                                 activate_accessory_by_mask(acc, acc.uncertain);
 464                 }
 465                 else if(acc.state!=acc.target)
 466                 {
 467                         unsigned changes = acc.state^acc.target;
 468                         if(!(changes&((1<<acc.bits)-1)))
 469                         {
 470                                 // All remaining changes are in non-physical bits
 471                                 acc.state.set(acc.state^changes);
 472                                 acc.state.commit(acc.state.serial);
 473                         }
 474                         else
 475                         {
 476                                 unsigned toggle_bit = 0;
 477                                 for(unsigned bit=1; (!toggle_bit && bit<=changes); bit<<=1)
 478                                         if((changes&bit) && (acc.valid_states&(1<<(acc.state^bit))))
 479                                                 toggle_bit = bit;
 480
 481                                 activate_accessory_by_mask(acc, toggle_bit);
 482                         }
 483                 }
 484                 else
 485                 {
 486                         accessory_queue.pop_front();
 487
 488                         if(acc.state==acc.target)
 489                         {
 490                                 if(acc.kind==Accessory::TURNOUT)
 491                                         signal_turnout.emit(acc.address, acc.state);
 492                                 else if(acc.kind==Accessory::SIGNAL)
 493                                         signal_signal.emit(acc.address, acc.state);
 494                         }
 495                 }
 496         }
 497
 498         if(active_accessory && off_timeout)
 499         {
 500                 bool success = (monitor.get_peak()>0.35f && monitor.get_current()<monitor.get_peak()-0.2f);
 501                 Time::TimeStamp t = Time::now();
 502                 if(t>off_timeout || success)
 503                 {
 504                         Accessory &acc = *active_accessory;
 505
 506                         unsigned bit = 1<<active_index;
 507
 508                         // Assume success if we were uncertain of the physical setting
 509                         if(acc.uncertain&bit)
 510                                 acc.uncertain &= ~bit;
 511                         else if(acc.kind==Accessory::TURNOUT && !success)
 512                         {
 513                                 if(debug>=1)
 514                                         IO::print("Peak current only %.2f A\n", monitor.get_peak());
 515                                 signal_turnout_failed.emit(acc.address);
 516                                 acc.state.rollback();
 517                                 if(acc.valid_states&(1<<(acc.target^bit)))
 518                                         acc.target ^= bit;
 519                                 else
 520                                         acc.target = acc.state;
 521                         }
 522
 523                         off_timeout = Time::TimeStamp();
 524                         PendingCommand cmd(acc, Accessory::DEACTIVATE, active_index);
 525                         command_queue.push(cmd);
 526                 }
 527         }
 528 }
 529
 530 void ArduControl::flush()
 531 {
 532         while(!command_queue.empty() || (power && !accessory_queue.empty()))
 533                 tick();
 534 }
 535
 536 void ArduControl::save_state() const
 537 {
 538         FS::RedirectedPath tmp_file(state_file);
 539         IO::BufferedFile out(tmp_file.str(), IO::M_WRITE);
 540         DataFile::Writer writer(out);
 541
 542         writer.write((DataFile::Statement("mfx_announce_serial"), mfx_announce.get_serial()));
 543         for(MfxInfoArray::const_iterator i=mfx_info.begin(); i!=mfx_info.end(); ++i)
 544         {
 545                 DataFile::Statement st("mfx_locomotive");
 546                 st.append(i->id);
 547                 st.sub.push_back((DataFile::Statement("address"), i->address));
 548                 st.sub.push_back((DataFile::Statement("name"), i->name));
 549                 writer.write(st);
 550         }
 551 }
 552
 553
 554 ArduControl::Tag::Tag():
 555         type(NONE),
 556         command(0),
 557         serial(0),
 558         id(0)
 559 { }
 560
 561
 562 ArduControl::Locomotive::Locomotive(Protocol p, unsigned a):
 563         id((p<<16)|a),
 564         proto(p),
 565         address(a),
 566         speed(0),
 567         reverse(false),
 568         funcs(0),
 569         last_change_age(0)
 570 { }
 571
 572 unsigned ArduControl::Locomotive::create_speed_dir_command(char *buffer) const
 573 {
 574         if(proto==MM)
 575         {
 576                 buffer[0] = MOTOROLA_SPEED_DIRECTION;
 577                 buffer[1] = address;
 578                 buffer[2] = funcs.pending&1;
 579                 buffer[3] = speed.pending+reverse.pending*0x80;
 580                 return 4;
 581         }
 582         else if(proto==MFX)
 583         {
 584                 buffer[0] = MFX_SPEED;
 585                 buffer[1] = address>>8;
 586                 buffer[2] = address;
 587                 buffer[3] = speed.pending+reverse.pending*0x80;
 588                 return 4;
 589         }
 590         else
 591                 return 0;
 592 }
 593
 594 unsigned ArduControl::Locomotive::create_speed_func_command(unsigned f, char *buffer) const
 595 {
 596         if(proto==MM)
 597         {
 598                 if(f<1 || f>4)
 599                         throw invalid_argument("Locomotive::create_speed_func_command");
 600
 601                 buffer[0] = MOTOROLA_SPEED_FUNCTION;
 602                 buffer[1] = address;
 603                 buffer[2] = (f<<4)|(((funcs.pending>>f)&1)<<1)|(funcs.pending&1);
 604                 buffer[3] = speed.pending;
 605                 return 4;
 606         }
 607         else if(proto==MFX)
 608         {
 609                 bool f16 = (funcs.pending>0xFF);
 610                 buffer[0] = (f16 ? MFX_SPEED_FUNCS16 : MFX_SPEED_FUNCS8);
 611                 buffer[1] = address>>8;
 612                 buffer[2] = address;
 613                 buffer[3] = speed.pending+reverse.pending*0x80;
 614                 if(f16)
 615                 {
 616                         buffer[4] = funcs.pending>>8;
 617                         buffer[5] = funcs.pending;
 618                         return 6;
 619                 }
 620                 else
 621                 {
 622                         buffer[4] = funcs.pending;
 623                         return 5;
 624                 }
 625         }
 626         else
 627                 return 0;
 628 }
 629
 630
 631 ArduControl::Accessory::Accessory(Kind k, unsigned a, unsigned b, unsigned s):
 632         kind(k),
 633         address(a),
 634         bits(b),
 635         valid_states(s),
 636         state(0),
 637         uncertain((1<<bits)-1),
 638         target(0),
 639         active_time((bits*700)*Time::msec)
 640 { }
 641
 642 unsigned ArduControl::Accessory::create_state_command(unsigned b, bool c, char *buffer) const
 643 {
 644         if(b>=bits)
 645                 throw invalid_argument("Accessory::create_state_command");
 646
 647         unsigned a = (address+b+3)*2;
 648         if(!((state.pending>>b)&1))
 649                 ++a;
 650         buffer[0] = MOTOROLA_SOLENOID;
 651         buffer[1] = a>>3;
 652         buffer[2] = ((a&7)<<4)|c;
 653         return 3;
 654 }
 655
 656
 657 ArduControl::Sensor::Sensor(unsigned a):
 658         address(a),
 659         state(false)
 660 { }
 661
 662
 663 ArduControl::PendingCommand::PendingCommand():
 664         length(0),
 665         repeat_count(1)
 666 { }
 667
 668 ArduControl::PendingCommand::PendingCommand(GeneralCommand cmd):
 669         length(0),
 670         repeat_count(1)
 671 {
 672         tag.type = Tag::GENERAL;
 673         tag.command = cmd;
 674 }
 675
 676 ArduControl::PendingCommand::PendingCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index):
 677         repeat_count(8)
 678 {
 679         tag.type = Tag::LOCOMOTIVE;
 680         tag.command = cmd;
 681         tag.id = loco.id;
 682         if(cmd==Locomotive::SPEED)
 683         {
 684                 tag.serial = loco.speed.serial;
 685                 length = loco.create_speed_dir_command(command);
 686         }
 687         else if(cmd==Locomotive::REVERSE)
 688         {
 689                 tag.serial = loco.reverse.serial;
 690                 length = loco.create_speed_dir_command(command);
 691         }
 692         else if(cmd==Locomotive::FUNCTIONS)
 693         {
 694                 tag.serial = loco.funcs.serial;
 695                 length = loco.create_speed_func_command(index, command);
 696         }
 697         else
 698                 throw invalid_argument("PendingCommand");
 699 }
 700
 701 ArduControl::PendingCommand::PendingCommand(Accessory &acc, Accessory::Command cmd, unsigned index):
 702         repeat_count(1)
 703 {
 704         tag.type = Tag::ACCESSORY;
 705         tag.command = cmd;
 706         tag.id = acc.address;
 707         if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE)
 708         {
 709                 tag.serial = acc.state.serial;
 710                 length = acc.create_state_command(index, (cmd==Accessory::ACTIVATE), command);
 711         }
 712         else
 713                 throw invalid_argument("PendingCommand");
 714 }
 715
 716
 717 template<typename T>
 718 void ArduControl::Queue<T>::push(const T &item)
 719 {
 720         MutexLock lock(mutex);
 721         items.push_back(item);
 722 }
 723
 724 template<typename T>
 725 bool ArduControl::Queue<T>::pop(T &item)
 726 {
 727         MutexLock lock(mutex);
 728         if(items.empty())
 729                 return false;
 730
 731         item = items.front();
 732         items.pop_front();
 733         return true;
 734 }
 735
 736 template<typename T>
 737 unsigned ArduControl::Queue<T>::size() const
 738 {
 739         return items.size();
 740 }
 741
 742 template<typename T>
 743 bool ArduControl::Queue<T>::empty() const
 744 {
 745         return items.empty();
 746 }
 747
 748
 749 bool ArduControl::CommandQueueTask::get_work(PendingCommand &cmd)
 750 {
 751         return queue.pop(cmd);
 752 }
 753
 754 void ArduControl::CommandQueueTask::push(const PendingCommand &cmd)
 755 {
 756         queue.push(cmd);
 757 }
 758
 759
 760 ArduControl::Task::Task(const string &n, unsigned p):
 761         name(n),
 762         priority(p)
 763 { }
 764
 765 void ArduControl::Task::sleep(const Time::TimeDelta &dt)
 766 {
 767         sleep_timeout = Time::now()+dt;
 768 }
 769
 770
 771 ArduControl::CommandQueueTask::CommandQueueTask():
 772         Task("CommandQueue")
 773 { }
 774
 775
 776 ArduControl::RefreshTask::RefreshTask():
 777         Task("Refresh", 2),
 778         next(cycle.end()),
 779         round(0),
 780         loco(0),
 781         phase(0)
 782 { }
 783
 784 bool ArduControl::RefreshTask::get_work(PendingCommand &cmd)
 785 {
 786         if(loco && loco->proto==MM && phase==0)
 787         {
 788                 cmd.length = loco->create_speed_func_command(round%4+1, cmd.command);
 789                 cmd.repeat_count = 2;
 790                 ++phase;
 791                 return true;
 792         }
 793
 794         loco = get_next_loco();
 795         if(!loco)
 796                 return false;
 797
 798         phase = 0;
 799         if(loco->proto==MM)
 800         {
 801                 cmd.length = loco->create_speed_dir_command(cmd.command);
 802                 cmd.repeat_count = 2;
 803         }
 804         else if(loco->proto==MFX)
 805                 cmd.length = loco->create_speed_func_command(0, cmd.command);
 806         else
 807                 return false;
 808
 809         return true;
 810 }
 811
 812 void ArduControl::RefreshTask::add_loco(Locomotive &l)
 813 {
 814         MutexLock lock(mutex);
 815         cycle.push_back(&l);
 816         if(cycle.size()>15)
 817         {
 818                 LocomotivePtrList::iterator oldest = cycle.begin();
 819                 for(LocomotivePtrList::iterator i=cycle.begin(); ++i!=cycle.end(); )
 820                         if((*i)->last_change_age>(*oldest)->last_change_age)
 821                                 oldest = i;
 822                 if(oldest==next)
 823                         advance();
 824                 cycle.erase(oldest);
 825         }
 826         if(next==cycle.end())
 827                 next = cycle.begin();
 828 }
 829
 830 void ArduControl::RefreshTask::remove_loco(Locomotive &l)
 831 {
 832         MutexLock lock(mutex);
 833         for(LocomotivePtrList::iterator i=cycle.begin(); i!=cycle.end(); ++i)
 834                 if(*i==&l)
 835                 {
 836                         if(i==next)
 837                         {
 838                                 if(cycle.size()>1)
 839                                         advance();
 840                                 else
 841                                         next = cycle.end();
 842                         }
 843                         cycle.erase(i);
 844                         return;
 845                 }
 846 }
 847
 848 ArduControl::Locomotive *ArduControl::RefreshTask::get_next_loco()
 849 {
 850         MutexLock lock(mutex);
 851         if(cycle.empty())
 852                 return 0;
 853
 854         Locomotive *l = *next;
 855         advance();
 856         return l;
 857 }
 858
 859 void ArduControl::RefreshTask::advance()
 860 {
 861         ++next;
 862         if(next==cycle.end())
 863         {
 864                 next = cycle.begin();
 865                 ++round;
 866         }
 867 }
 868
 869
 870 ArduControl::S88Task::S88Task(ArduControl &c):
 871         Task("S88"),
 872         control(c),
 873         n_octets(0),
 874         octets_remaining(0)
 875 { }
 876
 877 bool ArduControl::S88Task::get_work(PendingCommand &cmd)
 878 {
 879         if(octets_remaining || !n_octets)
 880                 return false;
 881
 882         Time::TimeStamp t = Time::now();
 883         if(last_poll)
 884                 latency = t-last_poll;
 885         last_poll = t;
 886
 887         octets_remaining = n_octets;
 888         cmd.command[0] = S88_READ;
 889         cmd.command[1] = octets_remaining;
 890         cmd.length = 2;
 891
 892         sleep(100*Time::msec);
 893
 894         return true;
 895 }
 896
 897 void ArduControl::S88Task::process_reply(const char *reply, unsigned length)
 898 {
 899         unsigned char type = reply[0];
 900         if(type==S88_DATA && length>2)
 901         {
 902                 unsigned offset = static_cast<unsigned char>(reply[1]);
 903                 unsigned count = length-2;
 904
 905                 SensorMap::iterator begin = control.sensors.lower_bound(offset*8+1);
 906                 SensorMap::iterator end = control.sensors.upper_bound((offset+count)*8);
 907                 for(SensorMap::iterator i=begin; i!=end; ++i)
 908                 {
 909                         unsigned bit_index = i->first-1-offset*8;
 910                         bool state = (reply[2+bit_index/8]>>(7-bit_index%8))&1;
 911                         i->second.state.set(state);
 912
 913                         Tag tag;
 914                         tag.type = Tag::SENSOR;
 915                         tag.command = Sensor::STATE;
 916                         tag.serial = i->second.state.serial;
 917                         tag.id = i->first;
 918                         control.completed_commands.push(tag);
 919                 }
 920
 921                 if(count>octets_remaining)
 922                         octets_remaining = 0;
 923                 else
 924                         octets_remaining -= count;
 925         }
 926 }
 927
 928 void ArduControl::S88Task::set_n_octets(unsigned n)
 929 {
 930         n_octets = n;
 931 }
 932
 933 void ArduControl::S88Task::grow_n_octets(unsigned n)
 934 {
 935         if(n>n_octets)
 936                 n_octets = n;
 937 }
 938
 939
 940 ArduControl::MfxAnnounceTask::MfxAnnounceTask():
 941         Task("MfxAnnounce", 1),
 942         serial(0)
 943 { }
 944
 945 bool ArduControl::MfxAnnounceTask::get_work(PendingCommand &cmd)
 946 {
 947         cmd.command[0] = MFX_ANNOUNCE;
 948         cmd.command[1] = serial>>8;
 949         cmd.command[2] = serial;
 950         cmd.length = 3;
 951
 952         sleep(400*Time::msec);
 953
 954         return true;
 955 }
 956
 957 void ArduControl::MfxAnnounceTask::set_serial(unsigned s)
 958 {
 959         serial = s;
 960 }
 961
 962
 963 ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c):
 964         Task("MfxSearch", 1),
 965         control(c),
 966         next_address(1),
 967         size(0),
 968         bits(0),
 969         misses(0),
 970         pending_info(0),
 971         read_array(0),
 972         read_offset(0),
 973         read_length(0),
 974         block_size(0)
 975 { }
 976
 977 bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
 978 {
 979         if(read_length>0)
 980         {
 981                 cmd.command[0] = MFX_READ;
 982                 cmd.command[1] = pending_info->address>>8;
 983                 cmd.command[2] = pending_info->address;
 984                 unsigned index = read_array*0x40+read_offset;
 985                 cmd.command[3] = index>>8;
 986                 cmd.command[4] = index;
 987                 unsigned length = (read_length>=4 ? 4 : read_length>=2 ? 2 : 1);
 988                 cmd.command[5] = length;
 989                 cmd.length = 6;
 990
 991                 sleep(100*Time::msec);
 992
 993                 return true;
 994         }
 995         else if(pending_info)
 996         {
 997                 queue.push(*pending_info);
 998                 Tag tag;
 999                 tag.type = Tag::GENERAL;
1000                 tag.command = NEW_LOCO;
1001                 tag.id = pending_info->id;
1002                 control.completed_commands.push(tag);
1003
1004                 if(control.debug>=1)
1005                         IO::print("Completed processing locomotive %s at address %d\n", pending_info->name, pending_info->address);
1006
1007                 delete pending_info;
1008                 pending_info = 0;
1009         }
1010
1011         if(size>32)
1012         {
1013                 unsigned address = 0;
1014                 if(MfxInfo *existing = control.find_mfx_info(bits))
1015                         address = existing->address;
1016                 else
1017                         address = next_address++;
1018
1019                 if(control.debug>=1)
1020                         IO::print("Assigning MFX address %d to decoder %08X\n", address, bits);
1021
1022                 pending_info = new MfxInfo;
1023                 pending_info->protocol = "MFX";
1024                 pending_info->address = address;
1025                 pending_info->name = format("%08X", bits);
1026                 pending_info->id = bits;
1027
1028                 cmd.command[0] = MFX_ASSIGN_ADDRESS;
1029                 cmd.command[1] = address>>8;
1030                 cmd.command[2] = address;
1031                 for(unsigned i=0; i<4; ++i)
1032                         cmd.command[3+i] = bits>>(24-i*8);
1033                 cmd.length = 7;
1034
1035                 size = 0;
1036                 bits = 0;
1037                 misses = 0;
1038
1039                 read_array = 0;
1040                 read_offset = 0;
1041                 read_length = 6;
1042
1043                 return true;
1044         }
1045
1046         cmd.command[0] = MFX_SEARCH;
1047         for(unsigned i=0; i<4; ++i)
1048                 cmd.command[1+i] = bits>>(24-i*8);
1049         cmd.command[5] = size;
1050         cmd.length = 6;
1051
1052         sleep(100*Time::msec);
1053
1054         if(control.debug>=1)
1055                 IO::print("Search %08X/%d\n", bits, size);
1056
1057         return true;
1058 }
1059
1060 void ArduControl::MfxSearchTask::process_reply(const char *reply, unsigned length)
1061 {
1062         unsigned char type = reply[0];
1063         if(type==MFX_SEARCH_FEEDBACK && length==2)
1064         {
1065                 if(reply[1])
1066                 {
1067                         misses = 0;
1068                         ++size;
1069                 }
1070                 else if(size>0 && misses<6)
1071                 {
1072                         ++misses;
1073                         bits ^= 1<<(32-size);
1074                 }
1075                 else
1076                 {
1077                         sleep(2*Time::sec);
1078                         bits = 0;
1079                         size = 0;
1080                         misses = 0;
1081                 }
1082         }
1083         else if(type==MFX_READ_FEEDBACK && length>=3)
1084         {
1085                 if(reply[1])
1086                 {
1087                         misses = 0;
1088
1089                         for(unsigned i=2; i<length; ++i)
1090                                 read_data[read_offset+i-2] = reply[i];
1091                         read_offset += length-2;
1092                         read_length -= length-2;
1093
1094                         if(!read_length)
1095                         {
1096                                 if(read_array==0)
1097                                         block_size = static_cast<unsigned char>(read_data[4])*static_cast<unsigned char>(read_data[5]);
1098
1099                                 bool array_handled = false;
1100                                 if(read_data[0]==0x18)
1101                                 {
1102                                         for(unsigned i=1; i<read_offset; ++i)
1103                                                 if(!read_data[i])
1104                                                 {
1105                                                         pending_info->name = string(read_data+1, i-1);
1106                                                         array_handled = true;
1107                                                         break;
1108                                                 }
1109
1110                                         if(!array_handled)
1111                                                 read_length = 4;
1112                                 }
1113                                 else
1114                                         array_handled = true;
1115
1116                                 if(array_handled && control.debug>=1)
1117                                 {
1118                                         IO::print("MFX CA %03X:", read_array);
1119                                         for(unsigned i=0; i<read_offset; ++i)
1120                                                 IO::print(" %02X", static_cast<unsigned char>(read_data[i]));
1121                                         IO::print("\n");
1122                                 }
1123
1124                                 if(array_handled && read_array<block_size)
1125                                 {
1126                                         ++read_array;
1127                                         read_offset = 0;
1128                                         read_length = 1;
1129                                 }
1130                         }
1131                 }
1132                 else
1133                 {
1134                         ++misses;
1135                         if(misses>=10)
1136                         {
1137                                 if(control.debug>=1)
1138                                         IO::print("Failed to read MFX configuration from %d\n", pending_info->address);
1139                                 read_length = 0;
1140                         }
1141                 }
1142         }
1143 }
1144
1145 void ArduControl::MfxSearchTask::set_next_address(unsigned a)
1146 {
1147         next_address = a;
1148 }
1149
1150 bool ArduControl::MfxSearchTask::pop_info(MfxInfo &info)
1151 {
1152         return queue.pop(info);
1153 }
1154
1155
1156 ArduControl::MonitorTask::MonitorTask():
1157         Task("Monitor"),
1158         voltage(0),
1159         current(0),
1160         base_level(0),
1161         peak_level(0),
1162         next_type(0)
1163 { }
1164
1165 bool ArduControl::MonitorTask::get_work(PendingCommand &cmd)
1166 {
1167         if(next_type==0)
1168                 cmd.command[0] = READ_INPUT_VOLTAGE;
1169         else
1170                 cmd.command[0] = READ_TRACK_CURRENT;
1171         cmd.length = 1;
1172
1173         sleep(200*Time::msec);
1174         next_type = (next_type+1)%5;
1175
1176         return true;
1177 }
1178
1179 void ArduControl::MonitorTask::process_reply(const char *reply, unsigned length)
1180 {
1181         unsigned char type = reply[0];
1182         if(type==INPUT_VOLTAGE && length==3)
1183                 voltage = ((static_cast<unsigned char>(reply[1])<<8) | static_cast<unsigned char>(reply[2]))/1000.0f;
1184         else if(type==TRACK_CURRENT && length==5)
1185         {
1186                 current = ((static_cast<unsigned char>(reply[1])<<8) | static_cast<unsigned char>(reply[2]))/1000.0f;
1187                 float peak = ((static_cast<unsigned char>(reply[3])<<8) | static_cast<unsigned char>(reply[4]))/1000.0f;
1188                 peak_level = max(peak_level, peak);
1189                 base_level = min(base_level, current);
1190         }
1191 }
1192
1193 void ArduControl::MonitorTask::reset_peak()
1194 {
1195         base_level = current;
1196         peak_level = current;
1197 }
1198
1199
1200 ArduControl::ControlThread::ControlThread(ArduControl &c):
1201         control(c),
1202         done(false)
1203 {
1204         tasks.push_back(&control.command_queue);
1205         tasks.push_back(&control.monitor);
1206         tasks.push_back(&control.mfx_announce);
1207         tasks.push_back(&control.mfx_search);
1208         tasks.push_back(&control.s88);
1209         tasks.push_back(&control.refresh);
1210
1211         launch();
1212 }
1213
1214 void ArduControl::ControlThread::exit()
1215 {
1216         done = true;
1217         join();
1218 }
1219
1220 void ArduControl::ControlThread::main()
1221 {
1222         init_baud_rate();
1223
1224         while(!done)
1225         {
1226                 PendingCommand cmd;
1227                 if(get_work(cmd))
1228                 {
1229                         bool success = true;
1230                         bool resync = false;
1231                         for(unsigned i=0; (success && i<cmd.repeat_count); ++i)
1232                         {
1233                                 unsigned result = do_command(cmd, control.command_timeout);
1234                                 success = (result==COMMAND_OK);
1235                                 resync = (result==0);
1236                         }
1237
1238                         if(success && cmd.tag)
1239                                 control.completed_commands.push(cmd.tag);
1240
1241                         if(resync)
1242                         {
1243                                 if(control.debug>=1)
1244                                         IO::print("Synchronization with ArduControl lost, attempting to recover\n");
1245                                 for(unsigned i=0; (resync && i<16); ++i)
1246                                 {
1247                                         control.serial.put('\xFF');
1248                                         while(IO::poll(control.serial, IO::P_INPUT, control.command_timeout))
1249                                                 resync = (control.serial.get()!=0xFF);
1250                                 }
1251                                 if(resync)
1252                                 {
1253                                         if(control.debug>=1)
1254                                                 IO::print("Resynchronization failed, giving up\n");
1255                                         done = true;
1256                                 }
1257                                 else
1258                                 {
1259                                         if(control.debug>=1)
1260                                                 IO::print("Resynchronization successful\n");
1261                                         if(cmd.tag)
1262                                                 control.command_queue.push(cmd);
1263                                 }
1264                         }
1265                 }
1266                 else
1267                         Time::sleep(10*Time::msec);
1268         }
1269 }
1270
1271 void ArduControl::ControlThread::init_baud_rate()
1272 {
1273         static unsigned rates[] = { 57600, 9600, 19200, 38400, 0 };
1274         unsigned rate = 0;
1275         control.serial.set_data_bits(8);
1276         control.serial.set_parity(IO::Serial::NONE);
1277         control.serial.set_stop_bits(1);
1278         for(unsigned i=0; rates[i]; ++i)
1279         {
1280                 control.serial.set_baud_rate(rates[i]);
1281                 control.serial.put('\xFF');
1282                 if(IO::poll(control.serial, IO::P_INPUT, 500*Time::msec))
1283                 {
1284                         int c = control.serial.get();
1285                         if(c==0xFF)
1286                         {
1287                                 rate = rates[i];
1288                                 break;
1289                         }
1290                 }
1291         }
1292
1293         if(!rate)
1294         {
1295                 if(control.debug>=1)
1296                         IO::print("ArduControl detection failed\n");
1297                 done = true;
1298                 return;
1299         }
1300
1301         if(control.debug>=1)
1302                 IO::print("ArduControl detected at %d bits/s\n", rate);
1303
1304         if(rate!=rates[0])
1305         {
1306                 PendingCommand cmd;
1307                 cmd.command[0] = SET_BAUD_RATE;
1308                 cmd.command[1] = rates[0]>>8;
1309                 cmd.command[2] = rates[0];
1310                 cmd.length = 3;
1311                 if(do_command(cmd, Time::sec)==COMMAND_OK)
1312                 {
1313                         control.serial.set_baud_rate(rates[0]);
1314                         Time::sleep(Time::sec);
1315                         if(do_command(cmd, Time::sec)==COMMAND_OK)
1316                         {
1317                                 if(control.debug>=1)
1318                                         IO::print("Rate changed to %d bits/s\n", rates[0]);
1319                         }
1320                 }
1321         }
1322 }
1323
1324 bool ArduControl::ControlThread::get_work(PendingCommand &cmd)
1325 {
1326         Time::TimeStamp t = Time::now();
1327
1328         unsigned count = 0;
1329         for(; (count<tasks.size() && tasks[count]->get_sleep_timeout()<=t); ++count) ;
1330
1331         for(; count>0; --count)
1332         {
1333                 unsigned i = 0;
1334                 for(unsigned j=1; j<count; ++j)
1335                         if(tasks[j]->get_priority()<tasks[i]->get_priority())
1336                                 i = j;
1337
1338                 Task *task = tasks[i];
1339                 bool result = task->get_work(cmd);
1340
1341                 Time::TimeStamp st = max(task->get_sleep_timeout(), t);
1342                 for(; (i+1<tasks.size() && tasks[i+1]->get_sleep_timeout()<=st); ++i)
1343                         tasks[i] = tasks[i+1];
1344                 tasks[i] = task;
1345
1346                 if(result)
1347                 {
1348                         if(control.debug>=2)
1349                                 IO::print("Scheduled task %s\n", task->get_name());
1350                         return true;
1351                 }
1352         }
1353
1354         // As fallback, send an idle packet for the MM protocol
1355         cmd.command[0] = MOTOROLA_SPEED;
1356         cmd.command[1] = 80;
1357         cmd.command[2] = 0;
1358         cmd.command[3] = 0;
1359         cmd.length = 4;
1360
1361         return true;
1362 }
1363
1364 unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd, const Time::TimeDelta &timeout)
1365 {
1366         if(control.debug>=2)
1367         {
1368                 string cmd_hex;
1369                 for(unsigned i=0; i<cmd.length; ++i)
1370                         cmd_hex += format(" %02X", static_cast<unsigned char>(cmd.command[i]));
1371                 IO::print("< %02X%s\n", cmd.length^0xFF, cmd_hex);
1372         }
1373
1374         control.serial.put(cmd.length^0xFF);
1375         control.serial.write(cmd.command, cmd.length);
1376
1377         unsigned result = 0;
1378         while(1)
1379         {
1380                 bool got_data;
1381                 if(result)
1382                         got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero);
1383                 else
1384                         got_data = IO::poll(control.serial, IO::P_INPUT, timeout);
1385
1386                 if(!got_data)
1387                         break;
1388
1389                 unsigned rlength = control.serial.get()^0xFF;
1390                 if(rlength>15)
1391                 {
1392                         IO::print("Invalid length %02X\n", rlength);
1393                         continue;
1394                 }
1395
1396                 char reply[15];
1397                 unsigned pos = 0;
1398                 while(pos<rlength)
1399                 {
1400                         if(!IO::poll(control.serial, IO::P_INPUT, timeout))
1401                                 return 0;
1402                         pos += control.serial.read(reply+pos, rlength-pos);
1403                 }
1404
1405                 if(control.debug>=2)
1406                 {
1407                         string reply_hex;
1408                         for(unsigned i=0; i<rlength; ++i)
1409                                 reply_hex += format(" %02X", static_cast<unsigned char>(reply[i]));
1410                         IO::print("> %02X%s\n", rlength^0xFF, reply_hex);
1411                 }
1412
1413                 unsigned r = process_reply(reply, rlength);
1414                 if(r && !result)
1415                         result = r;
1416         }
1417
1418         return result;
1419 }
1420
1421 unsigned ArduControl::ControlThread::process_reply(const char *reply, unsigned rlength)
1422 {
1423         unsigned char type = reply[0];
1424         if((type&0xE0)==0x80)
1425         {
1426                 if(type!=COMMAND_OK)
1427                         IO::print("Error %02X\n", type);
1428                 return type;
1429         }
1430         else if(type==POWER_STATE && rlength==2)
1431                 set_power(reply[1]);
1432         else if(type==OVERCURRENT)
1433         {
1434                 set_power(false);
1435                 IO::print("Overcurrent detected!\n");
1436         }
1437         else
1438         {
1439                 for(vector<Task *>::iterator i=tasks.begin(); i!=tasks.end(); ++i)
1440                         (*i)->process_reply(reply, rlength);
1441         }
1442
1443         return 0;
1444 }
1445
1446 void ArduControl::ControlThread::set_power(bool p)
1447 {
1448         control.power.set(p);
1449
1450         Tag tag;
1451         tag.type = Tag::GENERAL;
1452         tag.command = POWER;
1453         tag.serial = control.power.serial;
1454         control.completed_commands.push(tag);
1455 }
1456
1457
1458 ArduControl::Loader::Loader(ArduControl &c):
1459         DataFile::ObjectLoader<ArduControl>(c)
1460 {
1461         add("mfx_announce_serial", &Loader::mfx_announce_serial);
1462         add("mfx_locomotive", &Loader::mfx_locomotive);
1463 }
1464
1465 void ArduControl::Loader::mfx_announce_serial(unsigned s)
1466 {
1467         obj.mfx_announce.set_serial(s);
1468 }
1469
1470 void ArduControl::Loader::mfx_locomotive(unsigned id)
1471 {
1472         MfxInfo info;
1473         info.id = id;
1474         info.protocol = "MFX";
1475         load_sub(info);
1476         obj.add_mfx_info(info);
1477 }
1478
1479
1480 ArduControl::MfxInfo::Loader::Loader(MfxInfo &i):
1481         DataFile::ObjectLoader<MfxInfo>(i)
1482 {
1483         add("address", static_cast<unsigned MfxInfo::*>(&MfxInfo::address));
1484         add("name", static_cast<string MfxInfo::*>(&MfxInfo::name));
1485 }
1486
1487 } // namespace R2C2