source: other-projects/tipple-android/tipple-ar/tipple-standalone-hpg/src/org/greenstone/android/tipple/base/SensorFusion.java@ 26528

package org.greenstone.android.tipple.base;

// Based on "Android Sensor Fusion" by Paul Lawitzki
// http://www.thousand-thoughts.com/2012/03/android-sensor-fusion-tutorial/

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import java.util.Timer;
import java.util.TimerTask;

/**
 * Singleton which uses the gyroscope, accelerometer and compass to calculate the orientation.
 */
public class SensorFusion implements SensorEventListener {

    private static final float EPSILON = 1e-9f;
    private static final float NS2S = 1e-9f; // conversion from nanoseconds to seconds
    private static final int TIME_CONSTANT = 30; // how often to correct gyro drift
    private static final float FILTER_COEFFICIENT = 0.98f;
    private static final float GYRO_TOLERANCE = 0.5f; // error level at which gyro gets reset

    private static SensorFusion instance;
    private int clients = 0; // the number of activities who currently need sensor data

    private SensorManager mSensorManager = null;

    private float[] gyro = new float[3]; // angular speeds from gyro
    private float[] gyroMatrix = new float[9]; // rotation matrix from gyro data
    private float[] gyroOrientation = new float[3]; // orientation angles from gyro matrix

    private float[] magnet = new float[3]; // magnetic field vector
    private float[] accel = new float[3]; // accelerometer vector
    private float[] accMagOrientation = new float[3]; // orientation angles from accel and magnet
    private float[] fusedOrientation = new float[3]; // final orientation angles from sensor fusion
    private float[] accMagMatrix = new float[9]; // accel and magnet based rotation matrix
    private float gyroTimestamp;

    private Timer fuseTimer = new Timer();
    private TimerTask fuseTask = null;

    private SensorFusion(SensorManager sm) {
        mSensorManager = sm;
    }

    public static SensorFusion getInstance() {
        if (instance == null) {
            instance = new SensorFusion(
                    (SensorManager) Global.activity.getSystemService(Context.SENSOR_SERVICE));
        }
        return instance;
    }

    /**
     * Tells the SensorFusion object that there is someone who would like to know the orientation.
     * When there are no clients the SensorFusion object removes its event listeners.
     */
    public void addClient(String name) {
        if (clients++ == 0) {
            registerListeners();
        }
    }

    /**
     * Tells the SensorFusion object that someone no longer needs the orientation. When there are no
     * clients the SensorFusion object removes its event listeners.
     */
    public void removeClient(String name) {
        if (clients < 0) {
            throw new RuntimeException("SensorFusion: removed too many clients");
        }

        if (--clients == 0) {
            unregisterListeners();
        }
    }

    // Registers sensor listeners for the accelerometer, compass and gyroscope.
    // Creates a timer task to correct the gyro drift.
    private void registerListeners() {
        System.err.println("SensorFusion::registerListeners()");
        mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY),
                SensorManager.SENSOR_DELAY_FASTEST);

        mSensorManager.registerListener(this,
                mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD),
                SensorManager.SENSOR_DELAY_FASTEST);

        mSensorManager.registerListener(this,
                mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE),
                SensorManager.SENSOR_DELAY_NORMAL);

        fuseTask = new FuseTask();
        fuseTimer.scheduleAtFixedRate(fuseTask, 0, TIME_CONSTANT);
    }

    private void unregisterListeners() {
        System.err.println("SensorFusion::unregisterListeners()");
        mSensorManager.unregisterListener(this);
        fuseTask.cancel();
        fuseTask = null;
    }

    @Override
    public void onAccuracyChanged(Sensor sensor, int accuracy) {
    }

    @Override
    public void onSensorChanged(SensorEvent event) {
        switch (event.sensor.getType()) {
        case Sensor.TYPE_GRAVITY:
            // copy new accelerometer data into accel array and calculate orientation
            System.arraycopy(event.values, 0, accel, 0, 3);
            calculateAccMagOrientation();
            break;

        case Sensor.TYPE_MAGNETIC_FIELD:
            // copy new compass data into magnet array
            System.arraycopy(event.values, 0, magnet, 0, 3);
            if(ARActivity.directionss){
                ArDirection.setChanges(getBearingDegrees());
            }
            break;

        case Sensor.TYPE_GYROSCOPE:
            // process gyro data
            gyroFunction(event);
            break;
        }
    }

    // calculates orientation angles from accelerometer and compass output
    private void calculateAccMagOrientation() {
        if (SensorManager.getRotationMatrix(accMagMatrix, null, accel, magnet)) {
            SensorManager.getOrientation(accMagMatrix, accMagOrientation);
        }
    }

    // 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(float[] gyroValues, float[] deltaRotationVector,
            float timeFactor) {
        float[] normValues = new float[3];

        // Calculate the angular speed of the sample
        float omegaMagnitude = (float) Math.sqrt(gyroValues[0] * gyroValues[0] + gyroValues[1]
                * gyroValues[1] + gyroValues[2] * gyroValues[2]);

        // Normalize the rotation vector if it's big enough to get the axis
        if (omegaMagnitude > EPSILON) {
            normValues[0] = gyroValues[0] / omegaMagnitude;
            normValues[1] = gyroValues[1] / omegaMagnitude;
            normValues[2] = gyroValues[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.
        float thetaOverTwo = omegaMagnitude * timeFactor;
        float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
        float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);
        deltaRotationVector[0] = sinThetaOverTwo * normValues[0];
        deltaRotationVector[1] = sinThetaOverTwo * normValues[1];
        deltaRotationVector[2] = sinThetaOverTwo * normValues[2];
        deltaRotationVector[3] = cosThetaOverTwo;
    }

    // This function performs the integration of the gyroscope data.
    // It writes the gyroscope based orientation into gyroOrientation.
    private void gyroFunction(SensorEvent event) {
        // copy the new gyro values into the gyro array
        // convert the raw gyro data into a rotation vector
        float[] deltaVector = new float[4];
        if (gyroTimestamp != 0) {
            final float dT = (event.timestamp - gyroTimestamp) * NS2S;
            System.arraycopy(event.values, 0, gyro, 0, 3);
            getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);
        }

        // measurement done, save current time for next interval
        gyroTimestamp = event.timestamp;

        // convert rotation vector into rotation matrix
        float[] deltaMatrix = new float[9];
        SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);

        // apply the new rotation interval on the gyroscope based rotation matrix
        gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);

        // get the gyroscope based orientation from the rotation matrix
        SensorManager.getOrientation(gyroMatrix, gyroOrientation);
    }

    private float getGyroError() {
        float error = 0;
        // distance between the pairs of axes
        for (int i = 0; i < 9; i++) {
            error += (accMagMatrix[i] - gyroMatrix[i]) * (accMagMatrix[i] - gyroMatrix[i]);
        }
        return error;
    }

    private float filterAngle(float gyroAngle, float accMagAngle) {
        // corrects the gyro angle using the angle from the accelerometer/compass

        float diff = accMagAngle - gyroAngle;
        if (diff > Math.PI) {
            accMagAngle -= 2 * Math.PI;
        } else if (diff < -Math.PI) {
            accMagAngle += 2 * Math.PI;
        }

        float result = FILTER_COEFFICIENT * gyroAngle + (1 - FILTER_COEFFICIENT) * accMagAngle;

        if (result > Math.PI) {
            result -= 2 * Math.PI;
        } else if (result < -Math.PI) {
            result += 2 * Math.PI;
        }

        return result;
    }

    private float[] getRotationMatrixFromOrientation(float[] o) {
        float sinX = (float) Math.sin(o[1]);
        float cosX = (float) Math.cos(o[1]);
        float sinY = (float) Math.sin(o[2]);
        float cosY = (float) Math.cos(o[2]);
        float sinZ = (float) Math.sin(o[0]);
        float cosZ = (float) Math.cos(o[0]);

        float[] result = new float[] { cosY * cosZ - sinX * sinY * sinZ, cosX * sinZ,
                sinY * cosZ + sinX * cosY * sinZ, -cosY * sinZ - sinX * sinY * cosZ, cosX * cosZ,
                -sinY * sinZ + sinX * cosY * cosZ, -cosX * sinY, -sinX, cosX * cosY };

        return result;
    }

    private float[] matrixMultiplication(float[] A, float[] B) {
        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;
    }

    class FuseTask extends TimerTask {
        public void run() {
            
            for (int i = 0; i < 3; i++) {
                fusedOrientation[i] = filterAngle(gyroOrientation[i], accMagOrientation[i]);
            }

            // if the gyro is too far from the accel/magnet orientation, reset it
            if (getGyroError() > GYRO_TOLERANCE) {
                System.arraycopy(accMagOrientation, 0, fusedOrientation, 0, 3);
            }

            // overwrite gyro matrix and orientation with fused orientation
            // to compensate gyro drift
            gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
            System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
        }
    }

    public float[] getRotationMatrix() {
        return gyroMatrix;
    }

    /**
     * Converts a 3D location in world coordinates to phone coordinates
     */
    public float[] worldToPhone(float[] coord) {
        // gyroMatrix transforms phone coordinates to world coordinates. To convert the other way,
        // need to multiply by the inverse (which is the transpose for rotation matrices)
        return new float[] {
                coord[0] * gyroMatrix[0] + coord[1] * gyroMatrix[3] + coord[2] * gyroMatrix[6],
                coord[0] * gyroMatrix[1] + coord[1] * gyroMatrix[4] + coord[2] * gyroMatrix[7],
                coord[0] * gyroMatrix[2] + coord[1] * gyroMatrix[5] + coord[2] * gyroMatrix[8] };
    }

    public float getBearing() {
        return (float) fusedOrientation[0];
    }

    public float getBearingDegrees() {
        return (float) Math.toDegrees(getBearing());
    }

    /**
     * Returns the angle (in radians) the screen has been turned anticlockwise from landscape
     * position. e.g. 0 means the phone is landscape, -pi/2 means the phone is portrait.
     */
    public float getScreenAngle() {
        return (float) -Math.atan2(gyroMatrix[7], gyroMatrix[6]);
    }

    /**
     * Returns the angle (in degrees) the screen has been turned anticlockwise from landscape
     * position. e.g. 0 means the phone is landscape, -90 means the phone is portrait.
     */
    public float getScreenAngleDegrees() {
        return (float) Math.toDegrees(getScreenAngle());
    }
}