+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(!rate)
+ {
+ done = true;
+ return;
+ }
+
+ 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]);
+ Time::sleep(Time::sec);
+ if(do_command(cmd)==COMMAND_OK)
+ {
+ if(control.debug>=1)
+ IO::print("Rate changed to %d bits/s\n", rates[0]);
+ }
+ }
+ }
+}
+