From: Mikko Rasa <tdb@tdb.fi>
Date: Fri, 4 Apr 2014 15:47:54 +0000 (+0300)
Subject: Attempt to resync ArduControl if there's no response to a command
X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=df99c7e41d87084fe7f71729f3df33d7effc59cc;p=r2c2.git

Attempt to resync ArduControl if there's no response to a command
---

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 7f95b63..86b3666 100644
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -1025,10 +1025,41 @@ void ArduControl::ControlThread::main()
 		if(get_work(cmd))
 		{
 			bool success = true;
+			bool resync = false;
 			for(unsigned i=0; (success && i<cmd.repeat_count); ++i)
-				success = (do_command(cmd)==COMMAND_OK);
+			{
+				unsigned result = do_command(cmd, control.command_timeout);
+				success = (result==COMMAND_OK);
+				resync = (result==0);
+			}
+
 			if(success && cmd.tag)
 				control.completed_commands.push(cmd.tag);
+
+			if(resync)
+			{
+				if(control.debug>=1)
+					IO::print("Synchronization with ArduControl lost, attempting to recover\n");
+				for(unsigned i=0; (resync && i<16); ++i)
+				{
+					control.serial.put('\xFF');
+					while(IO::poll(control.serial, IO::P_INPUT, control.command_timeout))
+						resync = (control.serial.get()!=0xFF);
+				}
+				if(resync)
+				{
+					if(control.debug>=1)
+						IO::print("Resynchronization failed, giving up\n");
+					done = true;
+				}
+				else
+				{
+					if(control.debug>=1)
+						IO::print("Resynchronization successful\n");
+					if(cmd.tag)
+						control.command_queue.push(cmd);
+				}
+			}
 		}
 		else
 			Time::sleep(10*Time::msec);