From: Mikko Rasa Date: Fri, 8 Nov 2013 13:06:30 +0000 (+0200) Subject: Restructure ArduControl driver into more manageable units X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=4980a05569d29e19cb7af0d31627e17a94353a3b;p=r2c2.git Restructure ArduControl driver into more manageable units The control thread main function was getting a bit unwieldy at over 200 lines. --- diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp index 7111f53..d18dc97 100644 --- a/source/libr2c2/arducontrol.cpp +++ b/source/libr2c2/arducontrol.cpp @@ -27,7 +27,7 @@ ArduControl::ArduControl(const string &dev): next_mfx_address(1), thread(*this) { - QueuedCommand cmd; + PendingCommand cmd; cmd.command[0] = READ_POWER_STATE; cmd.length = 1; push_command(cmd); @@ -50,7 +50,7 @@ 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; @@ -117,7 +117,7 @@ void ArduControl::set_loco_speed(unsigned id, unsigned speed) if(loco.speed.set(speed)) { - QueuedCommand cmd(loco, Locomotive::SPEED); + PendingCommand cmd(loco, Locomotive::SPEED); push_command(cmd); add_loco_to_refresh(loco); @@ -129,7 +129,7 @@ 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); + PendingCommand cmd(loco, Locomotive::REVERSE); push_command(cmd); add_loco_to_refresh(loco); @@ -147,7 +147,7 @@ void ArduControl::set_loco_function(unsigned id, unsigned func, bool state) { if(func>0 || loco.proto!=MM) { - QueuedCommand cmd(loco, Locomotive::FUNCTIONS, func); + PendingCommand cmd(loco, Locomotive::FUNCTIONS, func); push_command(cmd); } @@ -421,7 +421,7 @@ void ArduControl::tick() unsigned i; for(i=0; (lowest_bit>>i)>1; ++i) ; acc.state.set(acc.state^lowest_bit); - QueuedCommand cmd(acc, Accessory::ACTIVATE, i); + PendingCommand cmd(acc, Accessory::ACTIVATE, i); push_command(cmd); } else @@ -434,7 +434,7 @@ void ArduControl::tick() if(t>off_timeout) { off_timeout = Time::TimeStamp(); - QueuedCommand cmd(*active_accessory, Accessory::DEACTIVATE); + PendingCommand cmd(*active_accessory, Accessory::DEACTIVATE); push_command(cmd); } } @@ -444,13 +444,13 @@ void ArduControl::flush() { } -void ArduControl::push_command(const QueuedCommand &cmd) +void ArduControl::push_command(const PendingCommand &cmd) { MutexLock lock(mutex); command_queue.push_back(cmd); } -bool ArduControl::pop_command(QueuedCommand &cmd) +bool ArduControl::pop_command(PendingCommand &cmd) { MutexLock lock(mutex); if(command_queue.empty()) @@ -583,296 +583,379 @@ ArduControl::Sensor::Sensor(unsigned a): { } -ArduControl::ControlThread::ControlThread(ArduControl &c): - control(c), - done(false) +ArduControl::PendingCommand::PendingCommand(): + length(0), + repeat_count(1) +{ } + +ArduControl::PendingCommand::PendingCommand(GeneralCommand cmd): + length(0), + repeat_count(1) { - launch(); + tag.type = Tag::GENERAL; + tag.command = cmd; } -void ArduControl::ControlThread::exit() +ArduControl::PendingCommand::PendingCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index): + repeat_count(8) { - done = true; - join(); + tag.type = Tag::LOCOMOTIVE; + tag.command = cmd; + tag.id = loco.id; + if(cmd==Locomotive::SPEED) + { + tag.serial = loco.speed.serial; + length = loco.create_speed_dir_command(command); + } + else if(cmd==Locomotive::REVERSE) + { + tag.serial = loco.reverse.serial; + length = loco.create_speed_dir_command(command); + } + else if(cmd==Locomotive::FUNCTIONS) + { + tag.serial = loco.funcs.serial; + length = loco.create_speed_func_command(index, command); + } + else + throw invalid_argument("PendingCommand"); } -void ArduControl::ControlThread::main() +ArduControl::PendingCommand::PendingCommand(Accessory &acc, Accessory::Command cmd, unsigned index): + repeat_count(1) { - char command[15]; - unsigned length = 0; - unsigned repeat_count = 0; - Tag tag; - Locomotive *loco = 0; - unsigned phase = 0; - unsigned s88_octets_remaining = 0; + 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("PendingCommand"); +} - Msp::Time::TimeStamp next_mfx_announce; - unsigned mfx_search_size = 0; - unsigned mfx_search_bits = 0; - Msp::Time::TimeStamp next_mfx_search; - while(!done) +ArduControl::RefreshTask::RefreshTask(ArduControl &c): + control(c), + loco(0), + phase(0) +{ } + +bool ArduControl::RefreshTask::get_work(PendingCommand &cmd) +{ + if(loco && loco->proto==MM && phase==0) { - Time::TimeStamp t = Time::now(); + cmd.length = loco->create_speed_func_command(control.refresh_counter%4+1, cmd.command); + cmd.repeat_count = 2; + ++phase; + return true; + } - if(!repeat_count) - { - tag = Tag(); - length = 0; - repeat_count = 1; - QueuedCommand qcmd; - if(control.pop_command(qcmd)) - { - length = qcmd.length; - copy(qcmd.command, qcmd.command+length, command); - if(qcmd.tag.type==Tag::LOCOMOTIVE) - repeat_count = 8; - tag = qcmd.tag; - } - else if(loco && phase==0) - { - length = loco->create_speed_func_command(control.refresh_counter%4+1, command); - repeat_count = 2; - ++phase; - } - else if(t>=next_mfx_announce) - { - command[0] = MFX_ANNOUNCE; - command[1] = control.mfx_announce_serial>>8; - command[2] = control.mfx_announce_serial; - length = 3; - next_mfx_announce = t+400*Time::msec; - } - else if(t>=next_mfx_search) - { - command[0] = MFX_SEARCH; - for(unsigned i=0; i<4; ++i) - command[1+i] = mfx_search_bits>>(24-i*8); - command[5] = mfx_search_size; - length = 6; - next_mfx_search = t+200*Time::msec; - if(control.debug>=1) - IO::print("Search %08X/%d\n", mfx_search_bits, mfx_search_size); - } - 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(loco->proto==MM) - { - length = loco->create_speed_dir_command(command); - repeat_count = 2; - phase = 0; - } - else if(loco->proto==MFX) - { - length = loco->create_speed_func_command(0, command); - phase = 1; - } - } - else - { - // Send an idle packet for the MM protocol - command[0] = MOTOROLA_SPEED; - command[1] = 80; - command[2] = 0; - command[3] = 0; - length = 4; - } - } + loco = control.get_loco_to_refresh(); + if(!loco) + return false; - if(control.debug>=2) + 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; +} + + +ArduControl::S88Task::S88Task(ArduControl &c): + control(c), + octets_remaining(0) +{ } + +bool ArduControl::S88Task::get_work(PendingCommand &cmd) +{ + if(octets_remaining || !control.n_s88_octets) + return false; + + cmd.command[0] = S88_READ; + cmd.command[1] = control.n_s88_octets; + cmd.length = 2; + octets_remaining = control.n_s88_octets; + + 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) { - string cmd_hex; - for(unsigned i=0; i(command[i])); - IO::print("< %02X%s\n", length^0xFF, cmd_hex); + 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.push_completed_tag(tag); } - control.serial.put(length^0xFF); - control.serial.write(command, length); - --repeat_count; + if(count>octets_remaining) + octets_remaining = 0; + else + octets_remaining -= count; + } +} - 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); - if(got_data) - { - unsigned rlength = control.serial.get()^0xFF; - if(rlength>15) - { - IO::print("Invalid length %02X\n", rlength); - continue; - } +ArduControl::MfxAnnounceTask::MfxAnnounceTask(ArduControl &c): + control(c) +{ } - 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); - } + cmd.command[0] = MFX_ANNOUNCE; + cmd.command[1] = control.mfx_announce_serial>>8; + cmd.command[2] = control.mfx_announce_serial; + cmd.length = 3; + next = t+400*Time::msec; - 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]); + return true; +} - Tag ptag; - ptag.type = Tag::GENERAL; - ptag.command = POWER; - ptag.serial = control.power.serial; - control.push_completed_tag(ptag); - } - else if(type==MFX_FEEDBACK && rlength==2) - { - if(command[0]==MFX_SEARCH) - { - bool finished = true; - if(reply[1]) - { - if(mfx_search_size==32) - { - if(control.debug>=1) - IO::print("Assigning MFX address %d to decoder %08X\n", control.next_mfx_address, mfx_search_bits); - - QueuedCommand qcmd; - qcmd.command[0] = MFX_ASSIGN_ADDRESS; - qcmd.command[1] = control.next_mfx_address>>8; - qcmd.command[2] = control.next_mfx_address; - for(unsigned i=0; i<4; ++i) - qcmd.command[3+i] = mfx_search_bits>>(24-i*8); - qcmd.length = 7; - control.push_command(qcmd); - ++control.next_mfx_address; - } - else - { - ++mfx_search_size; - finished = false; - } - } - else if(mfx_search_size>0) - { - unsigned mask = 1<<(32-mfx_search_size); - if(!(mfx_search_bits&mask)) - { - mfx_search_bits |= mask; - finished = false; - } - } - - if(finished) - { - next_mfx_search = t+2*Time::sec; - mfx_search_bits = 0; - mfx_search_size = 0; - } - } - } - 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); - } +ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c): + control(c), + size(0), + bits(0), + pending(false) +{ } - if(count>s88_octets_remaining) - s88_octets_remaining = 0; - else - s88_octets_remaining -= count; - } +bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd) +{ + if(size==32) + { + if(control.debug>=1) + IO::print("Assigning MFX address %d to decoder %08X\n", control.next_mfx_address, bits); + + cmd.command[0] = MFX_ASSIGN_ADDRESS; + cmd.command[1] = control.next_mfx_address>>8; + cmd.command[2] = control.next_mfx_address; + for(unsigned i=0; i<4; ++i) + cmd.command[3+i] = bits>>(24-i*8); + cmd.length = 7; + + size = 0; + bits = 0; + ++control.next_mfx_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; + pending = true; + + 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_FEEDBACK && length==2 && pending) + { + pending = false; + bool finished = true; + if(reply[1]) + { + if(size<32) + ++size; + finished = false; + } + else if(size>0) + { + unsigned mask = 1<<(32-size); + if(!(bits&mask)) + { + bits |= mask; + finished = false; } } + + if(finished) + { + next = Time::now()+2*Time::sec; + bits = 0; + size = 0; + } } } -ArduControl::QueuedCommand::QueuedCommand(): - length(0) -{ } +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)); -ArduControl::QueuedCommand::QueuedCommand(GeneralCommand cmd): - length(0) + launch(); +} + +void ArduControl::ControlThread::exit() { - tag.type = Tag::GENERAL; - tag.command = cmd; + done = true; + join(); } -ArduControl::QueuedCommand::QueuedCommand(Locomotive &loco, Locomotive::Command cmd, unsigned index) +void ArduControl::ControlThread::main() { - tag.type = Tag::LOCOMOTIVE; - tag.command = cmd; - tag.id = loco.id; - if(cmd==Locomotive::SPEED) + while(!done) { - tag.serial = loco.speed.serial; - length = loco.create_speed_dir_command(command); + PendingCommand cmd; + if(get_work(cmd)) + { + bool success = true; + for(unsigned i=0; (success && i::iterator i=tasks.begin(); i!=tasks.end(); ++i) + if((*i)->get_work(cmd)) + return true; + + // 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; + + return true; +} + +unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd) +{ + if(control.debug>=2) { - tag.serial = loco.reverse.serial; - length = loco.create_speed_dir_command(command); + string cmd_hex; + for(unsigned i=0; i(cmd.command[i])); + IO::print("< %02X%s\n", cmd.length^0xFF, cmd_hex); } - else if(cmd==Locomotive::FUNCTIONS) + + control.serial.put(cmd.length^0xFF); + control.serial.write(cmd.command, cmd.length); + + unsigned result = 0; + while(1) { - tag.serial = loco.funcs.serial; - length = loco.create_speed_func_command(index, command); + 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); + + if(!got_data) + break; + + 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); + } + + unsigned r = process_reply(reply, rlength); + if(r && !result) + result = r; } - else - throw invalid_argument("QueuedCommand"); + + return result; } -ArduControl::QueuedCommand::QueuedCommand(Accessory &acc, Accessory::Command cmd, unsigned index) +unsigned ArduControl::ControlThread::process_reply(const char *reply, unsigned rlength) { - tag.type = Tag::ACCESSORY; - tag.command = cmd; - tag.id = acc.address; - if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE) + unsigned char type = reply[0]; + if((type&0xE0)==0x80) { - tag.serial = acc.state.serial; - length = acc.create_state_command(index, (cmd==Accessory::ACTIVATE), command); + if(type!=COMMAND_OK) + IO::print("Error %02X\n", type); + return type; + } + else if(type==POWER_STATE && rlength==2) + { + control.power.set(reply[1]); + + Tag tag; + tag.type = Tag::GENERAL; + tag.command = POWER; + tag.serial = control.power.serial; + control.push_completed_tag(tag); } else - throw invalid_argument("QueuedCommand"); + { + for(vector::iterator i=tasks.begin(); i!=tasks.end(); ++i) + (*i)->process_reply(reply, rlength); + } + + return 0; } } // namespace R2C2 diff --git a/source/libr2c2/arducontrol.h b/source/libr2c2/arducontrol.h index 3a86cee..d794d00 100644 --- a/source/libr2c2/arducontrol.h +++ b/source/libr2c2/arducontrol.h @@ -166,11 +166,90 @@ private: Sensor(unsigned); }; + struct PendingCommand + { + Tag tag; + char command[15]; + unsigned char length; + unsigned repeat_count; + + PendingCommand(); + PendingCommand(GeneralCommand); + PendingCommand(Locomotive &, Locomotive::Command, unsigned = 0); + PendingCommand(Accessory &, Accessory::Command, unsigned = 0); + }; + + class Task + { + protected: + Task() { } + public: + virtual ~Task() { } + + virtual bool get_work(PendingCommand &) = 0; + virtual void process_reply(const char *, unsigned) { } + }; + + class RefreshTask: public Task + { + private: + ArduControl &control; + Locomotive *loco; + unsigned phase; + + public: + RefreshTask(ArduControl &); + + virtual bool get_work(PendingCommand &); + }; + + class S88Task: public Task + { + private: + ArduControl &control; + unsigned octets_remaining; + + public: + S88Task(ArduControl &); + + virtual bool get_work(PendingCommand &); + virtual void process_reply(const char *, unsigned); + }; + + class MfxAnnounceTask: public Task + { + private: + ArduControl &control; + Msp::Time::TimeStamp next; + + public: + MfxAnnounceTask(ArduControl &); + + virtual bool get_work(PendingCommand &); + }; + + class MfxSearchTask: public Task + { + private: + ArduControl &control; + Msp::Time::TimeStamp next; + unsigned size; + unsigned bits; + bool pending; + + public: + MfxSearchTask(ArduControl &); + + virtual bool get_work(PendingCommand &); + virtual void process_reply(const char *, unsigned); + }; + class ControlThread: public Msp::Thread { private: ArduControl &control; bool done; + std::vector tasks; public: ControlThread(ArduControl &); @@ -178,18 +257,9 @@ private: void exit(); private: virtual void main(); - }; - - struct QueuedCommand - { - Tag tag; - char command[15]; - unsigned char length; - - QueuedCommand(); - QueuedCommand(GeneralCommand); - QueuedCommand(Locomotive &, Locomotive::Command, unsigned = 0); - QueuedCommand(Accessory &, Accessory::Command, unsigned = 0); + bool get_work(PendingCommand &); + unsigned do_command(const PendingCommand &); + unsigned process_reply(const char *, unsigned); }; typedef std::map LocomotiveMap; @@ -211,7 +281,7 @@ private: AccessoryPtrList accessory_queue; Accessory *active_accessory; Msp::Time::TimeStamp off_timeout; - std::list command_queue; + std::list command_queue; std::list completed_commands; SensorMap sensors; @@ -278,8 +348,8 @@ public: virtual void flush(); private: - void push_command(const QueuedCommand &); - bool pop_command(QueuedCommand &); + void push_command(const PendingCommand &); + bool pop_command(PendingCommand &); void push_completed_tag(const Tag &); Tag pop_completed_tag(); };