From 8142544442787d8d43836229fc431818c895015f Mon Sep 17 00:00:00 2001 From: fkloft Date: Thu, 22 Jan 2015 17:54:26 +0100 Subject: [PATCH] add gyro-based compass --- .../cradle/iitc_mobile/IITC_UserLocation.java | 3 +- .../cradle/iitc_mobile/compass/Compass.java | 23 +- .../iitc_mobile/compass/GyroCompass.java | 361 ++++++++++++++++++ 3 files changed, 381 insertions(+), 6 deletions(-) create mode 100644 mobile/src/com/cradle/iitc_mobile/compass/GyroCompass.java diff --git a/mobile/src/com/cradle/iitc_mobile/IITC_UserLocation.java b/mobile/src/com/cradle/iitc_mobile/IITC_UserLocation.java index 4907effc..f8b160c3 100644 --- a/mobile/src/com/cradle/iitc_mobile/IITC_UserLocation.java +++ b/mobile/src/com/cradle/iitc_mobile/IITC_UserLocation.java @@ -7,7 +7,6 @@ import android.location.LocationManager; import android.os.Bundle; import android.view.Surface; -import com.cradle.iitc_mobile.compass.AccMagCompass; import com.cradle.iitc_mobile.compass.Compass; import com.cradle.iitc_mobile.compass.CompassListener; @@ -28,7 +27,7 @@ public class IITC_UserLocation implements CompassListener, LocationListener { public IITC_UserLocation(final IITC_Mobile iitc) { mIitc = iitc; - mCompass = new AccMagCompass(mIitc); + mCompass = Compass.getDefaultCompass(mIitc); // Acquire a reference to the Location Manager and Sensor Manager mLocationManager = (LocationManager) iitc.getSystemService(Context.LOCATION_SERVICE); diff --git a/mobile/src/com/cradle/iitc_mobile/compass/Compass.java b/mobile/src/com/cradle/iitc_mobile/compass/Compass.java index b211ff9b..43070e6d 100644 --- a/mobile/src/com/cradle/iitc_mobile/compass/Compass.java +++ b/mobile/src/com/cradle/iitc_mobile/compass/Compass.java @@ -1,22 +1,37 @@ package com.cradle.iitc_mobile.compass; +import android.content.Context; +import android.hardware.Sensor; +import android.hardware.SensorManager; + import java.util.ArrayList; public abstract class Compass { + public static Compass getDefaultCompass(final Context context) { + final Sensor gyro = ((SensorManager) context.getSystemService(Context.SENSOR_SERVICE)) + .getDefaultSensor(Sensor.TYPE_GYROSCOPE); + + if (gyro != null) + return new GyroCompass(context); + else + return new AccMagCompass(context); + + } private final ArrayList mListeners = new ArrayList(); + private boolean mStarted = false; + protected abstract void onStart(); + + protected abstract void onStop(); + protected void publishOrientation(final float x, final float y, final float z) { for (final CompassListener listener : mListeners) listener.onCompassChanged(x, y, z); } - protected abstract void onStart(); - - protected abstract void onStop(); - public void registerListener(final CompassListener listener) { mListeners.add(listener); diff --git a/mobile/src/com/cradle/iitc_mobile/compass/GyroCompass.java b/mobile/src/com/cradle/iitc_mobile/compass/GyroCompass.java new file mode 100644 index 00000000..562b3038 --- /dev/null +++ b/mobile/src/com/cradle/iitc_mobile/compass/GyroCompass.java @@ -0,0 +1,361 @@ +/************************************************************************************ + * Copyright (c) 2012 Paul Lawitzki + * Permission is hereby granted, free of charge, to any person obtaining + * a copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * The above copyright notice and this permission notice shall be included + * in all copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. + * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, + * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, + * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE + * OR OTHER DEALINGS IN THE SOFTWARE. + ************************************************************************************/ + +package com.cradle.iitc_mobile.compass; + +import android.content.Context; +import android.hardware.Sensor; +import android.hardware.SensorEvent; +import android.hardware.SensorEventListener; +import android.hardware.SensorManager; +import android.os.Handler; + +import java.util.Timer; +import java.util.TimerTask; + +public class GyroCompass extends Compass +{ + private static final float EPSILON = 0.000000001f; + private static final float FILTER_COEFFICIENT = 0.98f; + private static final float NS2S = 1.0f / 1000000000.0f; + private static final int TIME_CONSTANT = 30; + + private final AccMagCompass mAccMagCompass; + private final AccMagListener mAccMagListener = new AccMagListener(); + // orientation angles from accel and magnet + private float[] mAccMagOrientation = null; + private final Context mContext; + // final orientation angles from sensor fusion + private final float[] mFusedOrientation = new float[3]; + private final Timer mFuseTimer = new Timer(); + // angular speeds from gyro + private final float[] mGyro = new float[3]; + // rotation matrix from gyro data + private float[] mGyroMatrix = null; + // orientation angles from gyro matrix + private final float[] mGyroOrientation = { 0, 0, 0 }; + private final Sensor mSensor; + + private final SensorListener mSensorListener = new SensorListener(); + private SensorManager mSensorManager = null; + private FuseOrientationTask mTask; + private long mTimestamp; + private final Runnable mUpdateRunnable = new Runnable() + { + @Override + public void run() + { + publishOrientation(mFusedOrientation[0], mFusedOrientation[1], mFusedOrientation[2]); + } + }; + + public GyroCompass(final Context context) + { + this(context, new AccMagCompass(context)); + } + + public GyroCompass(final Context context, final AccMagCompass compass) + { + super(); + + mContext = context; + mAccMagCompass = compass; + + // get sensorManager and initialise sensor listeners + mSensorManager = (SensorManager) mContext.getSystemService(Context.SENSOR_SERVICE); + mSensor = mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE); + } + + private float[] getRotationMatrixFromOrientation(final float[] o) + { + final float[] xM = new float[9]; + final float[] yM = new float[9]; + final float[] zM = new float[9]; + + final float sinX = (float) Math.sin(o[1]); + final float cosX = (float) Math.cos(o[1]); + final float sinY = (float) Math.sin(o[2]); + final float cosY = (float) Math.cos(o[2]); + final float sinZ = (float) Math.sin(o[0]); + final float cosZ = (float) Math.cos(o[0]); + + // rotation about x-axis (pitch) + xM[0] = 1.0f; + xM[1] = 0.0f; + xM[2] = 0.0f; + xM[3] = 0.0f; + xM[4] = cosX; + xM[5] = sinX; + xM[6] = 0.0f; + xM[7] = -sinX; + xM[8] = cosX; + + // rotation about y-axis (roll) + yM[0] = cosY; + yM[1] = 0.0f; + yM[2] = sinY; + yM[3] = 0.0f; + yM[4] = 1.0f; + yM[5] = 0.0f; + yM[6] = -sinY; + yM[7] = 0.0f; + yM[8] = cosY; + + // rotation about z-axis (azimuth) + zM[0] = cosZ; + zM[1] = sinZ; + zM[2] = 0.0f; + zM[3] = -sinZ; + zM[4] = cosZ; + zM[5] = 0.0f; + zM[6] = 0.0f; + zM[7] = 0.0f; + zM[8] = 1.0f; + + // rotation order is y, x, z (roll, pitch, azimuth) + float[] resultMatrix = matrixMultiplication(xM, yM); + resultMatrix = matrixMultiplication(zM, resultMatrix); + return resultMatrix; + } + + // This function is borrowed from the Android reference + // at http://developer.android.com/reference/android/hardware/SensorEvent.html#values + // It calculates a rotation vector from the gyroscope angular speed values. + private void getRotationVectorFromGyro(final float[] values, final float[] deltaRotationVector, final float time) + { + final float[] normValues = new float[3]; + + // Calculate the angular speed of the sample + final float omegaMagnitude = + (float) Math.sqrt(values[0] * values[0] + values[1] * values[1] + values[2] * values[2]); + + // Normalize the rotation vector if it's big enough to get the axis + if (omegaMagnitude > EPSILON) + { + normValues[0] = values[0] / omegaMagnitude; + normValues[1] = values[1] / omegaMagnitude; + normValues[2] = values[2] / omegaMagnitude; + } + + // Integrate around this axis with the angular speed by the timestep + // in order to get a delta rotation from this sample over the timestep + // We will convert this axis-angle representation of the delta rotation + // into a quaternion before turning it into the rotation matrix. + final float thetaOverTwo = omegaMagnitude * time; + final float sinThetaOverTwo = (float) Math.sin(thetaOverTwo); + final float cosThetaOverTwo = (float) Math.cos(thetaOverTwo); + deltaRotationVector[0] = sinThetaOverTwo * normValues[0]; + deltaRotationVector[1] = sinThetaOverTwo * normValues[1]; + deltaRotationVector[2] = sinThetaOverTwo * normValues[2]; + deltaRotationVector[3] = cosThetaOverTwo; + } + + private float[] matrixMultiplication(final float[] A, final float[] B) + { + final float[] result = new float[9]; + + result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6]; + result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7]; + result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8]; + + result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6]; + result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7]; + result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8]; + + result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6]; + result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7]; + result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8]; + + return result; + } + + // This function performs the integration of the gyroscope data. + // It writes the gyroscope based orientation into gyroOrientation. + private void onGyroChanged(final SensorEvent event) + { + // don't start until first accelerometer/magnetometer orientation has been acquired + if (mAccMagOrientation == null) + return; + + // initialisation of the gyroscope based rotation matrix + if (mGyroMatrix == null) + mGyroMatrix = getRotationMatrixFromOrientation(mAccMagOrientation); + + // copy the new gyro values into the gyro array + // convert the raw gyro data into a rotation vector + final float[] deltaVector = new float[4]; + if (mTimestamp != 0) + { + final float dT = (event.timestamp - mTimestamp) * NS2S; + System.arraycopy(event.values, 0, mGyro, 0, 3); + getRotationVectorFromGyro(mGyro, deltaVector, dT / 2.0f); + } + + // measurement done, save current time for next interval + mTimestamp = event.timestamp; + + // convert rotation vector into rotation matrix + final float[] deltaMatrix = new float[9]; + SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector); + + // apply the new rotation interval on the gyroscope based rotation matrix + mGyroMatrix = matrixMultiplication(mGyroMatrix, deltaMatrix); + + // get the gyroscope based orientation from the rotation matrix + SensorManager.getOrientation(mGyroMatrix, mGyroOrientation); + } + + @Override + protected void onStart() + { + // restore the sensor listeners when user resumes the application. + mSensorManager.registerListener(mSensorListener, mSensor, SensorManager.SENSOR_DELAY_UI); + mAccMagCompass.registerListener(mAccMagListener); + + mTask = new FuseOrientationTask(); + mFuseTimer.scheduleAtFixedRate(mTask, 200, TIME_CONSTANT); + } + + @Override + protected void onStop() + { + mSensorManager.unregisterListener(mSensorListener); + mAccMagCompass.unregisterListener(mAccMagListener); + mTask.cancel(); + } + + private class AccMagListener implements CompassListener + { + @Override + public void onCompassChanged(final float x, final float y, final float z) + { + if (mAccMagOrientation == null) + { + mGyroOrientation[0] = x; + mGyroOrientation[1] = y; + mGyroOrientation[2] = z; + } + mAccMagOrientation = new float[] { x, y, z }; + + } + } + + private class FuseOrientationTask extends TimerTask + { + private final Handler mHandler = new Handler(); + + @Override + public void run() + { + if (mAccMagOrientation == null) + return; + + final float oneMinusCoeff = 1.0f - FILTER_COEFFICIENT; + + /* + * Fix for 179° <--> -179° transition problem: + * Check whether one of the two orientation angles (gyro or accMag) is negative while the + * other one is positive. + * If so, add 360° (2 * math.PI) to the negative value, perform the sensor fusion, and remove + * the 360° from the result + * if it is greater than 180°. This stabilizes the output in positive-to-negative-transition + * cases. + */ + + // azimuth + if (mGyroOrientation[0] < -0.5 * Math.PI && mAccMagOrientation[0] > 0.0) + { + mFusedOrientation[0] = (float) (FILTER_COEFFICIENT * + (mGyroOrientation[0] + 2.0 * Math.PI) + oneMinusCoeff * mAccMagOrientation[0]); + mFusedOrientation[0] -= (mFusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0; + } + else if (mAccMagOrientation[0] < -0.5 * Math.PI && mGyroOrientation[0] > 0.0) + { + mFusedOrientation[0] = (float) (FILTER_COEFFICIENT * mGyroOrientation[0] + + oneMinusCoeff * (mAccMagOrientation[0] + 2.0 * Math.PI)); + mFusedOrientation[0] -= (mFusedOrientation[0] > Math.PI) ? 2.0 * Math.PI : 0; + } + else + { + mFusedOrientation[0] = FILTER_COEFFICIENT * mGyroOrientation[0] + + oneMinusCoeff * mAccMagOrientation[0]; + } + + // pitch + if (mGyroOrientation[1] < -0.5 * Math.PI && mAccMagOrientation[1] > 0.0) + { + mFusedOrientation[1] = (float) (FILTER_COEFFICIENT * + (mGyroOrientation[1] + 2.0 * Math.PI) + oneMinusCoeff * mAccMagOrientation[1]); + mFusedOrientation[1] -= (mFusedOrientation[1] > Math.PI) ? 2.0 * Math.PI : 0; + } + else if (mAccMagOrientation[1] < -0.5 * Math.PI && mGyroOrientation[1] > 0.0) + { + mFusedOrientation[1] = (float) (FILTER_COEFFICIENT * mGyroOrientation[1] + + oneMinusCoeff * (mAccMagOrientation[1] + 2.0 * Math.PI)); + mFusedOrientation[1] -= (mFusedOrientation[1] > Math.PI) ? 2.0 * Math.PI : 0; + } + else + { + mFusedOrientation[1] = FILTER_COEFFICIENT * mGyroOrientation[1] + + oneMinusCoeff * mAccMagOrientation[1]; + } + + // roll + if (mGyroOrientation[2] < -0.5 * Math.PI && mAccMagOrientation[2] > 0.0) + { + mFusedOrientation[2] = (float) (FILTER_COEFFICIENT * + (mGyroOrientation[2] + 2.0 * Math.PI) + oneMinusCoeff * mAccMagOrientation[2]); + mFusedOrientation[2] -= (mFusedOrientation[2] > Math.PI) ? 2.0 * Math.PI : 0; + } + else if (mAccMagOrientation[2] < -0.5 * Math.PI && mGyroOrientation[2] > 0.0) + { + mFusedOrientation[2] = (float) (FILTER_COEFFICIENT * mGyroOrientation[2] + + oneMinusCoeff * (mAccMagOrientation[2] + 2.0 * Math.PI)); + mFusedOrientation[2] -= (mFusedOrientation[2] > Math.PI) ? 2.0 * Math.PI : 0; + } + else + { + mFusedOrientation[2] = FILTER_COEFFICIENT * mGyroOrientation[2] + + oneMinusCoeff * mAccMagOrientation[2]; + } + + // overwrite gyro matrix and orientation with fused orientation + // to comensate gyro drift + mGyroMatrix = getRotationMatrixFromOrientation(mFusedOrientation); + System.arraycopy(mFusedOrientation, 0, mGyroOrientation, 0, 3); + + // update sensor output in GUI + mHandler.post(mUpdateRunnable); + } + } + + private class SensorListener implements SensorEventListener + { + @Override + public void onAccuracyChanged(final Sensor sensor, final int accuracy) + { + } + + @Override + public void onSensorChanged(final SensorEvent event) + { + onGyroChanged(event); + } + } +}