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 7f95b631aa119bd77e7698d48c52b94fedd7bb23..86b3666b2ff13a478ed8a391fe3c2d916b04188d 100644 (file)
--- 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);