+ cycle.erase(i);
+ return;
+ }
+}
+
+ArduControl::Locomotive *ArduControl::RefreshTask::get_next_loco()
+{
+ MutexLock lock(mutex);
+ if(cycle.empty())
+ return 0;
+
+ Locomotive *l = *next;
+ advance();
+ return l;
+}
+
+void ArduControl::RefreshTask::advance()
+{
+ ++next;
+ if(next==cycle.end())
+ {
+ next = cycle.begin();
+ ++round;
+ }
+}
+
+
+ArduControl::S88Task::S88Task(ArduControl &c):
+ Task("S88"),
+ control(c),
+ n_octets(0),
+ octets_remaining(0)
+{ }
+
+bool ArduControl::S88Task::get_work(PendingCommand &cmd)
+{
+ if(octets_remaining || !n_octets)
+ return false;
+
+ Time::TimeStamp t = Time::now();
+ if(last_poll)
+ latency = t-last_poll;
+ last_poll = t;
+
+ octets_remaining = n_octets;
+ cmd.command[0] = S88_READ;
+ cmd.command[1] = octets_remaining;
+ cmd.length = 2;
+
+ sleep(100*Time::msec);
+
+ return true;
+}
+
+void ArduControl::S88Task::process_reply(const char *reply, unsigned length)
+{
+ unsigned char type = reply[0];
+ if(type==S88_DATA && length>2)
+ {
+ unsigned offset = static_cast<unsigned char>(reply[1]);
+ unsigned count = length-2;
+
+ SensorMap::iterator begin = control.sensors.lower_bound(offset*8+1);
+ SensorMap::iterator end = control.sensors.upper_bound((offset+count)*8);
+ for(SensorMap::iterator i=begin; i!=end; ++i)
+ {
+ unsigned bit_index = i->first-1-offset*8;
+ bool state = (reply[2+bit_index/8]>>(7-bit_index%8))&1;
+ i->second.state.set(state);
+
+ Tag tag;
+ tag.type = Tag::SENSOR;
+ tag.command = Sensor::STATE;
+ tag.serial = i->second.state.serial;
+ tag.id = i->first;
+ control.completed_commands.push(tag);
+ }
+
+ if(count>octets_remaining)
+ octets_remaining = 0;
+ else
+ octets_remaining -= count;
+ }
+}
+
+void ArduControl::S88Task::set_n_octets(unsigned n)
+{
+ n_octets = n;
+}
+
+void ArduControl::S88Task::grow_n_octets(unsigned n)
+{
+ if(n>n_octets)
+ n_octets = n;
+}
+
+
+ArduControl::MfxAnnounceTask::MfxAnnounceTask():
+ Task("MfxAnnounce", 1),
+ serial(0)
+{ }
+
+bool ArduControl::MfxAnnounceTask::get_work(PendingCommand &cmd)
+{
+ cmd.command[0] = MFX_ANNOUNCE;
+ cmd.command[1] = serial>>8;
+ cmd.command[2] = serial;
+ cmd.length = 3;
+
+ sleep(400*Time::msec);
+
+ return true;
+}
+
+void ArduControl::MfxAnnounceTask::set_serial(unsigned s)
+{
+ serial = s;
+}
+
+
+ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c):
+ Task("MfxSearch", 1),
+ control(c),
+ next_address(1),
+ size(0),
+ bits(0),
+ misses(0),
+ pending_info(0),
+ read_array(0),
+ read_offset(0),
+ read_length(0),
+ block_size(0)
+{ }
+
+bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
+{
+ if(read_length>0)
+ {
+ cmd.command[0] = MFX_READ;
+ cmd.command[1] = pending_info->address>>8;
+ cmd.command[2] = pending_info->address;
+ unsigned index = read_array*0x40+read_offset;
+ cmd.command[3] = index>>8;
+ cmd.command[4] = index;
+ unsigned length = (read_length>=4 ? 4 : read_length>=2 ? 2 : 1);
+ cmd.command[5] = length;
+ cmd.length = 6;
+
+ sleep(100*Time::msec);
+
+ return true;
+ }
+ else if(pending_info)
+ {
+ queue.push(*pending_info);
+ Tag tag;
+ tag.type = Tag::GENERAL;
+ tag.command = NEW_LOCO;
+ tag.id = pending_info->id;
+ control.completed_commands.push(tag);
+
+ if(control.debug>=1)
+ IO::print("Completed processing locomotive %s at address %d\n", pending_info->name, pending_info->address);
+
+ delete pending_info;
+ pending_info = 0;
+ }
+
+ if(size>32)
+ {
+ unsigned address = 0;
+ if(MfxInfo *existing = control.find_mfx_info(bits))
+ address = existing->address;
+ else
+ address = next_address++;
+
+ if(control.debug>=1)
+ IO::print("Assigning MFX address %d to decoder %08X\n", address, bits);
+
+ pending_info = new MfxInfo;
+ pending_info->protocol = "MFX";
+ pending_info->address = address;
+ pending_info->name = format("%08X", bits);
+ pending_info->id = bits;
+
+ cmd.command[0] = MFX_ASSIGN_ADDRESS;
+ cmd.command[1] = address>>8;
+ cmd.command[2] = address;
+ for(unsigned i=0; i<4; ++i)
+ cmd.command[3+i] = bits>>(24-i*8);
+ cmd.length = 7;
+
+ size = 0;
+ bits = 0;
+ misses = 0;
+
+ read_array = 0;
+ read_offset = 0;
+ read_length = 6;
+
+ return true;
+ }
+
+ cmd.command[0] = MFX_SEARCH;
+ for(unsigned i=0; i<4; ++i)
+ cmd.command[1+i] = bits>>(24-i*8);
+ cmd.command[5] = size;
+ cmd.length = 6;
+
+ sleep(100*Time::msec);
+
+ if(control.debug>=1)
+ IO::print("Search %08X/%d\n", bits, size);
+
+ return true;
+}
+
+void ArduControl::MfxSearchTask::process_reply(const char *reply, unsigned length)
+{
+ unsigned char type = reply[0];
+ if(type==MFX_SEARCH_FEEDBACK && length==2)
+ {
+ if(reply[1])
+ {
+ misses = 0;
+ ++size;
+ }
+ else if(size>0 && misses<6)
+ {
+ ++misses;
+ bits ^= 1<<(32-size);
+ }
+ else
+ {
+ sleep(2*Time::sec);
+ bits = 0;
+ size = 0;
+ misses = 0;
+ }
+ }
+ else if(type==MFX_READ_FEEDBACK && length>=3)
+ {
+ if(reply[1])
+ {
+ misses = 0;
+
+ for(unsigned i=2; i<length; ++i)
+ read_data[read_offset+i-2] = reply[i];
+ read_offset += length-2;
+ read_length -= length-2;
+
+ if(!read_length)