X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Farducontrol.cpp;h=e29614f6646a5c5fd3068d819c7e89c071a0c154;hb=59b3cebecfd8f1462c95bab447be70dfb5f38e92;hp=201d9a2ebdd73e7871527c2f09c87b3a7f29cba2;hpb=8935c058a29ce263f4539832049aa74be3762438;p=r2c2.git diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp index 201d9a2..e29614f 100644 --- a/source/libr2c2/arducontrol.cpp +++ b/source/libr2c2/arducontrol.cpp @@ -27,7 +27,7 @@ ArduControl::ArduControl(const string &dev): 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 +35,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,7 +51,7 @@ 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); } } @@ -84,6 +84,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) @@ -115,7 +123,7 @@ void ArduControl::set_loco_speed(unsigned id, unsigned speed) if(loco.speed.set(speed)) { PendingCommand cmd(loco, Locomotive::SPEED); - push_command(cmd); + command_queue.push(cmd); refresh.add_loco(loco); } @@ -127,7 +135,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); } @@ -145,7 +153,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); @@ -265,7 +273,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) { @@ -274,6 +283,23 @@ 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; + 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; + signal_locomotive_detected.emit(*i); + } + } } else if(tag.type==Tag::LOCOMOTIVE) { @@ -360,7 +386,7 @@ void ArduControl::tick() for(i=0; (lowest_bit>>i)>1; ++i) ; acc.state.set(acc.state^lowest_bit); PendingCommand cmd(acc, Accessory::ACTIVATE, i); - push_command(cmd); + command_queue.push(cmd); } else accessory_queue.pop_front(); @@ -373,7 +399,7 @@ void ArduControl::tick() { off_timeout = Time::TimeStamp(); PendingCommand cmd(*active_accessory, Accessory::DEACTIVATE); - push_command(cmd); + command_queue.push(cmd); } } } @@ -382,38 +408,6 @@ void ArduControl::flush() { } -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) -{ - 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; -} - ArduControl::Tag::Tag(): type(NONE), @@ -575,6 +569,26 @@ ArduControl::PendingCommand::PendingCommand(Accessory &acc, Accessory::Command c } +template +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; +} + + ArduControl::RefreshTask::RefreshTask(): next(cycle.end()), round(0), @@ -708,7 +722,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) @@ -760,16 +774,23 @@ ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c): next_address(1), size(0), bits(0), - pending(false) + misses(0) { } bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd) { - if(size==32) + 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; @@ -777,6 +798,10 @@ bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd) 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; @@ -795,7 +820,6 @@ bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd) cmd.length = 6; next = t+200*Time::msec; - pending = true; if(control.debug>=1) IO::print("Search %08X/%d\n", bits, size); @@ -806,35 +830,33 @@ bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd) void ArduControl::MfxSearchTask::process_reply(const char *reply, unsigned length) { unsigned char type = reply[0]; - if(type==MFX_FEEDBACK && length==2 && pending) + if(type==MFX_SEARCH_FEEDBACK && length==2) { - pending = false; - bool finished = true; if(reply[1]) { - if(size<32) - ++size; - finished = false; + misses = 0; + ++size; } - else if(size>0) + else if(size>0 && misses<6) { - unsigned mask = 1<<(32-size); - if(!(bits&mask)) - { - bits |= mask; - finished = false; - } + ++misses; + bits ^= 1<<(32-size); } - - if(finished) + else { next = Time::now()+2*Time::sec; bits = 0; size = 0; + misses = 0; } } } +bool ArduControl::MfxSearchTask::pop_info(MfxInfo &info) +{ + return queue.pop(info); +} + ArduControl::ControlThread::ControlThread(ArduControl &c): control(c), @@ -856,6 +878,8 @@ void ArduControl::ControlThread::exit() void ArduControl::ControlThread::main() { + init_baud_rate(); + while(!done) { PendingCommand cmd; @@ -865,16 +889,67 @@ void ArduControl::ControlThread::main() for(unsigned i=0; (success && i=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)==COMMAND_OK) + { + control.serial.set_baud_rate(rates[0]); + Time::sleep(Time::sec); + if(do_command(cmd)==COMMAND_OK) + { + if(control.debug>=1) + IO::print("Rate changed to %d bits/s\n", rates[0]); + } + } + } +} + bool ArduControl::ControlThread::get_work(PendingCommand &cmd) { - if(control.pop_command(cmd)) + if(control.command_queue.pop(cmd)) return true; for(vector::iterator i=tasks.begin(); i!=tasks.end(); ++i) @@ -961,7 +1036,7 @@ unsigned ArduControl::ControlThread::process_reply(const char *reply, unsigned r tag.type = Tag::GENERAL; tag.command = POWER; tag.serial = control.power.serial; - control.push_completed_tag(tag); + control.completed_commands.push(tag); } else {