From: Mikko Rasa Date: Tue, 12 Nov 2013 19:49:51 +0000 (+0200) Subject: Detect and set arducontrol baud rate on startup X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=0bfaea65cfa324dfbb517b551709219529ef24b8;p=r2c2.git Detect and set arducontrol baud rate on startup --- 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);