X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Farducontrol.cpp;h=7b5fc108f09bf0864776c67b0a9ce7f717edd4c7;hb=b85fa11e45805cd71383577642030d8d01f43447;hp=921526341b1c320de0928c847a88cbb7ff154343;hpb=70919a9e222568a8ffe610cd5320f3171f298f90;p=r2c2.git diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp index 9215263..7b5fc10 100644 --- a/source/libr2c2/arducontrol.cpp +++ b/source/libr2c2/arducontrol.cpp @@ -1,4 +1,7 @@ #include +#include +#include +#include #include #include #include "arducontrol.h" @@ -15,19 +18,40 @@ ArduControl::ProtocolInfo ArduControl::protocol_info[2] = { 0x3FFF, 126, 15 } // MFX }; -ArduControl::ArduControl(const string &dev): - serial(dev), - debug(1), +Driver::TelemetryInfo ArduControl::telemetry_info[6] = +{ + { "voltage", "Voltage", "V", 1 }, + { "current", "Current", "A", 2 }, + { "cmd-rate", "Cmd rate", "/ s", 0 }, + { "cmd-queue-depth", "Cmd queue", "", 0 }, + { "acc-queue-depth", "Acc queue", "", 0 }, + { "s88-latency", "S88 latency", "ms", 0 } +}; + +ArduControl::ArduControl(const Options &opts): + serial(opts.get(string(), "ttyUSB0")), + debug(opts.get("debug")), + state_file("arducontrol.state"), power(false), + halted(false), active_accessory(0), + command_timeout(200*Time::msec), s88(*this), mfx_search(*this), thread(*this) { + 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'; @@ -35,7 +59,7 @@ ArduControl::ArduControl(const string &dev): cmd.command[3] = 'C'; cmd.command[4] = '2'; cmd.length = 5; - push_command(cmd); + command_queue.push(cmd); } ArduControl::~ArduControl() @@ -51,12 +75,23 @@ void ArduControl::set_power(bool p) 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 @@ -107,6 +142,28 @@ 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; +} + +ArduControl::MfxInfo *ArduControl::find_mfx_info(unsigned id) +{ + for(MfxInfoArray::iterator i=mfx_info.begin(); i!=mfx_info.end(); ++i) + if(i->id==id) + return &*i; + return 0; +} + void ArduControl::remove_loco(unsigned id) { Locomotive &loco = get_item(locomotives, id); @@ -120,10 +177,13 @@ 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)) { PendingCommand cmd(loco, Locomotive::SPEED); - push_command(cmd); + command_queue.push(cmd); refresh.add_loco(loco); } @@ -135,7 +195,7 @@ void ArduControl::set_loco_reverse(unsigned id, bool rev) if(loco.reverse.set(rev)) { PendingCommand cmd(loco, Locomotive::REVERSE); - push_command(cmd); + command_queue.push(cmd); refresh.add_loco(loco); } @@ -153,7 +213,7 @@ void ArduControl::set_loco_function(unsigned id, unsigned func, bool state) if(func>0 || loco.proto!=MM) { PendingCommand cmd(loco, Locomotive::FUNCTIONS, func); - push_command(cmd); + command_queue.push(cmd); } refresh.add_loco(loco); @@ -165,7 +225,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) @@ -185,7 +245,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) @@ -203,7 +263,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); @@ -216,7 +276,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; } @@ -234,7 +294,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); @@ -249,6 +309,20 @@ 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) @@ -271,9 +345,36 @@ bool ArduControl::get_sensor(unsigned addr) const return get_item(sensors, addr).state; } +const Driver::TelemetryInfo *ArduControl::enumerate_telemetry(unsigned i) const +{ + if(i<6) + return telemetry_info+i; + else + return 0; +} + +float ArduControl::get_telemetry_value(const string &name) const +{ + if(name==telemetry_info[0].name) + return monitor.get_voltage(); + else if(name==telemetry_info[1].name) + return monitor.get_current(); + else if(name==telemetry_info[2].name) + return thread.get_command_rate(); + else if(name==telemetry_info[3].name) + return command_queue.size(); + else if(name==telemetry_info[4].name) + return accessory_queue.size(); + else if(name==telemetry_info[5].name) + return s88.get_latency()/Time::msec; + else + throw key_error(name); +} + void ArduControl::tick() { - while(Tag tag = pop_completed_tag()) + Tag tag; + while(completed_commands.pop(tag)) { if(tag.type==Tag::GENERAL) { @@ -287,15 +388,8 @@ void ArduControl::tick() MfxInfo info; if(mfx_search.pop_info(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; + MfxInfoArray::iterator i = add_mfx_info(info); + save_state(); signal_locomotive_detected.emit(*i); } } @@ -337,20 +431,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; } @@ -371,72 +456,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); - PendingCommand 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(); - PendingCommand 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 PendingCommand &cmd) -{ - MutexLock lock(mutex); - command_queue.push_back(cmd); -} - -bool ArduControl::pop_command(PendingCommand &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) +void ArduControl::save_state() const { - MutexLock lock(mutex); - completed_commands.push_back(tag); -} + FS::RedirectedPath tmp_file(state_file); + IO::BufferedFile out(tmp_file.str(), IO::M_WRITE); + DataFile::Writer writer(out); -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); + } } @@ -517,12 +634,15 @@ unsigned ArduControl::Locomotive::create_speed_func_command(unsigned f, char *bu } -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 +unsigned ArduControl::Queue::size() const +{ + return items.size(); +} + +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::Task::Task(const string &n, unsigned p): + name(n), + priority(p) +{ } + +void ArduControl::Task::sleep(const Time::TimeDelta &dt) +{ + sleep_timeout = Time::now()+dt; +} + + +ArduControl::CommandQueueTask::CommandQueueTask(): + Task("CommandQueue") +{ } + + ArduControl::RefreshTask::RefreshTask(): + Task("Refresh", 2), next(cycle.end()), round(0), loco(0), @@ -687,13 +867,14 @@ void ArduControl::RefreshTask::advance() ++next; if(next==cycle.end()) { - next= cycle.begin(); + next = cycle.begin(); ++round; } } ArduControl::S88Task::S88Task(ArduControl &c): + Task("S88"), control(c), n_octets(0), octets_remaining(0) @@ -704,11 +885,18 @@ 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; } @@ -733,7 +921,7 @@ void ArduControl::S88Task::process_reply(const char *reply, unsigned length) tag.command = Sensor::STATE; tag.serial = i->second.state.serial; tag.id = i->first; - control.push_completed_tag(tag); + control.completed_commands.push(tag); } if(count>octets_remaining) @@ -756,20 +944,18 @@ void ArduControl::S88Task::grow_n_octets(unsigned n) ArduControl::MfxAnnounceTask::MfxAnnounceTask(): + Task("MfxAnnounce", 1), 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; + + sleep(400*Time::msec); return true; } @@ -781,56 +967,95 @@ void ArduControl::MfxAnnounceTask::set_serial(unsigned s) ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c): + Task("MfxSearch", 1), control(c), next_address(1), size(0), bits(0), - misses(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", next_address, bits); + IO::print("Assigning MFX address %d to decoder %08X\n", address, bits); - MfxInfo info; - info.protocol = "MFX"; - info.address = next_address; - info.name = format("%08X", bits); - info.id = bits; - push_info(info); + 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] = next_address>>8; - cmd.command[2] = next_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; - cmd.tag.type = Tag::GENERAL; - cmd.tag.command = NEW_LOCO; - cmd.tag.id = bits; - size = 0; bits = 0; - ++next_address; + misses = 0; + + read_array = 0; + read_offset = 0; + read_length = 6; return true; } - Time::TimeStamp t = Time::now(); - if(t>(24-i*8); cmd.command[5] = size; cmd.length = 6; - next = t+200*Time::msec; + sleep(100*Time::msec); if(control.debug>=1) IO::print("Search %08X/%d\n", bits, size); @@ -855,35 +1080,137 @@ void ArduControl::MfxSearchTask::process_reply(const char *reply, unsigned lengt } else { - next = Time::now()+2*Time::sec; + 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(read_data[4])*static_cast(read_data[5]); + + bool array_handled = false; + if(read_data[0]==0x18) + { + for(unsigned i=1; iname = string(read_data+1, i-1); + array_handled = true; + break; + } + + if(!array_handled) + read_length = 4; + } + else + array_handled = true; + + if(array_handled && control.debug>=1) + { + IO::print("MFX CA %03X:", read_array); + for(unsigned i=0; i(read_data[i])); + IO::print("\n"); + } + + if(array_handled && read_array=10) + { + if(control.debug>=1) + IO::print("Failed to read MFX configuration from %d\n", pending_info->address); + read_length = 0; + } + } + } } -void ArduControl::MfxSearchTask::push_info(const MfxInfo &info) +void ArduControl::MfxSearchTask::set_next_address(unsigned a) { - MutexLock lock(mutex); - queue.push_back(info); + next_address = a; } bool ArduControl::MfxSearchTask::pop_info(MfxInfo &info) { - MutexLock lock(mutex); - if(queue.empty()) - return false; - info = queue.back(); - queue.pop_back(); + return queue.pop(info); +} + + +ArduControl::MonitorTask::MonitorTask(): + Task("Monitor"), + voltage(0), + current(0), + base_level(0), + peak_level(0), + next_type(0) +{ } + +bool ArduControl::MonitorTask::get_work(PendingCommand &cmd) +{ + if(next_type==0) + cmd.command[0] = READ_INPUT_VOLTAGE; + else + cmd.command[0] = READ_TRACK_CURRENT; + cmd.length = 1; + + sleep(200*Time::msec); + next_type = (next_type+1)%5; + return true; } +void ArduControl::MonitorTask::process_reply(const char *reply, unsigned length) +{ + unsigned char type = reply[0]; + if(type==INPUT_VOLTAGE && length==3) + voltage = ((static_cast(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) + done(false), + cmd_rate(20), + cmd_count(0) { + 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); @@ -901,6 +1228,7 @@ void ArduControl::ControlThread::exit() void ArduControl::ControlThread::main() { init_baud_rate(); + cmd_rate_start = Time::now(); while(!done) { @@ -908,10 +1236,49 @@ void ArduControl::ControlThread::main() if(get_work(cmd)) { bool success = true; + bool resync = false; for(unsigned i=0; (success && i=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); + } + } + + if(cmd_count>=cmd_rate) + { + Time::TimeStamp t = Time::now(); + cmd_rate = cmd_count/((t-cmd_rate_start)/Time::sec); + cmd_rate_start = t; + cmd_count = 0; + } } else Time::sleep(10*Time::msec); @@ -942,6 +1309,8 @@ void ArduControl::ControlThread::init_baud_rate() if(!rate) { + if(control.debug>=1) + IO::print("ArduControl detection failed\n"); done = true; return; } @@ -956,11 +1325,11 @@ void ArduControl::ControlThread::init_baud_rate() cmd.command[1] = rates[0]>>8; cmd.command[2] = rates[0]; cmd.length = 3; - if(do_command(cmd)==COMMAND_OK) + if(do_command(cmd, Time::sec)==COMMAND_OK) { control.serial.set_baud_rate(rates[0]); Time::sleep(Time::sec); - if(do_command(cmd)==COMMAND_OK) + if(do_command(cmd, Time::sec)==COMMAND_OK) { if(control.debug>=1) IO::print("Rate changed to %d bits/s\n", rates[0]); @@ -971,12 +1340,33 @@ void ArduControl::ControlThread::init_baud_rate() bool ArduControl::ControlThread::get_work(PendingCommand &cmd) { - if(control.pop_command(cmd)) - return true; + Time::TimeStamp t = Time::now(); + + unsigned count = 0; + for(; (countget_sleep_timeout()<=t); ++count) ; + + for(; count>0; --count) + { + unsigned i = 0; + for(unsigned j=1; jget_priority()get_priority()) + i = j; + + Task *task = tasks[i]; + bool result = task->get_work(cmd); + + Time::TimeStamp st = max(task->get_sleep_timeout(), t); + for(; (i+1get_sleep_timeout()<=st); ++i) + tasks[i] = tasks[i+1]; + tasks[i] = task; - for(vector::iterator i=tasks.begin(); i!=tasks.end(); ++i) - if((*i)->get_work(cmd)) + if(result) + { + if(control.debug>=2) + IO::print("Scheduled task %s\n", task->get_name()); return true; + } + } // As fallback, send an idle packet for the MM protocol cmd.command[0] = MOTOROLA_SPEED; @@ -988,7 +1378,7 @@ bool ArduControl::ControlThread::get_work(PendingCommand &cmd) return true; } -unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd) +unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd, const Time::TimeDelta &timeout) { if(control.debug>=2) { @@ -1008,7 +1398,7 @@ unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd) if(result) got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero); else - got_data = IO::poll(control.serial, IO::P_INPUT); + got_data = IO::poll(control.serial, IO::P_INPUT, timeout); if(!got_data) break; @@ -1023,7 +1413,11 @@ unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd) char reply[15]; unsigned pos = 0; while(pos=2) { @@ -1038,6 +1432,8 @@ unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd) result = r; } + ++cmd_count; + return result; } @@ -1051,14 +1447,11 @@ unsigned ArduControl::ControlThread::process_reply(const char *reply, unsigned r return type; } else if(type==POWER_STATE && rlength==2) + set_power(reply[1]); + else if(type==OVERCURRENT) { - control.power.set(reply[1]); - - Tag tag; - tag.type = Tag::GENERAL; - tag.command = POWER; - tag.serial = control.power.serial; - control.push_completed_tag(tag); + set_power(false); + IO::print("Overcurrent detected!\n"); } else { @@ -1069,4 +1462,45 @@ unsigned ArduControl::ControlThread::process_reply(const char *reply, unsigned r return 0; } +void ArduControl::ControlThread::set_power(bool p) +{ + 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