Skip to content

Commit

Permalink
Added ability to read direct sensor data to Android library
Browse files Browse the repository at this point in the history
  • Loading branch information
@ptrbrtz
ptrbrtz committed Oct 19, 2013
1 parent 82589be commit 9708aa4
Show file tree
Hide file tree
Showing 4 changed files with 150 additions and 64 deletions.
3 changes: 2 additions & 1 deletion Android/RazorAHRS/.classpath
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<classpath>
<classpathentry kind="con" path="com.android.ide.eclipse.adt.ANDROID_FRAMEWORK"/>
<classpathentry kind="src" path="src"/>
<classpathentry kind="src" path="gen"/>
<classpathentry kind="con" path="com.android.ide.eclipse.adt.ANDROID_FRAMEWORK"/>
<classpathentry kind="con" path="com.android.ide.eclipse.adt.LIBRARIES"/>
<classpathentry kind="output" path="bin/classes"/>
</classpath>
172 changes: 122 additions & 50 deletions Android/RazorAHRS/src/de/tuberlin/qu/razorahrs/RazorAHRS.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,11 @@
* for Sparkfun "9DOF Razor IMU" and "9DOF Sensor Stick"
*
* Released under GNU GPL (General Public License) v3.0
* Copyright (C) 2013 Peter Bartz
* Copyright (C) 2013 Peter Bartz [http://ptrbrtz.net]
* Copyright (C) 2011-2012 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin
* Written by Peter Bartz ([email protected])
*
* Infos, updates, bug reports and feedback:
* Infos, updates, bug reports, contributions and feedback:
* https://github.com/ptrbrtz/razor-9dof-ahrs
******************************************************************************************/

Expand All @@ -34,8 +34,8 @@
* <p>
* Bluetooth seems to be even more picky on Android than it is anyway. Be sure to have a look at the
* section about Android Bluetooth in the tutorial at
* <a href="http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs">
* http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs</a>!
* <a href="https://github.com/ptrbrtz/razor-9dof-ahrs">
* https://github.com/ptrbrtz/razor-9dof-ahrs</a>!
* <p>
* The app using this class has to
* <ul>
Expand All @@ -57,67 +57,108 @@ public class RazorAHRS {
private static final boolean DEBUG = false;
private static final String SYNCH_TOKEN = "#SYNCH";
private static final String NEW_LINE = "\r\n";

// Timeout to init Razor AHRS after a Bluetooth connection has been established
public static final int INIT_TIMEOUT_MS = 5000;

// IDs passed to internal message handler
private static final int MSG_ID__YPR_DATA = 0;
private static final int MSG_ID__IO_EXCEPTION_AND_DISCONNECT = 1;
private static final int MSG_ID__CONNECT_OK = 2;
private static final int MSG_ID__CONNECT_FAIL = 3;
private static final int MSG_ID__CONNECT_ATTEMPT = 4;

private static final int MSG_ID__AMG_DATA = 5;

private static final UUID UUID_SPP = UUID.fromString("00001101-0000-1000-8000-00805F9B34FB");


/**
* Razor output modes.
* Use <code>YAW_PITCH_ROLL_ANGLES</code> to receive yaw, pitch and roll in degrees. <br>
* Use <code>RAW_SENSOR_DATA</code> or <code>CALIBRATED_SENSOR_DATA</code> to read raw or
* calibrated xyz sensor data of the accelerometer, magnetometer and the gyroscope.
*/
public enum RazorOutputMode {
YAW_PITCH_ROLL_ANGLES,
RAW_SENSOR_DATA,
CALIBRATED_SENSOR_DATA
}
private RazorOutputMode razorOutputMode;

private enum ConnectionState {
DISCONNECTED,
CONNECTING,
CONNECTED,
USER_DISCONNECT_REQUEST
}
volatile private ConnectionState connectionState = ConnectionState.DISCONNECTED;

volatile private BluetoothSocket btSocket;
volatile private BluetoothDevice btDevice;
volatile private InputStream inStream;
volatile private OutputStream outStream;

private RazorListener razorListener;
private boolean callbacksEnabled = true;

BluetoothThread btThread;

private int numConnectAttempts;

// Object pools
ObjectPool<float[]> float3Pool = new ObjectPool<float[]>(new ObjectPool.ObjectFactory<float[]>() {
private ObjectPool<float[]> float3Pool = new ObjectPool<float[]>(new ObjectPool.ObjectFactory<float[]>() {
@Override
public float[] newObject() {
return new float[3];
}
});
private ObjectPool<float[]> float9Pool = new ObjectPool<float[]>(new ObjectPool.ObjectFactory<float[]>() {
@Override
public float[] newObject() {
return new float[9];
}
});

/**
* Constructor.
* Must be called from the thread where you want receive the RazorListener callbacks! So if you
* want to manipulate Android UI from the callbacks you have to call this from your main/UI
* want to manipulate Android UI from the callbacks you have to call this from your main/UI
* thread.
*
*
* @param btDevice {@link android.bluetooth.BluetoothDevice BluetoothDevice} holding the Razor
* AHRS to connect to.
* @param razorListener {@link RazorListener} that will be notified of Razor AHRS events.
* @throws RuntimeException thrown if one of the parameters is null.
*/
public RazorAHRS(BluetoothDevice btDevice, RazorListener razorListener)
throws RuntimeException {
this(btDevice, razorListener, RazorOutputMode.YAW_PITCH_ROLL_ANGLES);
}

/**
* Constructor.
* Must be called from the thread where you want receive the RazorListener callbacks! So if you
* want to manipulate Android UI from the callbacks you have to call this from your main/UI
* thread.
*
* @param btDevice {@link android.bluetooth.BluetoothDevice BluetoothDevice} holding the Razor
* AHRS to connect to.
* @param razorListener {@link RazorListener} that will be notified of Razor AHRS events.
* @param razorOutputMode {@link RazorOutputMode} that you desire.
* @throws RuntimeException thrown if one of the parameters is null.
*/
public RazorAHRS(BluetoothDevice btDevice, RazorListener razorListener, RazorOutputMode razorOutputMode)
throws RuntimeException {
if (btDevice == null)
throw new RuntimeException("BluetoothDevice can not be null.");
this.btDevice = btDevice;

if (razorListener == null)
throw new RuntimeException("RazorListener can not be null.");
this.razorListener = razorListener;

if (razorOutputMode == null)
throw new RuntimeException("RazorMode can not be null.");
this.razorOutputMode = razorOutputMode;
}

/**
Expand All @@ -126,15 +167,15 @@ public RazorAHRS(BluetoothDevice btDevice, RazorListener razorListener)
public boolean getCallbacksEnabled() {
return callbacksEnabled;
}

/**
* Enables/disables listener callbacks.
* @param enabled
*/
public void setCallbacksEnabled(boolean enabled) {
callbacksEnabled = enabled;
}

/**
* Connect and start reading. Both is done asynchronously. {@link RazorListener#onConnectOk()}
* or {@link RazorListener#onConnectFail(IOException)} callbacks will be invoked.
Expand All @@ -151,7 +192,7 @@ public void asyncConnect(int numConnectAttempts) {
btThread.join();
} catch (InterruptedException e) { }
}

// Bluetooth thread not running any more, we're definitely in DISCONNECTED state now

// Create new thread to connect to Razor AHRS and read input
Expand All @@ -172,14 +213,14 @@ public void asyncDisconnect() {
// Don't go to USER_DISCONNECT_REQUEST state if we are disconnected already
if (connectionState == ConnectionState.DISCONNECTED)
return;

// This is a wanted disconnect, so we force (blocking) I/O to break
connectionState = ConnectionState.USER_DISCONNECT_REQUEST;
closeSocketAndStreams();
}
if (DEBUG) Log.d(TAG, "asyncDisconnect() END");
}

/**
* Writes out a string using ASCII encoding. Assumes we're connected. Does not handle
* exceptions itself.
Expand All @@ -201,7 +242,7 @@ private void closeSocketAndStreams() {
if (outStream != null)
write("#o0");
} catch (IOException e) { }

// Close Bluetooth socket => I/O operations immediately will throw exception
try {
if (btSocket != null)
Expand Down Expand Up @@ -248,6 +289,22 @@ private byte readByte() throws IOException {
return (byte) in;
}

/**
* Converts a buffer of bytes to an array of floats. This method does not do any error
* checking on parameter array sizes.
* @param byteBuf Byte buffer with length of at least <code>numFloats * 4</code>.
* @param floatArr Float array with length of at least <code>numFloats</code>.
* @param numFloats Number of floats to convert
*/
private void byteBufferToFloatArray(byte[] byteBuf, float[] floatArr, int numFloats) {
//int numFloats = byteBuf.length / 4;
for (int i = 0; i < numFloats * 4; i += 4) {
// Convert from little endian (Razor) to big endian (Java) and interpret as float
floatArr[i/4] = Float.intBitsToFloat((byteBuf[i] & 0xff) + ((byteBuf[i+1] & 0xff) << 8) +
((byteBuf[i+2] & 0xff) << 16) + ((byteBuf[i+3] & 0xff) << 24));
}
}

/**
* Parse input stream for given token.
* @param token Token to find
Expand Down Expand Up @@ -319,7 +376,7 @@ private void initRazor() throws IOException {
write("#ob#o1#oe0#s" + configSynchID);
while (!readToken(configSynchReply, readByte())) { }
}

/**
* Opens Bluetooth connection to Razor AHRS.
* @throws IOException
Expand All @@ -331,7 +388,7 @@ private void connect() throws IOException {
if (DEBUG) Log.d(TAG, "btSocket is null in connect()");
throw new IOException("Could not create Bluetooth socket");
}

// This could be used to create the RFCOMM socekt on older Android devices where
//createRfcommSocketToServiceRecord is not present yet.
/*try {
Expand All @@ -340,13 +397,13 @@ private void connect() throws IOException {
} catch (Exception e) {
throw new IOException("Could not create Bluetooth socket using reflection");
}*/

// Connect socket to Razor AHRS
if (DEBUG) Log.d(TAG, "Canceling bt discovery");
BluetoothAdapter.getDefaultAdapter().cancelDiscovery(); // Recommended
if (DEBUG) Log.d(TAG, "Trying to connect() btSocket");
btSocket.connect();

// Get the input and output streams
if (DEBUG) Log.d(TAG, "Trying to create streams");
inStream = btSocket.getInputStream();
Expand All @@ -368,7 +425,7 @@ public void run() {
if (DEBUG) Log.d(TAG, "btDevice is null in run()");
throw new IOException("Bluetooth device is null");
}

// Make several attempts to connect
int i = 1;
while (true) {
Expand All @@ -382,17 +439,17 @@ public void run() {
// Maximum number of attempts reached or cancel requested?
if (i == numConnectAttempts || connectionState == ConnectionState.USER_DISCONNECT_REQUEST)
throw e;

// We couldn't connect on first try, manually starting Bluetooth discovery
// often helps
if (DEBUG) Log.d(TAG, "Starting BT discovery");
BluetoothAdapter.getDefaultAdapter().startDiscovery();
delay(5000); // 5 seconds - long enough?

i++;
}
}

// Set Razor output mode
if (DEBUG) Log.d(TAG, "Trying to set Razor output mode");
initRazor();
Expand All @@ -408,26 +465,35 @@ public void run() {

// Tell listener we're ready
sendToParentThread(MSG_ID__CONNECT_OK, null);

// Keep reading inStream until an exception occurs
if (DEBUG) Log.d(TAG, "Starting input loop");
while (true) {
// Read byte from input stream
inBuf[inBufPos++] = (byte) readByte();

if (inBufPos == 12) { // We received a full frame
float[] ypr = float3Pool.get();

// Convert from little endian (Razor) to big endian (Java) and interpret as float
ypr[0] = Float.intBitsToFloat((inBuf[0] & 0xff) + ((inBuf[1] & 0xff) << 8) + ((inBuf[2] & 0xff) << 16) + ((inBuf[3] & 0xff) << 24));
ypr[1] = Float.intBitsToFloat((inBuf[4] & 0xff) + ((inBuf[5] & 0xff) << 8) + ((inBuf[6] & 0xff) << 16) + ((inBuf[7] & 0xff) << 24));
ypr[2] = Float.intBitsToFloat((inBuf[8] & 0xff) + ((inBuf[9] & 0xff) << 8) + ((inBuf[10] & 0xff) << 16) + ((inBuf[11] & 0xff) << 24));

// Forward to parent thread handler
sendToParentThread(MSG_ID__YPR_DATA, ypr);

// Rewind input buffer position
inBufPos = 0;

if (razorOutputMode == RazorOutputMode.YAW_PITCH_ROLL_ANGLES) {
if (inBufPos == 12) { // We received a full frame
float[] ypr = float3Pool.get();
byteBufferToFloatArray(inBuf, ypr, 3);

// Forward to parent thread handler
sendToParentThread(MSG_ID__YPR_DATA, ypr);

// Rewind input buffer position
inBufPos = 0;
}
} else { // Raw or calibrated sensor data mode
if (inBufPos == 36) { // We received a full frame
float[] amg = float9Pool.get();
byteBufferToFloatArray(inBuf, amg, 9);

// Forward to parent thread handler
sendToParentThread(MSG_ID__AMG_DATA, amg);

// Rewind input buffer position
inBufPos = 0;
}
}
}
} catch (IOException e) {
Expand All @@ -446,13 +512,13 @@ public void run() {
} else {
// I/O error was caused on purpose, socket and streams are closed already
}

// I/O closed, thread done => we're disconnected now
connectionState = ConnectionState.DISCONNECTED;
}
}
}

/**
* Sends a message to Handler assigned to parent thread.
*
Expand All @@ -463,7 +529,7 @@ private void sendToParentThread(int msgId, Object o) {
if (callbacksEnabled)
parentThreadHandler.obtainMessage(msgId, o).sendToTarget();
}

/**
* Sends a message to Handler assigned to parent thread.
*
Expand All @@ -485,7 +551,7 @@ void delay(long ms) {
} catch (InterruptedException e) { }
}
}

/**
* Handler that forwards messages to the RazorListener callbacks. This handler runs in the
* thread this RazorAHRS object was created in and receives data from the Bluetooth I/O thread.
Expand All @@ -494,11 +560,17 @@ void delay(long ms) {
@Override
public void handleMessage(Message msg) {
switch (msg.what) {
case MSG_ID__YPR_DATA:
case MSG_ID__YPR_DATA: // Yaw, pitch and roll data
float[] ypr = (float[]) msg.obj;
razorListener.onAnglesUpdate(ypr[0], ypr[1], ypr[2]);
float3Pool.put(ypr);
break;
case MSG_ID__AMG_DATA: // Accelerometer, magnetometer and gyroscope data
float[] amg = (float[]) msg.obj;
razorListener.onSensorsUpdate(amg[0], amg[1], amg[2], amg[3], amg[4], amg[5],
amg[6], amg[7], amg[8]);
float9Pool.put(amg);
break;
case MSG_ID__IO_EXCEPTION_AND_DISCONNECT:
razorListener.onIOExceptionAndDisconnect((IOException) msg.obj);
break;
Expand Down
Loading

0 comments on commit 9708aa4

Please sign in to comment.