Detect and set arducontrol baud rate on startup

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 8c17234051752e2527295016e197d2b8d8e5237c..07a2215c4cfb4386940acdf185530cbefd9204e5 100644 (file)
--- 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 46af9238af3fe845e5e19af05ea285b27987361d..33696bea818077b1bc3aea0432a98b306a058621 100644 (file)
--- 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);