X-Git-Url: http://git.tdb.fi/?a=blobdiff_plain;f=source%2Flibr2c2%2Farducontrol.cpp;h=7111f53012b5ec177e23f718dcf5fdc607286307;hb=4b287b53e9127209864292f583b8ddbdd481d022;hp=e5d94ef9d5d180e0bf3f7f5a5cfdcd847f48ae77;hpb=daecd71f20c275fb6df128a3aaa4cfdda24b6886;p=r2c2.git

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<<func;
 	if(loco.funcs.set((loco.funcs&~mask)|(mask*state)))
 	{
-		if(func>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<length; ++i)
@@ -646,7 +721,7 @@ void ArduControl::ControlThread::main()
 				while(pos<rlength)
 					pos += control.serial.read(reply+pos, rlength-pos);
 
-				if(control.debug)
+				if(control.debug>=2)
 				{
 					string reply_hex;
 					for(unsigned i=0; i<rlength; ++i)
@@ -673,6 +748,52 @@ void ArduControl::ControlThread::main()
 					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<unsigned char>(reply[1]);