This commit is contained in:
Jon Atkins 2015-01-22 20:27:08 +00:00
commit b54194efa4
8 changed files with 638 additions and 193 deletions

View File

@ -2,7 +2,8 @@
./build.py $* ./build.py $*
FORMAT=$(echo "\033[1;33m%w%f\033[0m written") FORMAT=$(echo "\033[1;33m%w%f\033[0m written")
while inotifywait -qre close_write --exclude "iitc-debug.user.js|.git*" --format "$FORMAT" . while inotifywait -qre close_write --format "$FORMAT" . @./build/ @./mobile/bin/ @./mobile/gen/ @./.git/
do do
./build.py $* ./build.py $*
done done

View File

@ -7,9 +7,6 @@
width: 32px; width: 32px;
transform-origin: center; transform-origin: center;
-webkit-transform-origin: center; -webkit-transform-origin: center;
transition: all 200ms linear;
-moz-transition: all 200ms linear;
-webkit-transition: all 200ms linear;
} }
.user-location .container .inner, .user-location .container .inner,

View File

@ -43,7 +43,6 @@ import android.widget.TextView;
import android.widget.Toast; import android.widget.Toast;
import com.cradle.iitc_mobile.IITC_NavigationHelper.Pane; import com.cradle.iitc_mobile.IITC_NavigationHelper.Pane;
import com.cradle.iitc_mobile.prefs.PluginPreferenceActivity;
import com.cradle.iitc_mobile.prefs.PreferenceActivity; import com.cradle.iitc_mobile.prefs.PreferenceActivity;
import com.cradle.iitc_mobile.share.ShareActivity; import com.cradle.iitc_mobile.share.ShareActivity;
@ -723,6 +722,7 @@ public class IITC_Mobile extends Activity
public void setLoadingState(final boolean isLoading) { public void setLoadingState(final boolean isLoading) {
mIsLoading = isLoading; mIsLoading = isLoading;
mNavigationHelper.onLoadingStateChanged(); mNavigationHelper.onLoadingStateChanged();
mUserLocation.onLoadingStateChanged();
invalidateOptionsMenu(); invalidateOptionsMenu();
updateViews(); updateViews();
if (!isLoading) mFileManager.updatePlugins(false); if (!isLoading) mFileManager.updatePlugins(false);

View File

@ -1,40 +1,42 @@
package com.cradle.iitc_mobile; package com.cradle.iitc_mobile;
import android.content.Context; import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.location.Location; import android.location.Location;
import android.location.LocationListener; import android.location.LocationListener;
import android.location.LocationManager; import android.location.LocationManager;
import android.os.Bundle; import android.os.Bundle;
import android.view.Surface; import android.view.Surface;
public class IITC_UserLocation implements LocationListener, SensorEventListener { import com.cradle.iitc_mobile.compass.Compass;
private static final double SENSOR_DELAY_USER = 100 * 1e6; // 100 milliseconds import com.cradle.iitc_mobile.compass.CompassListener;
private int mMode = 0;
private boolean mRunning = false;
private boolean mLocationRegistered = false;
private boolean mOrientationRegistered = false;
private long mLastUpdate = 0;
private IITC_Mobile mIitc;
private Location mLastLocation = null;
private LocationManager mLocationManager;
private Sensor mSensorAccelerometer, mSensorMagnetometer;
private SensorManager mSensorManager = null;
private float[] mValuesGravity = null, mValuesGeomagnetic = null;
private double mOrientation = 0;
private boolean mFollowing = false;
public IITC_UserLocation(IITC_Mobile iitc) { public class IITC_UserLocation implements CompassListener, LocationListener {
private static final int TWO_MINUTES = 1000 * 60 * 2;
private final Compass mCompass;
private boolean mFollowing = false;
private final IITC_Mobile mIitc;
private Location mLastLocation = null;
private final LocationManager mLocationManager;
private boolean mLocationRegistered = false;
private int mMode = 0;
private double mOrientation = 0;
private boolean mOrientationRegistered = false;
private boolean mRunning = false;
public IITC_UserLocation(final IITC_Mobile iitc) {
mIitc = iitc; mIitc = iitc;
mCompass = Compass.getDefaultCompass(mIitc);
// Acquire a reference to the Location Manager and Sensor Manager // Acquire a reference to the Location Manager and Sensor Manager
mLocationManager = (LocationManager) iitc.getSystemService(Context.LOCATION_SERVICE); mLocationManager = (LocationManager) iitc.getSystemService(Context.LOCATION_SERVICE);
mSensorManager = (SensorManager) iitc.getSystemService(Context.SENSOR_SERVICE); }
mSensorAccelerometer = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
mSensorMagnetometer = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD); // Checks whether two providers are the same
private boolean isSameProvider(final String provider1, final String provider2) {
if (provider1 == null) { return provider2 == null; }
return provider1.equals(provider2);
} }
private void setOrientation(Double orientation) { private void setOrientation(Double orientation) {
@ -47,8 +49,6 @@ public class IITC_UserLocation implements LocationListener, SensorEventListener
while (orientation > mOrientation + 180) while (orientation > mOrientation + 180)
orientation -= 360; orientation -= 360;
mOrientation = orientation; mOrientation = orientation;
} else {
mOrientation = 0;
} }
mIitc.getWebView().loadJS("if(window.plugin && window.plugin.userLocation)" mIitc.getWebView().loadJS("if(window.plugin && window.plugin.userLocation)"
@ -56,19 +56,19 @@ public class IITC_UserLocation implements LocationListener, SensorEventListener
} }
private void updateListeners() { private void updateListeners() {
boolean useLocation = mRunning && mMode != 0; final boolean useLocation = mRunning && mMode != 0 && !mIitc.isLoading();
boolean useOrientation = mRunning && mMode == 2; final boolean useOrientation = useLocation && mMode == 2;
if (useLocation && !mLocationRegistered) { if (useLocation && !mLocationRegistered) {
try { try {
mLocationManager.requestLocationUpdates(LocationManager.NETWORK_PROVIDER, 0, 0, this); mLocationManager.requestLocationUpdates(LocationManager.NETWORK_PROVIDER, 0, 0, this);
} catch (IllegalArgumentException e) { } catch (final IllegalArgumentException e) {
// if the given provider doesn't exist // if the given provider doesn't exist
Log.w(e); Log.w(e);
} }
try { try {
mLocationManager.requestLocationUpdates(LocationManager.GPS_PROVIDER, 0, 0, this); mLocationManager.requestLocationUpdates(LocationManager.GPS_PROVIDER, 0, 0, this);
} catch (IllegalArgumentException e) { } catch (final IllegalArgumentException e) {
// if the given provider doesn't exist // if the given provider doesn't exist
Log.w(e); Log.w(e);
} }
@ -79,18 +79,64 @@ public class IITC_UserLocation implements LocationListener, SensorEventListener
mLocationRegistered = false; mLocationRegistered = false;
} }
if (useOrientation && !mOrientationRegistered && mSensorAccelerometer != null && mSensorMagnetometer != null) { if (useOrientation && !mOrientationRegistered) {
mSensorManager.registerListener(this, mSensorAccelerometer, SensorManager.SENSOR_DELAY_NORMAL); mCompass.registerListener(this);
mSensorManager.registerListener(this, mSensorMagnetometer, SensorManager.SENSOR_DELAY_NORMAL);
mOrientationRegistered = true; mOrientationRegistered = true;
} }
if (!useOrientation && mOrientationRegistered && mSensorAccelerometer != null && mSensorMagnetometer != null) { if (!useOrientation && mOrientationRegistered) {
mSensorManager.unregisterListener(this, mSensorAccelerometer); mCompass.unregisterListener(this);
mSensorManager.unregisterListener(this, mSensorMagnetometer);
mOrientationRegistered = false; mOrientationRegistered = false;
} }
} }
/**
* Determines whether one Location reading is better than the current Location fix
*
* @param location
* The new Location that you want to evaluate
* @param currentBestLocation
* The current Location fix, to which you want to compare the new one
*
* code copied from http://developer.android.com/guide/topics/location/strategies.html#BestEstimate
*/
protected boolean isBetterLocation(final Location location, final Location currentBestLocation) {
if (currentBestLocation == null) {
// A new location is always better than no location
return true;
}
// Check whether the new location fix is newer or older
final long timeDelta = location.getTime() - currentBestLocation.getTime();
final boolean isSignificantlyNewer = timeDelta > TWO_MINUTES;
final boolean isSignificantlyOlder = timeDelta < -TWO_MINUTES;
final boolean isNewer = timeDelta > 0;
// If it's been more than two minutes since the current location, use the new location
// because the user has likely moved
if (isSignificantlyNewer) {
return true;
// If the new location is more than two minutes older, it must be worse
} else if (isSignificantlyOlder) { return false; }
// Check whether the new location fix is more or less accurate
final int accuracyDelta = (int) (location.getAccuracy() - currentBestLocation.getAccuracy());
final boolean isLessAccurate = accuracyDelta > 0;
final boolean isMoreAccurate = accuracyDelta < 0;
final boolean isSignificantlyLessAccurate = accuracyDelta > 100;
// Check if the old and new location are from the same provider
final boolean isFromSameProvider = isSameProvider(location.getProvider(),
currentBestLocation.getProvider());
// Determine location quality using a combination of timeliness and accuracy
if (isMoreAccurate) {
return true;
} else if (isNewer && !isLessAccurate) {
return true;
} else if (isNewer && !isSignificantlyLessAccurate && isFromSameProvider) { return true; }
return false;
}
public boolean hasCurrentLocation() { public boolean hasCurrentLocation() {
if (!mLocationRegistered) return false; if (!mLocationRegistered) return false;
return mLastLocation != null; return mLastLocation != null;
@ -104,7 +150,7 @@ public class IITC_UserLocation implements LocationListener, SensorEventListener
// do not touch the javascript while iitc boots // do not touch the javascript while iitc boots
if (mIitc.isLoading()) return; if (mIitc.isLoading()) return;
Location location = mLastLocation; final Location location = mLastLocation;
if (location == null) return; if (location == null) return;
mIitc.getWebView().loadJS("if(window.plugin && window.plugin.userLocation)" mIitc.getWebView().loadJS("if(window.plugin && window.plugin.userLocation)"
@ -113,110 +159,32 @@ public class IITC_UserLocation implements LocationListener, SensorEventListener
+ location.getAccuracy() + ", " + persistentZoom + ");"); + location.getAccuracy() + ", " + persistentZoom + ");");
} }
public void onStart() { @Override
mRunning = true; public void onCompassChanged(final float x, final float y, final float z) {
updateListeners(); double orientation = Math.toDegrees(x);
// in case we just switched from loc+sensor to loc-only, let javascript know final int rotation = mIitc.getWindowManager().getDefaultDisplay().getRotation();
if (mMode == 1) { switch (rotation) {
setOrientation(null); case Surface.ROTATION_90:
} orientation += 90;
break;
case Surface.ROTATION_180:
orientation += 180;
break;
case Surface.ROTATION_270:
orientation += 270;
break;
} }
public void onStop() { setOrientation(orientation);
mRunning = false; }
public void onLoadingStateChanged() {
updateListeners(); updateListeners();
} }
public void reset() {
setFollowMode(false);
}
public void setFollowMode(boolean follow) {
mFollowing = follow;
mIitc.invalidateOptionsMenu();
}
private static final int TWO_MINUTES = 1000 * 60 * 2;
/**
* Determines whether one Location reading is better than the current Location fix
* @param location The new Location that you want to evaluate
* @param currentBestLocation The current Location fix, to which you want to compare the new one
*
* code copied from http://developer.android.com/guide/topics/location/strategies.html#BestEstimate
*/
protected boolean isBetterLocation(Location location, Location currentBestLocation) {
if (currentBestLocation == null) {
// A new location is always better than no location
return true;
}
// Check whether the new location fix is newer or older
long timeDelta = location.getTime() - currentBestLocation.getTime();
boolean isSignificantlyNewer = timeDelta > TWO_MINUTES;
boolean isSignificantlyOlder = timeDelta < -TWO_MINUTES;
boolean isNewer = timeDelta > 0;
// If it's been more than two minutes since the current location, use the new location
// because the user has likely moved
if (isSignificantlyNewer) {
return true;
// If the new location is more than two minutes older, it must be worse
} else if (isSignificantlyOlder) {
return false;
}
// Check whether the new location fix is more or less accurate
int accuracyDelta = (int) (location.getAccuracy() - currentBestLocation.getAccuracy());
boolean isLessAccurate = accuracyDelta > 0;
boolean isMoreAccurate = accuracyDelta < 0;
boolean isSignificantlyLessAccurate = accuracyDelta > 100;
// Check if the old and new location are from the same provider
boolean isFromSameProvider = isSameProvider(location.getProvider(),
currentBestLocation.getProvider());
// Determine location quality using a combination of timeliness and accuracy
if (isMoreAccurate) {
return true;
} else if (isNewer && !isLessAccurate) {
return true;
} else if (isNewer && !isSignificantlyLessAccurate && isFromSameProvider) {
return true;
}
return false;
}
// Checks whether two providers are the same
private boolean isSameProvider(String provider1, String provider2) {
if (provider1 == null) {
return provider2 == null;
}
return provider1.equals(provider2);
}
/**
* set the location mode to use. Available modes:
* 0: don't show user's position
* 1: show user's position
* 2: show user's position and orientation
*
* @return whether a reload is needed to reflect the changes made to the preferences
*/
public boolean setLocationMode(int mode) {
boolean needsReload = (mode == 0 && mMode != 0) || (mode != 0 && mMode == 0);
mMode = mode;
return needsReload;
}
// ------------------------------------------------------------------------
// <interface LocationListener>
@Override @Override
public void onLocationChanged(Location location) { public void onLocationChanged(final Location location) {
if (!isBetterLocation(location, mLastLocation)) return; if (!isBetterLocation(location, mLastLocation)) return;
mLastLocation = location; mLastLocation = location;
@ -230,69 +198,54 @@ public class IITC_UserLocation implements LocationListener, SensorEventListener
} }
@Override @Override
public void onProviderDisabled(String provider) { public void onProviderDisabled(final String provider) {
} }
@Override @Override
public void onProviderEnabled(String provider) { public void onProviderEnabled(final String provider) {
}
public void onStart() {
mRunning = true;
updateListeners();
// in case we just switched from loc+sensor to loc-only, let javascript know
if (mMode == 1) {
setOrientation(null);
}
} }
@Override @Override
public void onStatusChanged(String provider, int status, Bundle extras) { public void onStatusChanged(final String provider, final int status, final Bundle extras) {
} }
// </interface LocationListener> public void onStop() {
mRunning = false;
// ------------------------------------------------------------------------ updateListeners();
// <interface SensorEventListener>
@Override
public void onAccuracyChanged(Sensor sensor, int accuracy) {
} }
@Override public void reset() {
public void onSensorChanged(SensorEvent event) { setFollowMode(false);
if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
mValuesGravity = event.values;
if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
mValuesGeomagnetic = event.values;
// save some battery, 10 updates per second should be enough
if ((event.timestamp - mLastUpdate) < SENSOR_DELAY_USER) return;
mLastUpdate = event.timestamp;
// do not touch the javascript while iitc boots
if (mIitc.isLoading()) return;
// wait until both sensors have given us an event
if (mValuesGravity == null || mValuesGeomagnetic == null) return;
float R[] = new float[9];
float I[] = new float[9];
float orientation[] = new float[3];
if (!SensorManager.getRotationMatrix(R, I, mValuesGravity, mValuesGeomagnetic)) return;
SensorManager.getOrientation(R, orientation);
double direction = orientation[0] / Math.PI * 180;
int rotation = mIitc.getWindowManager().getDefaultDisplay().getRotation();
switch (rotation) {
case Surface.ROTATION_90:
direction += 90;
break;
case Surface.ROTATION_180:
direction += 180;
break;
case Surface.ROTATION_270:
direction += 270;
break;
} }
setOrientation(direction); public void setFollowMode(final boolean follow) {
mFollowing = follow;
mIitc.invalidateOptionsMenu();
} }
// </interface SensorEventListener> /**
* set the location mode to use. Available modes:
* 0: don't show user's position
* 1: show user's position
* 2: show user's position and orientation
*
* @return whether a reload is needed to reflect the changes made to the preferences
*/
public boolean setLocationMode(final int mode) {
final boolean needsReload = (mode == 0 && mMode != 0) || (mode != 0 && mMode == 0);
mMode = mode;
return needsReload;
}
} }

View File

@ -0,0 +1,74 @@
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;
public class AccMagCompass extends Compass {
private static final double SENSOR_DELAY_USER = 100 * 1e6; // 100 milliseconds
private final Context mContext;
private long mLastUpdate = 0;
private final SensorListener mListener = new SensorListener();
private final float[] mOrientation = new float[3];
private final float[] mRotationMatrix = new float[9];
private final Sensor mSensorAcc, mSensorMag;
private final SensorManager mSensorManager;
private float[] mValuesAcc = null, mValuesMag = null;
public AccMagCompass(final Context context) {
mContext = context;
mSensorManager = (SensorManager) mContext.getSystemService(Context.SENSOR_SERVICE);
mSensorAcc = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
mSensorMag = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
}
private void calculateOrientation() {
// wait until both sensors have given us an event
if (mValuesAcc == null || mValuesMag == null) return;
if (!SensorManager.getRotationMatrix(mRotationMatrix, null, mValuesAcc, mValuesMag)) return;
SensorManager.getOrientation(mRotationMatrix, mOrientation);
publishOrientation(mOrientation[0], mOrientation[1], mOrientation[2]);
}
@Override
protected void onStart() {
mSensorManager.registerListener(mListener, mSensorAcc, SensorManager.SENSOR_DELAY_NORMAL);
mSensorManager.registerListener(mListener, mSensorMag, SensorManager.SENSOR_DELAY_NORMAL);
}
@Override
protected void onStop() {
mSensorManager.unregisterListener(mListener);
}
private class SensorListener implements SensorEventListener {
@Override
public void onAccuracyChanged(final Sensor sensor, final int accuracy) {
}
@Override
public void onSensorChanged(final SensorEvent event) {
switch (event.sensor.getType()) {
case Sensor.TYPE_ACCELEROMETER:
mValuesAcc = event.values;
break;
case Sensor.TYPE_MAGNETIC_FIELD:
mValuesMag = event.values;
// save some battery, 10 updates per second should be enough
if ((event.timestamp - mLastUpdate) < SENSOR_DELAY_USER) break;
mLastUpdate = event.timestamp;
calculateOrientation();
}
}
}
}

View File

@ -0,0 +1,54 @@
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);
}
public void registerListener(final CompassListener listener)
{
mListeners.add(listener);
if (!mStarted)
{
onStart();
mStarted = true;
}
}
public void unregisterListener(final CompassListener listener)
{
mListeners.remove(listener);
if (mListeners.size() == 0)
{
onStop();
mStarted = false;
}
}
}

View File

@ -0,0 +1,5 @@
package com.cradle.iitc_mobile.compass;
public interface CompassListener {
public void onCompassChanged(float x, float y, float z);
}

View 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);
}
}
}