From b4beace6f1d3e018dbe1495430d32f828d406229 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Fri, 4 Apr 2014 18:47:13 +0300
Subject: [PATCH] Add a timeout to ArduControl commands

---
 source/libr2c2/arducontrol.cpp | 13 +++++++++----
 source/libr2c2/arducontrol.h   |  3 ++-
 2 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 1bb48d4..7f95b63 100644
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -25,6 +25,7 @@ ArduControl::ArduControl(const Options &opts):
 	power(false),
 	halted(false),
 	active_accessory(0),
+	command_timeout(200*Time::msec),
 	s88(*this),
 	mfx_search(*this),
 	thread(*this)
@@ -1074,11 +1075,11 @@ void ArduControl::ControlThread::init_baud_rate()
 		cmd.command[1] = rates[0]>>8;
 		cmd.command[2] = rates[0];
 		cmd.length = 3;
-		if(do_command(cmd)==COMMAND_OK)
+		if(do_command(cmd, Time::sec)==COMMAND_OK)
 		{
 			control.serial.set_baud_rate(rates[0]);
 			Time::sleep(Time::sec);
-			if(do_command(cmd)==COMMAND_OK)
+			if(do_command(cmd, Time::sec)==COMMAND_OK)
 			{
 				if(control.debug>=1)
 					IO::print("Rate changed to %d bits/s\n", rates[0]);
@@ -1106,7 +1107,7 @@ bool ArduControl::ControlThread::get_work(PendingCommand &cmd)
 	return true;
 }
 
-unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd)
+unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd, const Time::TimeDelta &timeout)
 {
 	if(control.debug>=2)
 	{
@@ -1126,7 +1127,7 @@ unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd)
 		if(result)
 			got_data = IO::poll(control.serial, IO::P_INPUT, Time::zero);
 		else
-			got_data = IO::poll(control.serial, IO::P_INPUT);
+			got_data = IO::poll(control.serial, IO::P_INPUT, timeout);
 
 		if(!got_data)
 			break;
@@ -1141,7 +1142,11 @@ unsigned ArduControl::ControlThread::do_command(const PendingCommand &cmd)
 		char reply[15];
 		unsigned pos = 0;
 		while(pos<rlength)
+		{
+			if(!IO::poll(control.serial, IO::P_INPUT, timeout))
+				return 0;
 			pos += control.serial.read(reply+pos, rlength-pos);
+		}
 
 		if(control.debug>=2)
 		{
diff --git a/source/libr2c2/arducontrol.h b/source/libr2c2/arducontrol.h
index 01ee281..40c7b3c 100644
--- a/source/libr2c2/arducontrol.h
+++ b/source/libr2c2/arducontrol.h
@@ -357,7 +357,7 @@ private:
 		virtual void main();
 		void init_baud_rate();
 		bool get_work(PendingCommand &);
-		unsigned do_command(const PendingCommand &);
+		unsigned do_command(const PendingCommand &, const Msp::Time::TimeDelta &);
 		unsigned process_reply(const char *, unsigned);
 	};
 
@@ -384,6 +384,7 @@ private:
 
 	SensorMap sensors;
 
+	Msp::Time::TimeDelta command_timeout;
 	Queue<PendingCommand> command_queue;
 	Queue<Tag> completed_commands;
 	RefreshTask refresh;
-- 
2.43.0