[26528] | 1 | package org.greenstone.android.tipple.base;
|
---|
| 2 |
|
---|
| 3 | // Based on "Android Sensor Fusion" by Paul Lawitzki
|
---|
| 4 | // http://www.thousand-thoughts.com/2012/03/android-sensor-fusion-tutorial/
|
---|
| 5 |
|
---|
| 6 | import android.content.Context;
|
---|
| 7 | import android.hardware.Sensor;
|
---|
| 8 | import android.hardware.SensorEvent;
|
---|
| 9 | import android.hardware.SensorEventListener;
|
---|
| 10 | import android.hardware.SensorManager;
|
---|
| 11 | import java.util.Timer;
|
---|
| 12 | import java.util.TimerTask;
|
---|
| 13 |
|
---|
| 14 | /**
|
---|
| 15 | * Singleton which uses the gyroscope, accelerometer and compass to calculate the orientation.
|
---|
| 16 | */
|
---|
| 17 | public class SensorFusion implements SensorEventListener {
|
---|
| 18 |
|
---|
| 19 | private static final float EPSILON = 1e-9f;
|
---|
| 20 | private static final float NS2S = 1e-9f; // conversion from nanoseconds to seconds
|
---|
| 21 | private static final int TIME_CONSTANT = 30; // how often to correct gyro drift
|
---|
| 22 | private static final float FILTER_COEFFICIENT = 0.98f;
|
---|
| 23 | private static final float GYRO_TOLERANCE = 0.5f; // error level at which gyro gets reset
|
---|
| 24 |
|
---|
| 25 | private static SensorFusion instance;
|
---|
| 26 | private int clients = 0; // the number of activities who currently need sensor data
|
---|
| 27 |
|
---|
| 28 | private SensorManager mSensorManager = null;
|
---|
| 29 |
|
---|
| 30 | private float[] gyro = new float[3]; // angular speeds from gyro
|
---|
| 31 | private float[] gyroMatrix = new float[9]; // rotation matrix from gyro data
|
---|
| 32 | private float[] gyroOrientation = new float[3]; // orientation angles from gyro matrix
|
---|
| 33 |
|
---|
| 34 | private float[] magnet = new float[3]; // magnetic field vector
|
---|
| 35 | private float[] accel = new float[3]; // accelerometer vector
|
---|
| 36 | private float[] accMagOrientation = new float[3]; // orientation angles from accel and magnet
|
---|
| 37 | private float[] fusedOrientation = new float[3]; // final orientation angles from sensor fusion
|
---|
| 38 | private float[] accMagMatrix = new float[9]; // accel and magnet based rotation matrix
|
---|
| 39 | private float gyroTimestamp;
|
---|
| 40 |
|
---|
| 41 | private Timer fuseTimer = new Timer();
|
---|
| 42 | private TimerTask fuseTask = null;
|
---|
| 43 |
|
---|
| 44 | private SensorFusion(SensorManager sm) {
|
---|
| 45 | mSensorManager = sm;
|
---|
| 46 | }
|
---|
| 47 |
|
---|
| 48 | public static SensorFusion getInstance() {
|
---|
| 49 | if (instance == null) {
|
---|
| 50 | instance = new SensorFusion(
|
---|
| 51 | (SensorManager) Global.activity.getSystemService(Context.SENSOR_SERVICE));
|
---|
| 52 | }
|
---|
| 53 | return instance;
|
---|
| 54 | }
|
---|
| 55 |
|
---|
| 56 | /**
|
---|
| 57 | * Tells the SensorFusion object that there is someone who would like to know the orientation.
|
---|
| 58 | * When there are no clients the SensorFusion object removes its event listeners.
|
---|
| 59 | */
|
---|
| 60 | public void addClient(String name) {
|
---|
| 61 | if (clients++ == 0) {
|
---|
| 62 | registerListeners();
|
---|
| 63 | }
|
---|
| 64 | }
|
---|
| 65 |
|
---|
| 66 | /**
|
---|
| 67 | * Tells the SensorFusion object that someone no longer needs the orientation. When there are no
|
---|
| 68 | * clients the SensorFusion object removes its event listeners.
|
---|
| 69 | */
|
---|
| 70 | public void removeClient(String name) {
|
---|
| 71 | if (clients < 0) {
|
---|
| 72 | throw new RuntimeException("SensorFusion: removed too many clients");
|
---|
| 73 | }
|
---|
| 74 |
|
---|
| 75 | if (--clients == 0) {
|
---|
| 76 | unregisterListeners();
|
---|
| 77 | }
|
---|
| 78 | }
|
---|
| 79 |
|
---|
| 80 | // Registers sensor listeners for the accelerometer, compass and gyroscope.
|
---|
| 81 | // Creates a timer task to correct the gyro drift.
|
---|
| 82 | private void registerListeners() {
|
---|
| 83 | System.err.println("SensorFusion::registerListeners()");
|
---|
| 84 | mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY),
|
---|
| 85 | SensorManager.SENSOR_DELAY_FASTEST);
|
---|
| 86 |
|
---|
| 87 | mSensorManager.registerListener(this,
|
---|
| 88 | mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD),
|
---|
| 89 | SensorManager.SENSOR_DELAY_FASTEST);
|
---|
| 90 |
|
---|
| 91 | mSensorManager.registerListener(this,
|
---|
| 92 | mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE),
|
---|
| 93 | SensorManager.SENSOR_DELAY_NORMAL);
|
---|
| 94 |
|
---|
| 95 | fuseTask = new FuseTask();
|
---|
| 96 | fuseTimer.scheduleAtFixedRate(fuseTask, 0, TIME_CONSTANT);
|
---|
| 97 | }
|
---|
| 98 |
|
---|
| 99 | private void unregisterListeners() {
|
---|
| 100 | System.err.println("SensorFusion::unregisterListeners()");
|
---|
| 101 | mSensorManager.unregisterListener(this);
|
---|
| 102 | fuseTask.cancel();
|
---|
| 103 | fuseTask = null;
|
---|
| 104 | }
|
---|
| 105 |
|
---|
| 106 | @Override
|
---|
| 107 | public void onAccuracyChanged(Sensor sensor, int accuracy) {
|
---|
| 108 | }
|
---|
| 109 |
|
---|
| 110 | @Override
|
---|
| 111 | public void onSensorChanged(SensorEvent event) {
|
---|
| 112 | switch (event.sensor.getType()) {
|
---|
| 113 | case Sensor.TYPE_GRAVITY:
|
---|
| 114 | // copy new accelerometer data into accel array and calculate orientation
|
---|
| 115 | System.arraycopy(event.values, 0, accel, 0, 3);
|
---|
| 116 | calculateAccMagOrientation();
|
---|
| 117 | break;
|
---|
| 118 |
|
---|
| 119 | case Sensor.TYPE_MAGNETIC_FIELD:
|
---|
| 120 | // copy new compass data into magnet array
|
---|
| 121 | System.arraycopy(event.values, 0, magnet, 0, 3);
|
---|
| 122 | if(ARActivity.directionss){
|
---|
| 123 | ArDirection.setChanges(getBearingDegrees());
|
---|
| 124 | }
|
---|
| 125 | break;
|
---|
| 126 |
|
---|
| 127 | case Sensor.TYPE_GYROSCOPE:
|
---|
| 128 | // process gyro data
|
---|
| 129 | gyroFunction(event);
|
---|
| 130 | break;
|
---|
| 131 | }
|
---|
| 132 | }
|
---|
| 133 |
|
---|
| 134 | // calculates orientation angles from accelerometer and compass output
|
---|
| 135 | private void calculateAccMagOrientation() {
|
---|
| 136 | if (SensorManager.getRotationMatrix(accMagMatrix, null, accel, magnet)) {
|
---|
| 137 | SensorManager.getOrientation(accMagMatrix, accMagOrientation);
|
---|
| 138 | }
|
---|
| 139 | }
|
---|
| 140 |
|
---|
| 141 | // This function is borrowed from the Android reference
|
---|
| 142 | // at http://developer.android.com/reference/android/hardware/SensorEvent.html#values
|
---|
| 143 | // It calculates a rotation vector from the gyroscope angular speed values.
|
---|
| 144 | private void getRotationVectorFromGyro(float[] gyroValues, float[] deltaRotationVector,
|
---|
| 145 | float timeFactor) {
|
---|
| 146 | float[] normValues = new float[3];
|
---|
| 147 |
|
---|
| 148 | // Calculate the angular speed of the sample
|
---|
| 149 | float omegaMagnitude = (float) Math.sqrt(gyroValues[0] * gyroValues[0] + gyroValues[1]
|
---|
| 150 | * gyroValues[1] + gyroValues[2] * gyroValues[2]);
|
---|
| 151 |
|
---|
| 152 | // Normalize the rotation vector if it's big enough to get the axis
|
---|
| 153 | if (omegaMagnitude > EPSILON) {
|
---|
| 154 | normValues[0] = gyroValues[0] / omegaMagnitude;
|
---|
| 155 | normValues[1] = gyroValues[1] / omegaMagnitude;
|
---|
| 156 | normValues[2] = gyroValues[2] / omegaMagnitude;
|
---|
| 157 | }
|
---|
| 158 |
|
---|
| 159 | // Integrate around this axis with the angular speed by the timestep
|
---|
| 160 | // in order to get a delta rotation from this sample over the timestep
|
---|
| 161 | // We will convert this axis-angle representation of the delta rotation
|
---|
| 162 | // into a quaternion before turning it into the rotation matrix.
|
---|
| 163 | float thetaOverTwo = omegaMagnitude * timeFactor;
|
---|
| 164 | float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
|
---|
| 165 | float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);
|
---|
| 166 | deltaRotationVector[0] = sinThetaOverTwo * normValues[0];
|
---|
| 167 | deltaRotationVector[1] = sinThetaOverTwo * normValues[1];
|
---|
| 168 | deltaRotationVector[2] = sinThetaOverTwo * normValues[2];
|
---|
| 169 | deltaRotationVector[3] = cosThetaOverTwo;
|
---|
| 170 | }
|
---|
| 171 |
|
---|
| 172 | // This function performs the integration of the gyroscope data.
|
---|
| 173 | // It writes the gyroscope based orientation into gyroOrientation.
|
---|
| 174 | private void gyroFunction(SensorEvent event) {
|
---|
| 175 | // copy the new gyro values into the gyro array
|
---|
| 176 | // convert the raw gyro data into a rotation vector
|
---|
| 177 | float[] deltaVector = new float[4];
|
---|
| 178 | if (gyroTimestamp != 0) {
|
---|
| 179 | final float dT = (event.timestamp - gyroTimestamp) * NS2S;
|
---|
| 180 | System.arraycopy(event.values, 0, gyro, 0, 3);
|
---|
| 181 | getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);
|
---|
| 182 | }
|
---|
| 183 |
|
---|
| 184 | // measurement done, save current time for next interval
|
---|
| 185 | gyroTimestamp = event.timestamp;
|
---|
| 186 |
|
---|
| 187 | // convert rotation vector into rotation matrix
|
---|
| 188 | float[] deltaMatrix = new float[9];
|
---|
| 189 | SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);
|
---|
| 190 |
|
---|
| 191 | // apply the new rotation interval on the gyroscope based rotation matrix
|
---|
| 192 | gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);
|
---|
| 193 |
|
---|
| 194 | // get the gyroscope based orientation from the rotation matrix
|
---|
| 195 | SensorManager.getOrientation(gyroMatrix, gyroOrientation);
|
---|
| 196 | }
|
---|
| 197 |
|
---|
| 198 | private float getGyroError() {
|
---|
| 199 | float error = 0;
|
---|
| 200 | // distance between the pairs of axes
|
---|
| 201 | for (int i = 0; i < 9; i++) {
|
---|
| 202 | error += (accMagMatrix[i] - gyroMatrix[i]) * (accMagMatrix[i] - gyroMatrix[i]);
|
---|
| 203 | }
|
---|
| 204 | return error;
|
---|
| 205 | }
|
---|
| 206 |
|
---|
| 207 | private float filterAngle(float gyroAngle, float accMagAngle) {
|
---|
| 208 | // corrects the gyro angle using the angle from the accelerometer/compass
|
---|
| 209 |
|
---|
| 210 | float diff = accMagAngle - gyroAngle;
|
---|
| 211 | if (diff > Math.PI) {
|
---|
| 212 | accMagAngle -= 2 * Math.PI;
|
---|
| 213 | } else if (diff < -Math.PI) {
|
---|
| 214 | accMagAngle += 2 * Math.PI;
|
---|
| 215 | }
|
---|
| 216 |
|
---|
| 217 | float result = FILTER_COEFFICIENT * gyroAngle + (1 - FILTER_COEFFICIENT) * accMagAngle;
|
---|
| 218 |
|
---|
| 219 | if (result > Math.PI) {
|
---|
| 220 | result -= 2 * Math.PI;
|
---|
| 221 | } else if (result < -Math.PI) {
|
---|
| 222 | result += 2 * Math.PI;
|
---|
| 223 | }
|
---|
| 224 |
|
---|
| 225 | return result;
|
---|
| 226 | }
|
---|
| 227 |
|
---|
| 228 | private float[] getRotationMatrixFromOrientation(float[] o) {
|
---|
| 229 | float sinX = (float) Math.sin(o[1]);
|
---|
| 230 | float cosX = (float) Math.cos(o[1]);
|
---|
| 231 | float sinY = (float) Math.sin(o[2]);
|
---|
| 232 | float cosY = (float) Math.cos(o[2]);
|
---|
| 233 | float sinZ = (float) Math.sin(o[0]);
|
---|
| 234 | float cosZ = (float) Math.cos(o[0]);
|
---|
| 235 |
|
---|
| 236 | float[] result = new float[] { cosY * cosZ - sinX * sinY * sinZ, cosX * sinZ,
|
---|
| 237 | sinY * cosZ + sinX * cosY * sinZ, -cosY * sinZ - sinX * sinY * cosZ, cosX * cosZ,
|
---|
| 238 | -sinY * sinZ + sinX * cosY * cosZ, -cosX * sinY, -sinX, cosX * cosY };
|
---|
| 239 |
|
---|
| 240 | return result;
|
---|
| 241 | }
|
---|
| 242 |
|
---|
| 243 | private float[] matrixMultiplication(float[] A, float[] B) {
|
---|
| 244 | float[] result = new float[9];
|
---|
| 245 |
|
---|
| 246 | result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
|
---|
| 247 | result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
|
---|
| 248 | result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];
|
---|
| 249 |
|
---|
| 250 | result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
|
---|
| 251 | result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
|
---|
| 252 | result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];
|
---|
| 253 |
|
---|
| 254 | result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
|
---|
| 255 | result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
|
---|
| 256 | result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];
|
---|
| 257 |
|
---|
| 258 | return result;
|
---|
| 259 | }
|
---|
| 260 |
|
---|
| 261 | class FuseTask extends TimerTask {
|
---|
| 262 | public void run() {
|
---|
| 263 |
|
---|
| 264 | for (int i = 0; i < 3; i++) {
|
---|
| 265 | fusedOrientation[i] = filterAngle(gyroOrientation[i], accMagOrientation[i]);
|
---|
| 266 | }
|
---|
| 267 |
|
---|
| 268 | // if the gyro is too far from the accel/magnet orientation, reset it
|
---|
| 269 | if (getGyroError() > GYRO_TOLERANCE) {
|
---|
| 270 | System.arraycopy(accMagOrientation, 0, fusedOrientation, 0, 3);
|
---|
| 271 | }
|
---|
| 272 |
|
---|
| 273 | // overwrite gyro matrix and orientation with fused orientation
|
---|
| 274 | // to compensate gyro drift
|
---|
| 275 | gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
|
---|
| 276 | System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
|
---|
| 277 | }
|
---|
| 278 | }
|
---|
| 279 |
|
---|
| 280 | public float[] getRotationMatrix() {
|
---|
| 281 | return gyroMatrix;
|
---|
| 282 | }
|
---|
| 283 |
|
---|
| 284 | /**
|
---|
| 285 | * Converts a 3D location in world coordinates to phone coordinates
|
---|
| 286 | */
|
---|
| 287 | public float[] worldToPhone(float[] coord) {
|
---|
| 288 | // gyroMatrix transforms phone coordinates to world coordinates. To convert the other way,
|
---|
| 289 | // need to multiply by the inverse (which is the transpose for rotation matrices)
|
---|
| 290 | return new float[] {
|
---|
| 291 | coord[0] * gyroMatrix[0] + coord[1] * gyroMatrix[3] + coord[2] * gyroMatrix[6],
|
---|
| 292 | coord[0] * gyroMatrix[1] + coord[1] * gyroMatrix[4] + coord[2] * gyroMatrix[7],
|
---|
| 293 | coord[0] * gyroMatrix[2] + coord[1] * gyroMatrix[5] + coord[2] * gyroMatrix[8] };
|
---|
| 294 | }
|
---|
| 295 |
|
---|
| 296 | public float getBearing() {
|
---|
| 297 | return (float) fusedOrientation[0];
|
---|
| 298 | }
|
---|
| 299 |
|
---|
| 300 | public float getBearingDegrees() {
|
---|
| 301 | return (float) Math.toDegrees(getBearing());
|
---|
| 302 | }
|
---|
| 303 |
|
---|
| 304 | /**
|
---|
| 305 | * Returns the angle (in radians) the screen has been turned anticlockwise from landscape
|
---|
| 306 | * position. e.g. 0 means the phone is landscape, -pi/2 means the phone is portrait.
|
---|
| 307 | */
|
---|
| 308 | public float getScreenAngle() {
|
---|
| 309 | return (float) -Math.atan2(gyroMatrix[7], gyroMatrix[6]);
|
---|
| 310 | }
|
---|
| 311 |
|
---|
| 312 | /**
|
---|
| 313 | * Returns the angle (in degrees) the screen has been turned anticlockwise from landscape
|
---|
| 314 | * position. e.g. 0 means the phone is landscape, -90 means the phone is portrait.
|
---|
| 315 | */
|
---|
| 316 | public float getScreenAngleDegrees() {
|
---|
| 317 | return (float) Math.toDegrees(getScreenAngle());
|
---|
| 318 | }
|
---|
| 319 | }
|
---|