* 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 - * - * http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs! + * + * https://github.com/ptrbrtz/razor-9dof-ahrs! *
* The app using this class has to *
YAW_PITCH_ROLL_ANGLES to receive yaw, pitch and roll in degrees. RAW_SENSOR_DATA or CALIBRATED_SENSOR_DATA 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,
@@ -77,7 +91,7 @@ private enum ConnectionState {
USER_DISCONNECT_REQUEST
}
volatile private ConnectionState connectionState = ConnectionState.DISCONNECTED;
-
+
volatile private BluetoothSocket btSocket;
volatile private BluetoothDevice btDevice;
volatile private InputStream inStream;
@@ -85,25 +99,31 @@ private enum ConnectionState {
private RazorListener razorListener;
private boolean callbacksEnabled = true;
-
+
BluetoothThread btThread;
-
+
private int numConnectAttempts;
-
+
// Object pools
- ObjectPoolnumFloats * 4.
+ * @param floatArr Float array with length of at least numFloats.
+ * @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
@@ -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
@@ -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 {
@@ -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();
@@ -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) {
@@ -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();
@@ -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) {
@@ -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.
*
@@ -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.
*
@@ -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.
@@ -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;
diff --git a/Android/RazorAHRS/src/de/tuberlin/qu/razorahrs/RazorListener.java b/Android/RazorAHRS/src/de/tuberlin/qu/razorahrs/RazorListener.java
index 0914aee..ebe1772 100644
--- a/Android/RazorAHRS/src/de/tuberlin/qu/razorahrs/RazorListener.java
+++ b/Android/RazorAHRS/src/de/tuberlin/qu/razorahrs/RazorListener.java
@@ -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 (peter-bartz@gmx.de)
*
-* Infos, updates, bug reports and feedback:
+* Infos, updates, bug reports, contributions and feedback:
* https://github.com/ptrbrtz/razor-9dof-ahrs
******************************************************************************************/
@@ -22,7 +22,7 @@
* @author Peter Bartz
*/
public interface RazorListener {
-
+
/**
* Invoked when updated yaw/pitch/roll angles are available.
* @param yaw
@@ -30,35 +30,49 @@ public interface RazorListener {
* @param roll
*/
void onAnglesUpdate(float yaw, float pitch, float roll);
-
+
+ /**
+ * Invoked when updated accelerometer/magnetometer/gyroscope data is available.
+ * @param accX
+ * @param accY
+ * @param accZ
+ * @param magX
+ * @param magY
+ * @param magZ
+ * @param gyrX
+ * @param gyrY
+ * @param gyrZ
+ */
+ void onSensorsUpdate(float accX, float accY, float accZ, float magX, float magY, float magZ,
+ float gyrX, float gyrY, float gyrZ);
+
/**
* Invoked when an IOException occurred.
* RazorAHRS will be disconnected already.
- *
+ *
* @param e The IOException that was thrown
*/
void onIOExceptionAndDisconnect(IOException e);
-
+
/**
* Invoked when making an attempt to connect.Because connecting via Bluetooth often fails,
* multiple attempts can be made.
* See {@link RazorAHRS#RazorAHRS(android.bluetooth.BluetoothDevice, RazorListener, int)}.
- *
+ *
* @param attempt Current attempt
* @param maxAttempts Maximum number of attempts to be made
*/
void onConnectAttempt(int attempt, int maxAttempts);
-
+
/**
* Invoked when connecting completed successfully.
*/
void onConnectOk();
-
+
/**
* Invoked when connecting failed.
- *
+ *
* @param e Exception that was thrown
*/
void onConnectFail(Exception e);
-
}
diff --git a/Android/RazorExampleApp/.classpath b/Android/RazorExampleApp/.classpath
index 621a7bf..4160c21 100644
--- a/Android/RazorExampleApp/.classpath
+++ b/Android/RazorExampleApp/.classpath
@@ -1,9 +1,8 @@