From df99c7e41d87084fe7f71729f3df33d7effc59cc Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Fri, 4 Apr 2014 18:47:54 +0300
Subject: [PATCH] Attempt to resync ArduControl if there's no response to a
 command

---
 source/libr2c2/arducontrol.cpp | 33 ++++++++++++++++++++++++++++++++-
 1 file changed, 32 insertions(+), 1 deletion(-)

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);
-- 
2.43.0