SHARE
TWEET

Untitled

a guest Apr 2nd, 2014 74 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
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top