add gyro-based compass
This commit is contained in:
fkloft
parent
cd8186ffa3
commit
8142544442
@ -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);
|
||||
|
||||
@ -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<CompassListener> mListeners = new ArrayList<CompassListener>();
|
||||
|
||||
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);
|
||||
|
||||
361
mobile/src/com/cradle/iitc_mobile/compass/GyroCompass.java
Normal file
361
mobile/src/com/cradle/iitc_mobile/compass/GyroCompass.java
Normal file
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user