From 0bfaea65cfa324dfbb517b551709219529ef24b8 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Tue, 12 Nov 2013 21:49:51 +0200
Subject: [PATCH] Detect and set arducontrol baud rate on startup

---
 source/libr2c2/arducontrol.cpp | 47 ++++++++++++++++++++++++++++++++++
 source/libr2c2/arducontrol.h   |  3 +++
 2 files changed, 50 insertions(+)

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 8c17234..07a2215 100644
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -848,6 +848,8 @@ void ArduControl::ControlThread::exit()
 
 void ArduControl::ControlThread::main()
 {
+	init_baud_rate();
+
 	while(!done)
 	{
 		PendingCommand cmd;
@@ -864,6 +866,51 @@ void ArduControl::ControlThread::main()
 	}
 }
 
+void ArduControl::ControlThread::init_baud_rate()
+{
+	static unsigned rates[] = { 57600, 9600, 19200, 38400, 0 };
+	unsigned rate = 0;
+	control.serial.set_data_bits(8);
+	control.serial.set_parity(IO::Serial::NONE);
+	control.serial.set_stop_bits(1);
+	for(unsigned i=0; rates[i]; ++i)
+	{
+		control.serial.set_baud_rate(rates[i]);
+		control.serial.put('\xFF');
+		if(IO::poll(control.serial, IO::P_INPUT, 500*Time::msec))
+		{
+			int c = control.serial.get();
+			if(c==0xFF)
+			{
+				rate = rates[i];
+				break;
+			}
+		}
+	}
+
+	if(control.debug>=1)
+		IO::print("ArduControl detected at %d bits/s\n", rate);
+
+	if(rate!=rates[0])
+	{
+		PendingCommand cmd;
+		cmd.command[0] = SET_BAUD_RATE;
+		cmd.command[1] = rates[0]>>8;
+		cmd.command[2] = rates[0];
+		cmd.length = 3;
+		if(do_command(cmd)==COMMAND_OK)
+		{
+			control.serial.set_baud_rate(rates[0]);
+			if(do_command(cmd)==COMMAND_OK)
+			{
+				Time::sleep(Time::sec);
+				if(control.debug>=1)
+					IO::print("Rate changed to %d bits/s\n", rates[0]);
+			}
+		}
+	}
+}
+
 bool ArduControl::ControlThread::get_work(PendingCommand &cmd)
 {
 	if(control.pop_command(cmd))
diff --git a/source/libr2c2/arducontrol.h b/source/libr2c2/arducontrol.h
index 46af923..33696be 100644
--- a/source/libr2c2/arducontrol.h
+++ b/source/libr2c2/arducontrol.h
@@ -35,6 +35,7 @@ private:
 		MFX_SPEED_FUNCS8 = 0x29,
 		MFX_SPEED_FUNCS16 = 0x2A,
 		S88_READ = 0x30,
+		SET_BAUD_RATE = 0x70,
 		COMMAND_OK = 0x80,
 		RECEIVE_OVERRUN = 0x81,
 		FRAMING_ERROR = 0x82,
@@ -42,6 +43,7 @@ private:
 		LENGTH_ERROR = 0x84,
 		INVALID_VALUE = 0x85,
 		OVERCURRENT = 0xA0,
+		BAUD_CHANGE_FAILED = 0xA1,
 		TRACK_CURRENT = 0xC0,
 		INPUT_VOLTAGE = 0xC1,
 		POWER_STATE = 0xC2,
@@ -276,6 +278,7 @@ private:
 		void exit();
 	private:
 		virtual void main();
+		void init_baud_rate();
 		bool get_work(PendingCommand &);
 		unsigned do_command(const PendingCommand &);
 		unsigned process_reply(const char *, unsigned);
-- 
2.43.0