Guest User

Untitled

a guest
Apr 2nd, 2014
119
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. public class RazorAHRS {
  2. private static final String TAG = "RazorAHRS";
  3. private static final boolean DEBUG = false;
  4. private static final String SYNCH_TOKEN = "#SYNCH";
  5. private static final String NEW_LINE = "\r\n";
  6.  
  7. // Timeout to init Razor AHRS after a Bluetooth connection has been established
  8. public static final int INIT_TIMEOUT_MS = 10000;
  9.  
  10. // IDs passed to internal message handler
  11. private static final int MSG_ID__YPR_DATA = 0;
  12. private static final int MSG_ID__IO_EXCEPTION_AND_DISCONNECT = 1;
  13. private static final int MSG_ID__CONNECT_OK = 2;
  14. private static final int MSG_ID__CONNECT_FAIL = 3;
  15. private static final int MSG_ID__CONNECT_ATTEMPT = 4;
  16. private static final int MSG_ID__AMG_DATA = 5;
  17.  
  18. private static final UUID UUID_SPP = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");
  19.  
  20. /**
  21. * Razor output modes.
  22. * Use <code>YAW_PITCH_ROLL_ANGLES</code> to receive yaw, pitch and roll in degrees. <br>
  23. * Use <code>RAW_SENSOR_DATA</code> or <code>CALIBRATED_SENSOR_DATA</code> to read raw or
  24. * calibrated xyz sensor data of the accelerometer, magnetometer and the gyroscope.
  25. */
  26. public enum RazorOutputMode {
  27. YAW_PITCH_ROLL_ANGLES,
  28. RAW_SENSOR_DATA,
  29. CALIBRATED_SENSOR_DATA
  30. }
  31. private RazorOutputMode razorOutputMode;
  32.  
  33. private enum ConnectionState {
  34. DISCONNECTED,
  35. CONNECTING,
  36. CONNECTED,
  37. USER_DISCONNECT_REQUEST
  38. }
  39. volatile private ConnectionState connectionState = ConnectionState.DISCONNECTED;
  40.  
  41. volatile private BluetoothSocket btSocket;
  42. volatile private BluetoothDevice btDevice;
  43. volatile private InputStream inStream;
  44. volatile private OutputStream outStream;
  45.  
  46. private RazorListener razorListener;
  47. private boolean callbacksEnabled = true;
  48.  
  49. BluetoothThread btThread;
  50.  
  51. private int numConnectAttempts;
  52.  
  53. // Object pools
  54. private ObjectPool<float[]> float3Pool = new ObjectPool<float[]>(new ObjectPool.ObjectFactory<float[]>() {
  55. @Override
  56. public float[] newObject() {
  57. return new float[3];
  58. }
  59. });
  60. private ObjectPool<float[]> float9Pool = new ObjectPool<float[]>(new ObjectPool.ObjectFactory<float[]>() {
  61. @Override
  62. public float[] newObject() {
  63. return new float[9];
  64. }
  65. });
  66.  
  67. /**
  68. * Constructor.
  69. * Must be called from the thread where you want receive the RazorListener callbacks! So if you
  70. * want to manipulate Android UI from the callbacks you have to call this from your main/UI
  71. * thread.
  72. *
  73. * @param btDevice {@link android.bluetooth.BluetoothDevice BluetoothDevice} holding the Razor
  74. * AHRS to connect to.
  75. * @param razorListener {@link RazorListener} that will be notified of Razor AHRS events.
  76. * @throws RuntimeException thrown if one of the parameters is null.
  77. */
  78. public RazorAHRS(BluetoothDevice btDevice, RazorListener razorListener)
  79. throws RuntimeException {
  80. this(btDevice, razorListener, RazorOutputMode.CALIBRATED_SENSOR_DATA);
  81. }
  82.  
  83. /**
  84. * Constructor.
  85. * Must be called from the thread where you want receive the RazorListener callbacks! So if you
  86. * want to manipulate Android UI from the callbacks you have to call this from your main/UI
  87. * thread.
  88. *
  89. * @param btDevice {@link android.bluetooth.BluetoothDevice BluetoothDevice} holding the Razor
  90. * AHRS to connect to.
  91. * @param razorListener {@link RazorListener} that will be notified of Razor AHRS events.
  92. * @param razorOutputMode {@link RazorOutputMode} that you desire.
  93. * @throws RuntimeException thrown if one of the parameters is null.
  94. */
  95. public RazorAHRS(BluetoothDevice btDevice, RazorListener razorListener, RazorOutputMode razorOutputMode)
  96. throws RuntimeException {
  97. if (btDevice == null)
  98. throw new RuntimeException("BluetoothDevice can not be null.");
  99. this.btDevice = btDevice;
  100.  
  101. if (razorListener == null)
  102. throw new RuntimeException("RazorListener can not be null.");
  103. this.razorListener = razorListener;
  104.  
  105. if (razorOutputMode == null)
  106. throw new RuntimeException("RazorMode can not be null.");
  107. this.razorOutputMode = razorOutputMode;
  108. }
  109.  
  110. /**
  111. * @return <code>true</code> if listener callbacks are currently enabled, <code>false</code> else.
  112. */
  113. public boolean getCallbacksEnabled() {
  114. return callbacksEnabled;
  115. }
  116.  
  117. /**
  118. * Enables/disables listener callbacks.
  119. * @param enabled
  120. */
  121. public void setCallbacksEnabled(boolean enabled) {
  122. callbacksEnabled = enabled;
  123. }
  124.  
  125. /**
  126. * Connect and start reading. Both is done asynchronously. {@link RazorListener#onConnectOk()}
  127. * or {@link RazorListener#onConnectFail(IOException)} callbacks will be invoked.
  128. *
  129. * @param numConnectAttempts Number of attempts to make when trying to connect. Often connecting
  130. * only works on the 2rd try or later. Bluetooth hooray.
  131. */
  132. public void asyncConnect(int numConnectAttempts) {
  133. if (DEBUG) Log.d(TAG, "asyncConnect() BEGIN");
  134. // Disconnect and wait for running thread to end, if needed
  135. if (btThread != null) {
  136. asyncDisconnect();
  137. try {
  138. btThread.join();
  139. } catch (InterruptedException e) { }
  140. }
  141.  
  142. // Bluetooth thread not running any more, we're definitely in DISCONNECTED state now
  143.  
  144. // Create new thread to connect to Razor AHRS and read input
  145. this.numConnectAttempts = numConnectAttempts;
  146. connectionState = ConnectionState.CONNECTING;
  147. btThread = new BluetoothThread();
  148. btThread.start();
  149. if (DEBUG) Log.d(TAG, "asyncConnect() END");
  150. }
  151.  
  152. /**
  153. * Disconnects from Razor AHRS. If still connecting this will also cancel the connection process.
  154. */
  155. public void asyncDisconnect() {
  156. if (DEBUG) Log.d(TAG, "asyncDisconnect() BEGIN");
  157. synchronized (connectionState) {
  158. if (DEBUG) Log.d(TAG, "asyncDisconnect() SNYNCHRONIZED");
  159. // Don't go to USER_DISCONNECT_REQUEST state if we are disconnected already
  160. if (connectionState == ConnectionState.DISCONNECTED)
  161. return;
  162.  
  163. // This is a wanted disconnect, so we force (blocking) I/O to break
  164. connectionState = ConnectionState.USER_DISCONNECT_REQUEST;
  165. closeSocketAndStreams();
  166. }
  167. if (DEBUG) Log.d(TAG, "asyncDisconnect() END");
  168. }
  169.  
  170. /**
  171. * Writes out a string using ASCII encoding. Assumes we're connected. Does not handle
  172. * exceptions itself.
  173. *
  174. * @param text Text to send out
  175. * @throws IOException
  176. */
  177. private void write(String text) throws IOException {
  178. outStream.write(EncodingUtils.getAsciiBytes(text));
  179. }
  180.  
  181. /**
  182. * Closes I/O streams and Bluetooth socket.
  183. */
  184. private void closeSocketAndStreams() {
  185. if (DEBUG) Log.d(TAG, "closeSocketAndStreams() BEGIN");
  186. // Try to switch off streaming output of Razor in preparation of next connect
  187. try {
  188. if (outStream != null)
  189. write("#o0");
  190. } catch (IOException e) { }
  191.  
  192. // Close Bluetooth socket => I/O operations immediately will throw exception
  193. try {
  194. if (btSocket != null)
  195. btSocket.close();
  196. } catch (IOException e) { }
  197. if (DEBUG) Log.d(TAG, "closeSocketAndStreams() BT SOCKET CLOSED");
  198.  
  199. // Close streams
  200. try {
  201. if (inStream != null)
  202. inStream.close();
  203. } catch (IOException e) { }
  204. try {
  205. if (outStream != null)
  206. outStream.close();
  207. } catch (IOException e) { }
  208. if (DEBUG) Log.d(TAG, "closeSocketAndStreams() STREAMS CLOSED");
  209.  
  210. // Do not set socket and streams null, because input thread might still access them
  211. //inStream = null;
  212. //outStream = null;
  213. //btSocket = null;
  214.  
  215. if (DEBUG) Log.d(TAG, "closeSocketAndStreams() END");
  216. }
  217.  
  218. /**
  219. * Thread that handles connecting to and reading from Razor AHRS.
  220. */
  221. private class BluetoothThread extends Thread {
  222. byte[] inBuf = new byte[512];
  223. int inBufPos = 0;
  224.  
  225. /**
  226. * Blocks until it can read one byte of input, assumes we have a connection up and running.
  227. *
  228. * @return One byte from input stream
  229. * @throws IOException If reading input stream fails
  230. */
  231. private byte readByte() throws IOException {
  232. int in = inStream.read();
  233. if (in == -1)
  234. throw new IOException("End of Stream");
  235. return (byte) in;
  236. }
  237.  
  238. /**
  239. * Converts a buffer of bytes to an array of floats. This method does not do any error
  240. * checking on parameter array sizes.
  241. * @param byteBuf Byte buffer with length of at least <code>numFloats * 4</code>.
  242. * @param floatArr Float array with length of at least <code>numFloats</code>.
  243. * @param numFloats Number of floats to convert
  244. */
  245. private void byteBufferToFloatArray(byte[] byteBuf, float[] floatArr, int numFloats) {
  246. //int numFloats = byteBuf.length / 4;
  247. for (int i = 0; i < numFloats * 4; i += 4) {
  248. // Convert from little endian (Razor) to big endian (Java) and interpret as float
  249. floatArr[i/4] = Float.intBitsToFloat((byteBuf[i] & 0xff) + ((byteBuf[i+1] & 0xff) << 8) +
  250. ((byteBuf[i+2] & 0xff) << 16) + ((byteBuf[i+3] & 0xff) << 24));
  251. }
  252. }
  253.  
  254. /**
  255. * Parse input stream for given token.
  256. * @param token Token to find
  257. * @param in Next byte from input stream
  258. * @return <code>true</code> if token was found
  259. */
  260. private boolean readToken(byte[] token, byte in) {
  261. if (in == token[inBufPos++]) {
  262. if (inBufPos == token.length) {
  263. // Synch token found
  264. inBufPos = 0;
  265. if (DEBUG) Log.d(TAG, "Token found");
  266. return true;
  267. }
  268. } else {
  269. inBufPos = 0;
  270. }
  271.  
  272. return false;
  273. }
  274.  
  275. /**
  276. * Synches with Razor AHRS and sets parameters.
  277. * @throws IOException
  278. */
  279. private void initRazor() throws IOException {
  280. long t0, t1, t2;
  281.  
  282. // Start time
  283. t0 = SystemClock.uptimeMillis();
  284.  
  285. /* See if Razor is there */
  286. // Request synch token to see when Razor is up and running
  287. final String contactSynchID = "00";
  288. final String contactSynchRequest = "#s" + contactSynchID;
  289. final byte[] contactSynchReply = EncodingUtils.getAsciiBytes(SYNCH_TOKEN + contactSynchID + NEW_LINE);
  290. write(contactSynchRequest);
  291. t1 = SystemClock.uptimeMillis();
  292.  
  293. while (true) {
  294. // Check timeout
  295. t2 = SystemClock.uptimeMillis();
  296. if (t2 - t1 > 200) {
  297. // 200ms elapsed since last request and no answer -> request synch again.
  298. // (This happens when DTR is connected and Razor resets on connect)
  299. write(contactSynchRequest);
  300. t1 = t2;
  301. }
  302. if (t2 - t0 > INIT_TIMEOUT_MS)
  303. // Timeout -> tracker not present
  304. throw new IOException("Can not init Razor: response timeout");
  305.  
  306. // See if we can read something
  307. if (inStream.available() > 0) {
  308. // Synch token found?
  309. if (readToken(contactSynchReply, readByte()))
  310. break;
  311. } else {
  312. // No data available, wait
  313. delay(5); // 5ms
  314. }
  315. }
  316.  
  317. /* Configure tracker */
  318. // Set binary output mode, enable continuous streaming, disable errors and request synch
  319. // token. So we're good, no matter what state the tracker currently is in.
  320. final String configSynchID = "01";
  321. final byte[] configSynchReply = EncodingUtils.getAsciiBytes(SYNCH_TOKEN + configSynchID + NEW_LINE);
  322. write("#ob#o1#oe0#s" + configSynchID);
  323. while (!readToken(configSynchReply, readByte())) { }
  324. }
  325.  
  326. /**
  327. * Opens Bluetooth connection to Razor AHRS.
  328. * @throws IOException
  329. */
  330. private void connect() throws IOException {
  331. // Create Bluetooth socket
  332. btSocket = btDevice.createRfcommSocketToServiceRecord(UUID_SPP);
  333. if (btSocket == null) {
  334. if (DEBUG) Log.d(TAG, "btSocket is null in connect()");
  335. throw new IOException("Could not create Bluetooth socket");
  336. }
  337.  
  338. // This could be used to create the RFCOMM socekt on older Android devices where
  339. //createRfcommSocketToServiceRecord is not present yet.
  340. /*try {
  341. Method m = btDevice.getClass().getMethod("createRfcommSocket", new Class[] { int.class });
  342. btSocket = (BluetoothSocket) m.invoke(btDevice, Integer.valueOf(1));
  343. } catch (Exception e) {
  344. throw new IOException("Could not create Bluetooth socket using reflection");
  345. }*/
  346.  
  347. // Connect socket to Razor AHRS
  348. if (DEBUG) Log.d(TAG, "Canceling bt discovery");
  349. BluetoothAdapter.getDefaultAdapter().cancelDiscovery(); // Recommended
  350. if (DEBUG) Log.d(TAG, "Trying to connect() btSocket");
  351. btSocket.connect();
  352.  
  353. // Get the input and output streams
  354. if (DEBUG) Log.d(TAG, "Trying to create streams");
  355. inStream = btSocket.getInputStream();
  356. outStream = btSocket.getOutputStream();
  357. if (inStream == null || outStream == null) {
  358. if (DEBUG) Log.d(TAG, "Could not create I/O stream(s) in connect()");
  359. throw new IOException("Could not create I/O stream(s)");
  360. }
  361. }
  362.  
  363. /**
  364. * Bluetooth I/O thread entry method.
  365. */
  366. public void run() {
  367. if (DEBUG) Log.d(TAG, "Bluetooth I/O thread started");
  368. try {
  369. // Check if btDevice is set
  370. if (btDevice == null) {
  371. if (DEBUG) Log.d(TAG, "btDevice is null in run()");
  372. throw new IOException("Bluetooth device is null");
  373. }
  374.  
  375. // Make several attempts to connect
  376. int i = 1;
  377. while (true) {
  378. if (DEBUG) Log.d(TAG, "Connect attempt " + i + " of " + numConnectAttempts);
  379. sendToParentThread(MSG_ID__CONNECT_ATTEMPT, i);
  380. try {
  381. connect();
  382. break; // Alrighty!
  383. } catch (IOException e) {
  384. if (DEBUG) Log.d(TAG, "Attempt failed: " + e.getMessage());
  385. // Maximum number of attempts reached or cancel requested?
  386. if (i == numConnectAttempts || connectionState == ConnectionState.USER_DISCONNECT_REQUEST)
  387. throw e;
  388.  
  389. // We couldn't connect on first try, manually starting Bluetooth discovery
  390. // often helps
  391. if (DEBUG) Log.d(TAG, "Starting BT discovery");
  392. BluetoothAdapter.getDefaultAdapter().startDiscovery();
  393. delay(5000); // 5 seconds - long enough?
  394.  
  395. i++;
  396. }
  397. }
  398.  
  399. // Set Razor output mode
  400. if (DEBUG) Log.d(TAG, "Trying to set Razor output mode");
  401. initRazor();
  402.  
  403. // We're connected and initialized (unless disconnect was requested)
  404. synchronized (connectionState) {
  405. if (connectionState == ConnectionState.USER_DISCONNECT_REQUEST) {
  406. closeSocketAndStreams();
  407. throw new IOException(); // Dummy exception to force disconnect
  408. }
  409. else connectionState = ConnectionState.CONNECTED;
  410. }
  411.  
  412. // Tell listener we're ready
  413. sendToParentThread(MSG_ID__CONNECT_OK, null);
  414.  
  415. // Keep reading inStream until an exception occurs
  416. if (DEBUG) Log.d(TAG, "Starting input loop");
  417. while (true) {
  418. // Read byte from input stream
  419. inBuf[inBufPos++] = (byte) readByte();
  420.  
  421. if (razorOutputMode == RazorOutputMode.YAW_PITCH_ROLL_ANGLES) {
  422. if (inBufPos == 12) { // We received a full frame
  423. float[] ypr = float3Pool.get();
  424. byteBufferToFloatArray(inBuf, ypr, 3);
  425.  
  426. // Forward to parent thread handler
  427. sendToParentThread(MSG_ID__YPR_DATA, ypr);
  428.  
  429. // Rewind input buffer position
  430. inBufPos = 0;
  431. }
  432. } else { // Raw or calibrated sensor data mode
  433. if (inBufPos == 36) { // We received a full frame
  434. float[] amg = float9Pool.get();
  435. byteBufferToFloatArray(inBuf, amg, 9);
  436.  
  437. // Forward to parent thread handler
  438. sendToParentThread(MSG_ID__AMG_DATA, amg);
  439.  
  440. // Rewind input buffer position
  441. inBufPos = 0;
  442. }
  443. }
  444. }
  445. } catch (IOException e) {
  446. if (DEBUG) Log.d(TAG, "IOException in Bluetooth thread: " + e.getMessage());
  447. synchronized (connectionState) {
  448. // Don't forward exception if it was thrown because we broke I/O on purpose in
  449. // other thread when user requested disconnect
  450. if (connectionState != ConnectionState.USER_DISCONNECT_REQUEST) {
  451. // There was a true I/O error, cleanup and forward exception
  452. closeSocketAndStreams();
  453. if (DEBUG) Log.d(TAG, "Forwarding exception");
  454. if (connectionState == ConnectionState.CONNECTING)
  455. sendToParentThread(MSG_ID__CONNECT_FAIL, e);
  456. else
  457. sendToParentThread(MSG_ID__IO_EXCEPTION_AND_DISCONNECT, e);
  458. } else {
  459. // I/O error was caused on purpose, socket and streams are closed already
  460. }
  461.  
  462. // I/O closed, thread done => we're disconnected now
  463. connectionState = ConnectionState.DISCONNECTED;
  464. }
  465. }
  466. }
  467.  
  468. /**
  469. * Sends a message to Handler assigned to parent thread.
  470. *
  471. * @param msgId
  472. * @param data
  473. */
  474. private void sendToParentThread(int msgId, Object o) {
  475. if (callbacksEnabled)
  476. parentThreadHandler.obtainMessage(msgId, o).sendToTarget();
  477. }
  478.  
  479. /**
  480. * Sends a message to Handler assigned to parent thread.
  481. *
  482. * @param msgId
  483. * @param data
  484. */
  485. private void sendToParentThread(int msgId, int i) {
  486. if (callbacksEnabled)
  487. parentThreadHandler.obtainMessage(msgId, i, -1).sendToTarget();
  488. }
  489.  
  490. /**
  491. * Wrapper for {@link Thread#sleep(long)};
  492. * @param ms Milliseconds
  493. */
  494. void delay(long ms) {
  495. try {
  496. sleep(ms); // Sleep 5ms
  497. } catch (InterruptedException e) { }
  498. }
  499. }
  500.  
  501. /**
  502. * Handler that forwards messages to the RazorListener callbacks. This handler runs in the
  503. * thread this RazorAHRS object was created in and receives data from the Bluetooth I/O thread.
  504. */
  505. private Handler parentThreadHandler = new Handler() {
  506. @Override
  507. public void handleMessage(Message msg) {
  508. switch (msg.what) {
  509. case MSG_ID__YPR_DATA: // Yaw, pitch and roll data
  510. float[] ypr = (float[]) msg.obj;
  511. razorListener.onAnglesUpdate(ypr[0], ypr[1], ypr[2]);
  512. float3Pool.put(ypr);
  513. break;
  514. case MSG_ID__AMG_DATA: // Accelerometer, magnetometer and gyroscope data
  515. float[] amg = (float[]) msg.obj;
  516. razorListener.onSensorsUpdate(amg[0], amg[1], amg[2], amg[3], amg[4], amg[5],
  517. amg[6], amg[7], amg[8]);
  518. float9Pool.put(amg);
  519. break;
  520. case MSG_ID__IO_EXCEPTION_AND_DISCONNECT:
  521. razorListener.onIOExceptionAndDisconnect((IOException) msg.obj);
  522. break;
  523. case MSG_ID__CONNECT_ATTEMPT:
  524. razorListener.onConnectAttempt(msg.arg1, numConnectAttempts);
  525. break;
  526. case MSG_ID__CONNECT_OK:
  527. razorListener.onConnectOk();
  528. break;
  529. case MSG_ID__CONNECT_FAIL:
  530. razorListener.onConnectFail((IOException) msg.obj);
  531. break;
  532. }
  533. }
  534. };
  535. }
RAW Paste Data