X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Farducontrol.cpp;h=4acfc60e0e761a0c5aaa7d4c260a0023988a8034;hb=aa48746158b19ad795a3cc656c4dfe17044baf54;hp=e5d94ef9d5d180e0bf3f7f5a5cfdcd847f48ae77;hpb=daecd71f20c275fb6df128a3aaa4cfdda24b6886;p=r2c2.git diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp index e5d94ef..4acfc60 100644 --- a/source/libr2c2/arducontrol.cpp +++ b/source/libr2c2/arducontrol.cpp @@ -1,4 +1,7 @@ #include +#include +#include +#include #include #include #include "arducontrol.h" @@ -11,23 +14,42 @@ namespace R2C2 { ArduControl::ProtocolInfo ArduControl::protocol_info[2] = { - { 79, 14, 4 } // MM + { 79, 14, 4 }, // MM + { 0x3FFF, 126, 15 } // MFX }; -ArduControl::ArduControl(const string &dev): - serial(dev), - debug(true), +ArduControl::ArduControl(const Options &opts): + serial(opts.get(string(), "ttyUSB0")), + debug(opts.get("debug")), + state_file("arducontrol.state"), power(false), - next_refresh(refresh_cycle.end()), - refresh_counter(0), + halted(false), active_accessory(0), - n_s88_octets(0), + command_timeout(200*Time::msec), + s88(*this), + mfx_search(*this), thread(*this) { - QueuedCommand cmd; + if(FS::exists(state_file)) + DataFile::load(*this, state_file.str()); + + unsigned max_address = 0; + for(MfxInfoArray::const_iterator i=mfx_info.begin(); i!=mfx_info.end(); ++i) + max_address = max(max_address, i->address); + mfx_search.set_next_address(max_address+1); + + PendingCommand cmd; cmd.command[0] = READ_POWER_STATE; cmd.length = 1; - push_command(cmd); + command_queue.push(cmd); + + cmd.command[0] = MFX_SET_STATION_ID; + cmd.command[1] = 'R'; + cmd.command[2] = '2'; + cmd.command[3] = 'C'; + cmd.command[4] = '2'; + cmd.length = 5; + command_queue.push(cmd); } ArduControl::~ArduControl() @@ -39,22 +61,35 @@ void ArduControl::set_power(bool p) { if(power.set(p)) { - QueuedCommand cmd(POWER); + PendingCommand cmd(POWER); cmd.tag.serial = power.serial; cmd.command[0] = (p ? POWER_ON : POWER_OFF); cmd.length = 1; - push_command(cmd); + command_queue.push(cmd); } } -void ArduControl::halt(bool) +void ArduControl::halt(bool h) { + if(h==halted) + return; + + halted = h; + if(halted) + { + for(LocomotiveMap::const_iterator i=locomotives.begin(); i!=locomotives.end(); ++i) + set_loco_speed(i->first, 0); + } + + signal_halt.emit(halted); } const char *ArduControl::enumerate_protocols(unsigned i) const { if(i==0) return "MM"; + else if(i==1) + return "MFX"; else return 0; } @@ -74,6 +109,14 @@ unsigned ArduControl::get_protocol_speed_steps(const string &proto_name) const return protocol_info[map_protocol(proto_name)].max_speed; } +const Driver::DetectedLocomotive *ArduControl::enumerate_detected_locos(unsigned i) const +{ + if(i>=mfx_info.size()) + return 0; + + return &mfx_info[i]; +} + unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &) { if(!addr) @@ -89,10 +132,24 @@ unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const Ve return loco.id; } +ArduControl::MfxInfoArray::iterator ArduControl::add_mfx_info(const MfxInfo &info) +{ + MfxInfoArray::iterator i; + for(i=mfx_info.begin(); (i!=mfx_info.end() && i->id!=info.id); ++i) ; + if(i==mfx_info.end()) + { + mfx_info.push_back(info); + i = --mfx_info.end(); + } + else + *i = info; + return i; +} + void ArduControl::remove_loco(unsigned id) { Locomotive &loco = get_item(locomotives, id); - remove_loco_from_refresh(loco); + refresh.remove_loco(loco); locomotives.erase(id); } @@ -102,12 +159,15 @@ void ArduControl::set_loco_speed(unsigned id, unsigned speed) if(speed>protocol_info[loco.proto].max_speed) throw invalid_argument("ArduControl::set_loco_speed"); + if(speed && halted) + return; + if(loco.speed.set(speed)) { - QueuedCommand cmd(loco, Locomotive::SPEED); - push_command(cmd); + PendingCommand cmd(loco, Locomotive::SPEED); + command_queue.push(cmd); - add_loco_to_refresh(loco); + refresh.add_loco(loco); } } @@ -116,10 +176,10 @@ void ArduControl::set_loco_reverse(unsigned id, bool rev) Locomotive &loco = get_item(locomotives, id); if(loco.reverse.set(rev)) { - QueuedCommand cmd(loco, Locomotive::REVERSE); - push_command(cmd); + PendingCommand cmd(loco, Locomotive::REVERSE); + command_queue.push(cmd); - add_loco_to_refresh(loco); + refresh.add_loco(loco); } } @@ -132,70 +192,13 @@ void ArduControl::set_loco_function(unsigned id, unsigned func, bool state) unsigned mask = 1<0) - { - QueuedCommand cmd(loco, Locomotive::FUNCTIONS, func); - push_command(cmd); - } - - add_loco_to_refresh(loco); - } -} - -void ArduControl::add_loco_to_refresh(Locomotive &loco) -{ - MutexLock lock(mutex); - refresh_cycle.push_back(&loco); - if(refresh_cycle.size()>15) - { - LocomotivePtrList::iterator oldest = refresh_cycle.begin(); - for(LocomotivePtrList::iterator i=refresh_cycle.begin(); ++i!=refresh_cycle.end(); ) - if((*i)->last_change_age>(*oldest)->last_change_age) - oldest = i; - if(oldest==next_refresh) - advance_next_refresh(); - refresh_cycle.erase(oldest); - } - if(next_refresh==refresh_cycle.end()) - next_refresh = refresh_cycle.begin(); -} - -void ArduControl::remove_loco_from_refresh(Locomotive &loco) -{ - MutexLock lock(mutex); - for(LocomotivePtrList::iterator i=refresh_cycle.begin(); i!=refresh_cycle.end(); ++i) - if(*i==&loco) + if(func>0 || loco.proto!=MM) { - if(i==next_refresh) - { - if(refresh_cycle.size()>1) - advance_next_refresh(); - else - next_refresh = refresh_cycle.end(); - } - refresh_cycle.erase(i); - return; + PendingCommand cmd(loco, Locomotive::FUNCTIONS, func); + command_queue.push(cmd); } -} -ArduControl::Locomotive *ArduControl::get_loco_to_refresh() -{ - MutexLock lock(mutex); - if(refresh_cycle.empty()) - return 0; - - Locomotive *loco = *next_refresh; - advance_next_refresh(); - return loco; -} - -void ArduControl::advance_next_refresh() -{ - ++next_refresh; - if(next_refresh==refresh_cycle.end()) - { - next_refresh = refresh_cycle.begin(); - ++refresh_counter; + refresh.add_loco(loco); } } @@ -204,7 +207,7 @@ unsigned ArduControl::add_turnout(unsigned addr, const TrackType &type) if(!addr || !type.is_turnout()) throw invalid_argument("ArduControl::add_turnout"); - return add_accessory(Accessory::TURNOUT, addr, type.get_state_bits()); + return add_accessory(Accessory::TURNOUT, addr, type.get_state_bits(), type.get_paths()); } void ArduControl::remove_turnout(unsigned addr) @@ -224,7 +227,7 @@ unsigned ArduControl::get_turnout(unsigned addr) const unsigned ArduControl::add_signal(unsigned addr, const SignalType &) { - return add_accessory(Accessory::SIGNAL, addr, 1); + return add_accessory(Accessory::SIGNAL, addr, 1, 3); } void ArduControl::remove_signal(unsigned addr) @@ -242,7 +245,7 @@ unsigned ArduControl::get_signal(unsigned addr) const return get_accessory(Accessory::SIGNAL, addr); } -unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits) +unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits, unsigned states) { AccessoryMap::iterator i = accessories.lower_bound(addr); AccessoryMap::iterator j = accessories.upper_bound(addr+bits-1); @@ -255,7 +258,7 @@ unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigne throw key_error(addr); } - insert_unique(accessories, addr, Accessory(kind, addr, bits)); + insert_unique(accessories, addr, Accessory(kind, addr, bits, states)); return addr; } @@ -273,7 +276,7 @@ void ArduControl::set_accessory(Accessory::Kind kind, unsigned addr, unsigned st if(acc.kind!=kind) throw key_error(addr); - if(state!=acc.target) + if(state!=acc.target || acc.uncertain) { acc.target = state; accessory_queue.push_back(&acc); @@ -288,15 +291,27 @@ unsigned ArduControl::get_accessory(Accessory::Kind kind, unsigned addr) const return acc.state; } +void ArduControl::activate_accessory_by_mask(Accessory &acc, unsigned mask) +{ + unsigned bit = mask&~(mask-1); + for(active_index=0; (bit>>active_index)>1; ++active_index) ; + acc.state.set((acc.state&~bit)|(acc.target&bit)); + if(debug>=1) + IO::print("Setting accessory %d bit %d, state=%d\n", acc.address, active_index, acc.state.pending); + PendingCommand cmd(acc, Accessory::ACTIVATE, active_index); + command_queue.push(cmd); + active_accessory = &acc; + + monitor.reset_peak(); +} + unsigned ArduControl::add_sensor(unsigned addr) { if(!addr) throw invalid_argument("ArduControl::add_sensor"); insert_unique(sensors, addr, Sensor(addr)); - unsigned octet_index = (addr-1)/8; - if(octet_index>=n_s88_octets) - n_s88_octets = octet_index+1; + s88.grow_n_octets((addr+7)/8); return addr; } @@ -304,7 +319,7 @@ unsigned ArduControl::add_sensor(unsigned addr) void ArduControl::remove_sensor(unsigned addr) { remove_existing(sensors, addr); - // TODO update n_s88_octets + // TODO update s88.n_octets } bool ArduControl::get_sensor(unsigned addr) const @@ -314,7 +329,8 @@ bool ArduControl::get_sensor(unsigned addr) const void ArduControl::tick() { - while(Tag tag = pop_completed_tag()) + Tag tag; + while(completed_commands.pop(tag)) { if(tag.type==Tag::GENERAL) { @@ -323,6 +339,16 @@ void ArduControl::tick() if(power.commit(tag.serial)) signal_power.emit(power.current); } + else if(tag.command==NEW_LOCO) + { + MfxInfo info; + if(mfx_search.pop_info(info)) + { + MfxInfoArray::iterator i = add_mfx_info(info); + save_state(); + signal_locomotive_detected.emit(*i); + } + } } else if(tag.type==Tag::LOCOMOTIVE) { @@ -361,20 +387,11 @@ void ArduControl::tick() Accessory &acc = i->second; if(tag.command==Accessory::ACTIVATE) - { off_timeout = Time::now()+acc.active_time; - } else if(tag.command==Accessory::DEACTIVATE) { if(acc.state.commit(tag.serial)) { - if(acc.state==acc.target) - { - if(acc.kind==Accessory::TURNOUT) - signal_turnout.emit(acc.address, acc.state); - else if(acc.kind==Accessory::SIGNAL) - signal_signal.emit(acc.address, acc.state); - } if(&acc==active_accessory) active_accessory = 0; } @@ -395,72 +412,104 @@ void ArduControl::tick() } } - while(!active_accessory && !accessory_queue.empty()) + while(power && !active_accessory && !accessory_queue.empty()) { Accessory &acc = *accessory_queue.front(); - if(acc.state!=acc.target) + if(acc.uncertain) + { + unsigned zeroes = acc.uncertain&~acc.target; + if(zeroes) + activate_accessory_by_mask(acc, zeroes); + else + activate_accessory_by_mask(acc, acc.uncertain); + } + else if(acc.state!=acc.target) { - active_accessory = &acc; - unsigned changes = acc.state^acc.target; - unsigned lowest_bit = changes&~(changes-1); - unsigned i; - for(i=0; (lowest_bit>>i)>1; ++i) ; - acc.state.set(acc.state^lowest_bit); - QueuedCommand cmd(acc, Accessory::ACTIVATE, i); - push_command(cmd); + if(!(changes&((1<0.35f && monitor.get_current()off_timeout) + if(t>off_timeout || success) { + Accessory &acc = *active_accessory; + + unsigned bit = 1<=1) + IO::print("Peak current only %.2f A\n", monitor.get_peak()); + signal_turnout_failed.emit(acc.address); + acc.state.rollback(); + if(acc.valid_states&(1<<(acc.target^bit))) + acc.target ^= bit; + else + acc.target = acc.state; + } + off_timeout = Time::TimeStamp(); - QueuedCommand cmd(*active_accessory, Accessory::DEACTIVATE); - push_command(cmd); + PendingCommand cmd(acc, Accessory::DEACTIVATE, active_index); + command_queue.push(cmd); } } } void ArduControl::flush() { + while(!command_queue.empty() || (power && !accessory_queue.empty())) + tick(); } -void ArduControl::push_command(const QueuedCommand &cmd) +void ArduControl::save_state() const { - MutexLock lock(mutex); - command_queue.push_back(cmd); -} + FS::RedirectedPath tmp_file(state_file); + IO::BufferedFile out(tmp_file.str(), IO::M_WRITE); + DataFile::Writer writer(out); -bool ArduControl::pop_command(QueuedCommand &cmd) -{ - MutexLock lock(mutex); - if(command_queue.empty()) - return false; - cmd = command_queue.front(); - command_queue.pop_front(); - return true; -} - -void ArduControl::push_completed_tag(const Tag &tag) -{ - MutexLock lock(mutex); - completed_commands.push_back(tag); -} - -ArduControl::Tag ArduControl::pop_completed_tag() -{ - MutexLock lock(mutex); - if(completed_commands.empty()) - return Tag(); - Tag tag = completed_commands.front(); - completed_commands.pop_front(); - return tag; + writer.write((DataFile::Statement("mfx_announce_serial"), mfx_announce.get_serial())); + for(MfxInfoArray::const_iterator i=mfx_info.begin(); i!=mfx_info.end(); ++i) + { + DataFile::Statement st("mfx_locomotive"); + st.append(i->id); + st.sub.push_back((DataFile::Statement("address"), i->address)); + st.sub.push_back((DataFile::Statement("name"), i->name)); + writer.write(st); + } } @@ -492,6 +541,14 @@ unsigned ArduControl::Locomotive::create_speed_dir_command(char *buffer) const buffer[3] = speed.pending+reverse.pending*0x80; return 4; } + else if(proto==MFX) + { + buffer[0] = MFX_SPEED; + buffer[1] = address>>8; + buffer[2] = address; + buffer[3] = speed.pending+reverse.pending*0x80; + return 4; + } else return 0; } @@ -509,17 +566,39 @@ unsigned ArduControl::Locomotive::create_speed_func_command(unsigned f, char *bu buffer[3] = speed.pending; return 4; } + else if(proto==MFX) + { + bool f16 = (funcs.pending>0xFF); + buffer[0] = (f16 ? MFX_SPEED_FUNCS16 : MFX_SPEED_FUNCS8); + buffer[1] = address>>8; + buffer[2] = address; + buffer[3] = speed.pending+reverse.pending*0x80; + if(f16) + { + buffer[4] = funcs.pending>>8; + buffer[5] = funcs.pending; + return 6; + } + else + { + buffer[4] = funcs.pending; + return 5; + } + } else return 0; } -ArduControl::Accessory::Accessory(Kind k, unsigned a, unsigned b): +ArduControl::Accessory::Accessory(Kind k, unsigned a, unsigned b, unsigned s): kind(k), address(a), bits(b), + valid_states(s), state(0), - active_time(500*Time::msec) + uncertain((1< +void ArduControl::Queue::push(const T &item) +{ + MutexLock lock(mutex); + items.push_back(item); +} + +template +bool ArduControl::Queue::pop(T &item) +{ + MutexLock lock(mutex); + if(items.empty()) + return false; + + item = items.front(); + items.pop_front(); + return true; +} + +template +bool ArduControl::Queue::empty() const +{ + return items.empty(); +} + + +bool ArduControl::CommandQueueTask::get_work(PendingCommand &cmd) +{ + return queue.pop(cmd); +} + +void ArduControl::CommandQueueTask::push(const PendingCommand &cmd) +{ + queue.push(cmd); +} + + +ArduControl::RefreshTask::RefreshTask(): + next(cycle.end()), + round(0), + loco(0), + phase(0) +{ } + +bool ArduControl::RefreshTask::get_work(PendingCommand &cmd) +{ + if(loco && loco->proto==MM && phase==0) + { + cmd.length = loco->create_speed_func_command(round%4+1, cmd.command); + cmd.repeat_count = 2; + ++phase; + return true; + } + + loco = get_next_loco(); + if(!loco) + return false; + + phase = 0; + if(loco->proto==MM) + { + cmd.length = loco->create_speed_dir_command(cmd.command); + cmd.repeat_count = 2; + } + else if(loco->proto==MFX) + cmd.length = loco->create_speed_func_command(0, cmd.command); + else + return false; + + return true; +} + +void ArduControl::RefreshTask::add_loco(Locomotive &l) +{ + MutexLock lock(mutex); + cycle.push_back(&l); + if(cycle.size()>15) + { + LocomotivePtrList::iterator oldest = cycle.begin(); + for(LocomotivePtrList::iterator i=cycle.begin(); ++i!=cycle.end(); ) + if((*i)->last_change_age>(*oldest)->last_change_age) + oldest = i; + if(oldest==next) + advance(); + cycle.erase(oldest); + } + if(next==cycle.end()) + next = cycle.begin(); +} + +void ArduControl::RefreshTask::remove_loco(Locomotive &l) +{ + MutexLock lock(mutex); + for(LocomotivePtrList::iterator i=cycle.begin(); i!=cycle.end(); ++i) + if(*i==&l) + { + if(i==next) + { + if(cycle.size()>1) + advance(); + else + next = cycle.end(); + } + 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): + control(c), + n_octets(0), + octets_remaining(0), + delay(0) +{ } + +bool ArduControl::S88Task::get_work(PendingCommand &cmd) +{ + if(delay) + { + --delay; + return false; + } + if(octets_remaining || !n_octets) + return false; + + octets_remaining = n_octets; + cmd.command[0] = S88_READ; + cmd.command[1] = octets_remaining; + cmd.length = 2; + + delay = 4; + + 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(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(): + serial(0) +{ } + +bool ArduControl::MfxAnnounceTask::get_work(PendingCommand &cmd) +{ + Time::TimeStamp t = Time::now(); + if(t>8; + cmd.command[2] = serial; + cmd.length = 3; + next = t+400*Time::msec; + + return true; +} + +void ArduControl::MfxAnnounceTask::set_serial(unsigned s) +{ + serial = s; +} + + +ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c): + control(c), + next_address(1), + size(0), + bits(0), + misses(0) +{ } + +bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd) +{ + if(size>32) + { + if(control.debug>=1) + IO::print("Assigning MFX address %d to decoder %08X\n", next_address, bits); + + MfxInfo info; + info.protocol = "MFX"; + info.address = next_address; + info.name = format("%08X", bits); + info.id = bits; + queue.push(info); + + cmd.command[0] = MFX_ASSIGN_ADDRESS; + cmd.command[1] = next_address>>8; + cmd.command[2] = next_address; + for(unsigned i=0; i<4; ++i) + cmd.command[3+i] = bits>>(24-i*8); + cmd.length = 7; + + cmd.tag.type = Tag::GENERAL; + cmd.tag.command = NEW_LOCO; + cmd.tag.id = bits; + + size = 0; + bits = 0; + ++next_address; + + return true; + } + + Time::TimeStamp t = Time::now(); + if(t>(24-i*8); + cmd.command[5] = size; + cmd.length = 6; + + next = t+200*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 + { + next = Time::now()+2*Time::sec; + bits = 0; + size = 0; + misses = 0; + } + } +} + +void ArduControl::MfxSearchTask::set_next_address(unsigned a) +{ + next_address = a; +} + +bool ArduControl::MfxSearchTask::pop_info(MfxInfo &info) +{ + return queue.pop(info); +} + + +ArduControl::MonitorTask::MonitorTask(): + voltage(0), + current(0), + base_level(0), + peak_level(0), + next_type(0) +{ } + +bool ArduControl::MonitorTask::get_work(PendingCommand &cmd) +{ + Time::TimeStamp t = Time::now(); + if(t(reply[1])<<8) | static_cast(reply[2]))/1000.0f; + else if(type==TRACK_CURRENT && length==5) + { + current = ((static_cast(reply[1])<<8) | static_cast(reply[2]))/1000.0f; + float peak = ((static_cast(reply[3])<<8) | static_cast(reply[4]))/1000.0f; + peak_level = max(peak_level, peak); + base_level = min(base_level, current); + } +} + +void ArduControl::MonitorTask::reset_peak() +{ + base_level = current; + peak_level = current; +} + + ArduControl::ControlThread::ControlThread(ArduControl &c): control(c), done(false) { + tasks.push_back(&control.command_queue); + tasks.push_back(&control.monitor); + tasks.push_back(&control.mfx_announce); + tasks.push_back(&control.mfx_search); + tasks.push_back(&control.s88); + tasks.push_back(&control.refresh); + launch(); } @@ -558,200 +1064,245 @@ void ArduControl::ControlThread::exit() void ArduControl::ControlThread::main() { - char command[15]; - unsigned length = 0; - unsigned repeat_count = 0; - Tag tag; - Locomotive *loco = 0; - unsigned phase = 0; - unsigned s88_octets_remaining = 0; + init_baud_rate(); + while(!done) { - if(!repeat_count) + PendingCommand cmd; + if(get_work(cmd)) { - tag = Tag(); - length = 0; - repeat_count = 1; - QueuedCommand qcmd; - if(control.pop_command(qcmd)) + bool success = true; + bool resync = false; + for(unsigned i=0; (success && icreate_speed_func_command(control.refresh_counter%4+1, command); - repeat_count = 2; - ++phase; - } - else if(!s88_octets_remaining && control.n_s88_octets) - { - command[0] = S88_READ; - command[1] = control.n_s88_octets; - length = 2; - s88_octets_remaining = control.n_s88_octets; - } - else if((loco = control.get_loco_to_refresh())) + + if(success && cmd.tag) + control.completed_commands.push(cmd.tag); + + if(resync) { - length = loco->create_speed_dir_command(command); - repeat_count = 2; - phase = 0; + if(control.debug>=1) + IO::print("Synchronization with ArduControl lost, attempting to recover\n"); + for(unsigned i=0; (resync && i<16); ++i) + { + control.serial.put('\xFF'); + while(IO::poll(control.serial, IO::P_INPUT, control.command_timeout)) + resync = (control.serial.get()!=0xFF); + } + if(resync) + { + if(control.debug>=1) + IO::print("Resynchronization failed, giving up\n"); + done = true; + } + else + { + if(control.debug>=1) + IO::print("Resynchronization successful\n"); + if(cmd.tag) + control.command_queue.push(cmd); + } } - else + } + else + Time::sleep(10*Time::msec); + } +} + +void ArduControl::ControlThread::init_baud_rate() +{ + static unsigned rates[] = { 57600, 9600, 19200, 38400, 0 }; + unsigned rate = 0; + control.serial.set_data_bits(8); + control.serial.set_parity(IO::Serial::NONE); + control.serial.set_stop_bits(1); + for(unsigned i=0; rates[i]; ++i) + { + control.serial.set_baud_rate(rates[i]); + control.serial.put('\xFF'); + if(IO::poll(control.serial, IO::P_INPUT, 500*Time::msec)) + { + int c = control.serial.get(); + if(c==0xFF) { - // Send an idle packet for the MM protocol - command[0] = MOTOROLA_SPEED; - command[1] = 80; - command[2] = 0; - command[3] = 0; - length = 4; + rate = rates[i]; + break; } } + } - if(control.debug) + if(!rate) + { + if(control.debug>=1) + IO::print("ArduControl detection failed\n"); + done = true; + return; + } + + if(control.debug>=1) + IO::print("ArduControl detected at %d bits/s\n", rate); + + if(rate!=rates[0]) + { + PendingCommand cmd; + cmd.command[0] = SET_BAUD_RATE; + cmd.command[1] = rates[0]>>8; + cmd.command[2] = rates[0]; + cmd.length = 3; + if(do_command(cmd, Time::sec)==COMMAND_OK) { - string cmd_hex; - for(unsigned i=0; i(command[i])); - IO::print("< %02X%s\n", length^0xFF, cmd_hex); + control.serial.set_baud_rate(rates[0]); + Time::sleep(Time::sec); + if(do_command(cmd, Time::sec)==COMMAND_OK) + { + if(control.debug>=1) + IO::print("Rate changed to %d bits/s\n", rates[0]); + } } + } +} - control.serial.put(length^0xFF); - control.serial.write(command, length); - --repeat_count; +bool ArduControl::ControlThread::get_work(PendingCommand &cmd) +{ + for(vector::iterator i=tasks.begin(); i!=tasks.end(); ++i) + if((*i)->get_work(cmd)) + return true; - bool got_reply = false; - bool got_data = false; - while(!got_reply || got_data) - { - if(got_reply) - got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero); - else - got_data = IO::poll(control.serial, IO::P_INPUT); + // As fallback, send an idle packet for the MM protocol + cmd.command[0] = MOTOROLA_SPEED; + cmd.command[1] = 80; + cmd.command[2] = 0; + cmd.command[3] = 0; + cmd.length = 4; - if(got_data) - { - unsigned rlength = control.serial.get()^0xFF; - if(rlength>15) - { - IO::print("Invalid length %02X\n", rlength); - continue; - } + return true; +} + +unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd, const Time::TimeDelta &timeout) +{ + if(control.debug>=2) + { + string cmd_hex; + for(unsigned i=0; i(cmd.command[i])); + IO::print("< %02X%s\n", cmd.length^0xFF, cmd_hex); + } - char reply[15]; - unsigned pos = 0; - while(pos(reply[i])); - IO::print("> %02X%s\n", rlength^0xFF, reply_hex); - } + unsigned result = 0; + while(1) + { + bool got_data; + if(result) + got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero); + else + got_data = IO::poll(control.serial, IO::P_INPUT, timeout); - unsigned char type = reply[0]; - if((type&0xE0)==0x80) - { - got_reply = true; - if(type!=COMMAND_OK) - IO::print("Error %02X\n", type); - else if(tag && !repeat_count) - control.push_completed_tag(tag); - } - else if(type==POWER_STATE && rlength==2) - { - control.power.set(reply[1]); + if(!got_data) + break; - Tag ptag; - ptag.type = Tag::GENERAL; - ptag.command = POWER; - ptag.serial = control.power.serial; - control.push_completed_tag(ptag); - } - else if(type==S88_DATA && rlength>2) - { - unsigned offset = static_cast(reply[1]); - unsigned count = rlength-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 stag; - stag.type = Tag::SENSOR; - stag.command = Sensor::STATE; - stag.serial = i->second.state.serial; - stag.id = i->first; - control.push_completed_tag(stag); - } - - if(count>s88_octets_remaining) - s88_octets_remaining = 0; - else - s88_octets_remaining -= count; - } - } + unsigned rlength = control.serial.get()^0xFF; + if(rlength>15) + { + IO::print("Invalid length %02X\n", rlength); + continue; } - } -} + char reply[15]; + unsigned pos = 0; + while(pos=2) + { + string reply_hex; + for(unsigned i=0; i(reply[i])); + IO::print("> %02X%s\n", rlength^0xFF, reply_hex); + } -ArduControl::QueuedCommand::QueuedCommand(GeneralCommand cmd): - length(0) -{ - tag.type = Tag::GENERAL; - tag.command = cmd; + unsigned r = process_reply(reply, rlength); + if(r && !result) + result = r; + } + + return result; } -ArduControl::QueuedCommand::QueuedCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index) +unsigned ArduControl::ControlThread::process_reply(const char *reply, unsigned rlength) { - tag.type = Tag::LOCOMOTIVE; - tag.command = cmd; - tag.id = loco.id; - if(cmd==Locomotive::SPEED) + unsigned char type = reply[0]; + if((type&0xE0)==0x80) { - tag.serial = loco.speed.serial; - length = loco.create_speed_dir_command(command); + if(type!=COMMAND_OK) + IO::print("Error %02X\n", type); + return type; } - else if(cmd==Locomotive::REVERSE) + else if(type==POWER_STATE && rlength==2) + set_power(reply[1]); + else if(type==OVERCURRENT) { - tag.serial = loco.reverse.serial; - length = loco.create_speed_dir_command(command); + set_power(false); + IO::print("Overcurrent detected!\n"); } - else if(cmd==Locomotive::FUNCTIONS) + else { - tag.serial = loco.funcs.serial; - length = loco.create_speed_func_command(index, command); + for(vector::iterator i=tasks.begin(); i!=tasks.end(); ++i) + (*i)->process_reply(reply, rlength); } - else - throw invalid_argument("QueuedCommand"); + + return 0; } -ArduControl::QueuedCommand::QueuedCommand(Accessory &acc, Accessory::Command cmd, unsigned index) +void ArduControl::ControlThread::set_power(bool p) { - tag.type = Tag::ACCESSORY; - tag.command = cmd; - tag.id = acc.address; - if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE) - { - tag.serial = acc.state.serial; - length = acc.create_state_command(index, (cmd==Accessory::ACTIVATE), command); - } - else - throw invalid_argument("QueuedCommand"); + control.power.set(p); + + Tag tag; + tag.type = Tag::GENERAL; + tag.command = POWER; + tag.serial = control.power.serial; + control.completed_commands.push(tag); +} + + +ArduControl::Loader::Loader(ArduControl &c): + DataFile::ObjectLoader(c) +{ + add("mfx_announce_serial", &Loader::mfx_announce_serial); + add("mfx_locomotive", &Loader::mfx_locomotive); +} + +void ArduControl::Loader::mfx_announce_serial(unsigned s) +{ + obj.mfx_announce.set_serial(s); +} + +void ArduControl::Loader::mfx_locomotive(unsigned id) +{ + MfxInfo info; + info.id = id; + info.protocol = "MFX"; + load_sub(info); + obj.add_mfx_info(info); +} + + +ArduControl::MfxInfo::Loader::Loader(MfxInfo &i): + DataFile::ObjectLoader(i) +{ + add("address", static_cast(&MfxInfo::address)); + add("name", static_cast(&MfxInfo::name)); } } // namespace R2C2