X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Farducontrol.cpp;h=47584f44bb8b145496a0b6d9a52c118f4c68fcd9;hb=58c56db5f740d9d3c56ff64f4062b108076c63c3;hp=d18dc97590ee0a4af0a7ed1ab71e0aa59321087a;hpb=4980a05569d29e19cb7af0d31627e17a94353a3b;p=r2c2.git diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp index d18dc97..47584f4 100644 --- a/source/libr2c2/arducontrol.cpp +++ b/source/libr2c2/arducontrol.cpp @@ -19,12 +19,9 @@ ArduControl::ArduControl(const string &dev): serial(dev), debug(1), power(false), - next_refresh(refresh_cycle.end()), - refresh_counter(0), active_accessory(0), - n_s88_octets(0), - mfx_announce_serial(0), - next_mfx_address(1), + s88(*this), + mfx_search(*this), thread(*this) { PendingCommand cmd; @@ -87,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) @@ -105,7 +110,7 @@ unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const Ve void ArduControl::remove_loco(unsigned id) { Locomotive &loco = get_item(locomotives, id); - remove_loco_from_refresh(loco); + refresh.remove_loco(loco); locomotives.erase(id); } @@ -120,7 +125,7 @@ void ArduControl::set_loco_speed(unsigned id, unsigned speed) PendingCommand cmd(loco, Locomotive::SPEED); push_command(cmd); - add_loco_to_refresh(loco); + refresh.add_loco(loco); } } @@ -132,7 +137,7 @@ void ArduControl::set_loco_reverse(unsigned id, bool rev) PendingCommand cmd(loco, Locomotive::REVERSE); push_command(cmd); - add_loco_to_refresh(loco); + refresh.add_loco(loco); } } @@ -151,64 +156,7 @@ void ArduControl::set_loco_function(unsigned id, unsigned func, bool state) 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(i==next_refresh) - { - if(refresh_cycle.size()>1) - advance_next_refresh(); - else - next_refresh = refresh_cycle.end(); - } - refresh_cycle.erase(i); - return; - } -} - -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); } } @@ -307,9 +255,7 @@ unsigned ArduControl::add_sensor(unsigned 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; } @@ -317,7 +263,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 @@ -336,6 +282,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) { @@ -637,8 +600,9 @@ ArduControl::PendingCommand::PendingCommand(Accessory &acc, Accessory::Command c } -ArduControl::RefreshTask::RefreshTask(ArduControl &c): - control(c), +ArduControl::RefreshTask::RefreshTask(): + next(cycle.end()), + round(0), loco(0), phase(0) { } @@ -647,13 +611,13 @@ bool ArduControl::RefreshTask::get_work(PendingCommand &cmd) { if(loco && loco->proto==MM && phase==0) { - cmd.length = loco->create_speed_func_command(control.refresh_counter%4+1, cmd.command); + cmd.length = loco->create_speed_func_command(round%4+1, cmd.command); cmd.repeat_count = 2; ++phase; return true; } - loco = control.get_loco_to_refresh(); + loco = get_next_loco(); if(!loco) return false; @@ -671,21 +635,79 @@ bool ArduControl::RefreshTask::get_work(PendingCommand &cmd) 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) { } bool ArduControl::S88Task::get_work(PendingCommand &cmd) { - if(octets_remaining || !control.n_s88_octets) + if(octets_remaining || !n_octets) return false; + octets_remaining = n_octets; cmd.command[0] = S88_READ; - cmd.command[1] = control.n_s88_octets; + cmd.command[1] = octets_remaining; cmd.length = 2; - octets_remaining = control.n_s88_octets; return true; } @@ -721,9 +743,20 @@ void ArduControl::S88Task::process_reply(const char *reply, unsigned length) } } +void ArduControl::S88Task::set_n_octets(unsigned n) +{ + n_octets = n; +} -ArduControl::MfxAnnounceTask::MfxAnnounceTask(ArduControl &c): - control(c) +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) @@ -733,39 +766,56 @@ bool ArduControl::MfxAnnounceTask::get_work(PendingCommand &cmd) return false; cmd.command[0] = MFX_ANNOUNCE; - cmd.command[1] = control.mfx_announce_serial>>8; - cmd.command[2] = control.mfx_announce_serial; + cmd.command[1] = serial>>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), - 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", control.next_mfx_address, bits); + 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; + push_info(info); cmd.command[0] = MFX_ASSIGN_ADDRESS; - cmd.command[1] = control.next_mfx_address>>8; - cmd.command[2] = control.next_mfx_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; - ++control.next_mfx_address; + ++next_address; return true; } @@ -781,7 +831,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); @@ -792,44 +841,53 @@ 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; } } } +void ArduControl::MfxSearchTask::push_info(const MfxInfo &info) +{ + MutexLock lock(mutex); + queue.push_back(info); +} + +bool ArduControl::MfxSearchTask::pop_info(MfxInfo &info) +{ + MutexLock lock(mutex); + if(queue.empty()) + return false; + info = queue.back(); + queue.pop_back(); + return true; +} + ArduControl::ControlThread::ControlThread(ArduControl &c): control(c), done(false) { - tasks.push_back(new MfxAnnounceTask(control)); - tasks.push_back(new MfxSearchTask(control)); - tasks.push_back(new S88Task(control)); - tasks.push_back(new RefreshTask(control)); + tasks.push_back(&control.mfx_announce); + tasks.push_back(&control.mfx_search); + tasks.push_back(&control.s88); + tasks.push_back(&control.refresh); launch(); } @@ -842,6 +900,8 @@ void ArduControl::ControlThread::exit() void ArduControl::ControlThread::main() { + init_baud_rate(); + while(!done) { PendingCommand cmd; @@ -858,6 +918,51 @@ void ArduControl::ControlThread::main() } } +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) + { + rate = rates[i]; + break; + } + } + } + + 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)==COMMAND_OK) + { + control.serial.set_baud_rate(rates[0]); + if(do_command(cmd)==COMMAND_OK) + { + Time::sleep(Time::sec); + 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))