From 4b287b53e9127209864292f583b8ddbdd481d022 Mon Sep 17 00:00:00 2001 From: Mikko Rasa Date: Fri, 8 Nov 2013 01:55:29 +0200 Subject: [PATCH] Add basic MFX support to arducontrol driver A lot of polishing is still needed. Locomotive discovery in particular has several deficiencies. --- source/libr2c2/arducontrol.cpp | 137 +++++++++++++++++++++++++++++++-- source/libr2c2/arducontrol.h | 19 ++++- 2 files changed, 145 insertions(+), 11 deletions(-) diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp index e5d94ef..7111f53 100644 --- a/source/libr2c2/arducontrol.cpp +++ b/source/libr2c2/arducontrol.cpp @@ -11,23 +11,34 @@ 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), + 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), thread(*this) { QueuedCommand cmd; cmd.command[0] = READ_POWER_STATE; cmd.length = 1; push_command(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; + push_command(cmd); } ArduControl::~ArduControl() @@ -55,6 +66,8 @@ const char *ArduControl::enumerate_protocols(unsigned i) const { if(i==0) return "MM"; + else if(i==1) + return "MFX"; else return 0; } @@ -132,7 +145,7 @@ void ArduControl::set_loco_function(unsigned id, unsigned func, bool state) unsigned mask = 1<0) + if(func>0 || loco.proto!=MM) { QueuedCommand cmd(loco, Locomotive::FUNCTIONS, func); push_command(cmd); @@ -492,6 +505,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,6 +530,25 @@ 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; } @@ -565,8 +605,16 @@ void ArduControl::ControlThread::main() Locomotive *loco = 0; unsigned phase = 0; unsigned s88_octets_remaining = 0; + + Msp::Time::TimeStamp next_mfx_announce; + unsigned mfx_search_size = 0; + unsigned mfx_search_bits = 0; + Msp::Time::TimeStamp next_mfx_search; + while(!done) { + Time::TimeStamp t = Time::now(); + if(!repeat_count) { tag = Tag(); @@ -587,6 +635,25 @@ void ArduControl::ControlThread::main() 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; @@ -596,9 +663,17 @@ void ArduControl::ControlThread::main() } else if((loco = control.get_loco_to_refresh())) { - length = loco->create_speed_dir_command(command); - repeat_count = 2; - phase = 0; + 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 { @@ -611,7 +686,7 @@ void ArduControl::ControlThread::main() } } - if(control.debug) + if(control.debug>=2) { string cmd_hex; for(unsigned i=0; i=2) { string reply_hex; for(unsigned i=0; i=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]); diff --git a/source/libr2c2/arducontrol.h b/source/libr2c2/arducontrol.h index 4e9ef63..3a86cee 100644 --- a/source/libr2c2/arducontrol.h +++ b/source/libr2c2/arducontrol.h @@ -26,6 +26,14 @@ private: MOTOROLA_SPEED_DIRECTION = 0x13, MOTOROLA_SPEED_FUNCTION = 0x14, MOTOROLA_SOLENOID = 0x15, + MFX_SET_STATION_ID = 0x21, + MFX_ANNOUNCE = 0x22, + MFX_SEARCH = 0x23, + MFX_ASSIGN_ADDRESS = 0x24, + MFX_PING = 0x25, + MFX_SPEED = 0x28, + MFX_SPEED_FUNCS8 = 0x29, + MFX_SPEED_FUNCS16 = 0x2A, S88_READ = 0x30, COMMAND_OK = 0x80, RECEIVE_OVERRUN = 0x81, @@ -37,7 +45,8 @@ private: TRACK_CURRENT = 0xC0, INPUT_VOLTAGE = 0xC1, POWER_STATE = 0xC2, - S88_DATA = 0xD0 + S88_DATA = 0xD0, + MFX_FEEDBACK = 0xD1 }; struct Tag @@ -68,7 +77,8 @@ private: enum Protocol { - MM + MM, + MFX }; struct ProtocolInfo @@ -189,7 +199,7 @@ private: typedef std::map SensorMap; Msp::IO::Serial serial; - bool debug; + unsigned debug; ControlledVariable power; @@ -207,6 +217,9 @@ private: SensorMap sensors; unsigned n_s88_octets; + unsigned mfx_announce_serial; + unsigned next_mfx_address; + Msp::Mutex mutex; ControlThread thread; -- 2.43.0