sensor based bearing for mapbox

AverageAngeTest.java

package ch.ymc.mapbox;

import junit.framework.TestCase;

/**
 * Created by nkuebler on 14/07/14.
 */
public class AverageAngleTest extends TestCase {

    public void testAverage0() {
        AverageAngle averageAngle = new AverageAngle(1);
        assertTrue(Double.isNaN(averageAngle.getAverage()));
    }

    public void testAverage1() {
        AverageAngle averageAngle = new AverageAngle(1);
        averageAngle.putValue(0.3);
        assertEquals(0.3, averageAngle.getAverage(), 0.0001);
    }

    public void testAverage2() {
        AverageAngle averageAngle = new AverageAngle(2);
        averageAngle.putValue(0.3);
        averageAngle.putValue(0.2);
        assertEquals(0.25, averageAngle.getAverage(), 0.0001);
    }

    public void testAverage2b() {
        AverageAngle averageAngle = new AverageAngle(2);
        averageAngle.putValue(-0.1);
        averageAngle.putValue(0.1);
        assertEquals(0.0, averageAngle.getAverage(), 0.0001);
    }

    public void testAverage2c() {
        AverageAngle averageAngle = new AverageAngle(2);
        averageAngle.putValue(Math.PI * 2 -0.1);
        averageAngle.putValue(0.1);
        assertEquals(0.0, averageAngle.getAverage(), 0.0001);
    }

    public void testAverage2d() {
        AverageAngle averageAngle = new AverageAngle(2);
        averageAngle.putValue(Math.PI * 2);
        averageAngle.putValue(0.0);
        assertEquals(0.0, averageAngle.getAverage(), 0.0001);
    }

    public void testAverage3() {
        AverageAngle averageAngle = new AverageAngle(3);
        averageAngle.putValue(Math.PI * 2 -0.1);
        averageAngle.putValue(0.1);
        averageAngle.putValue(0.1);
        assertEquals(0.0333, averageAngle.getAverage(), 0.001);
    }
}

AverageAngle.java

package ch.ymc.mapbox;

/**
 * Created by nkuebler on 14/07/14.
 */
public class AverageAngle {

    private double[] values;

    private int currentIndex;

    private int frames;

    private boolean isFull;

    private double averageValue = Double.NaN;

    public AverageAngle(int frames) {
        this.frames = frames;
        this.currentIndex = 0;
        this.values = new double[frames];
    }

    public void putValue(double d) {
        values[currentIndex] = d;
        if(currentIndex == frames - 1) {
            currentIndex = 0;
            isFull = true;
        } else {
            currentIndex++;
        }
        updateAverageValue();
    }

    public double getAverage() {
        return this.averageValue;
    }

    private void updateAverageValue() {
        int numberOfElementsToConsider = frames;
        if(!isFull) {
            numberOfElementsToConsider = currentIndex + 1;
        }

        if(numberOfElementsToConsider == 1) {
            this.averageValue = values[0];
            return;
        }

        // Formula: http://en.wikipedia.org/wiki/Circular_mean
        double sumSin = 0.0;
        double sumCos = 0.0;
        for(int i=0; i < numberOfElementsToConsider; i++) {
            double v = values[i];
            sumSin += Math.sin(v);
            sumCos += Math.cos(v);
        }
        this.averageValue = Math.atan2(sumSin, sumCos);
    }
}

GpsCompassLocationProvider.java

package ch.ymc.mapbox;

import android.content.Context;
import android.hardware.GeomagneticField;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.location.Location;

import com.mapbox.mapboxsdk.overlay.GpsLocationProvider;
import com.mapbox.mapboxsdk.overlay.UserLocationOverlay;

import java.lang.reflect.InvocationTargetException;
import java.lang.reflect.Method;

/**
 * Uses compass to provide more reactive bearing.
 *
 * Created by nkuebler on 02/06/14.
 */
public class GpsCompassLocationProvider extends GpsLocationProvider implements SensorEventListener {

    private static int THROTTLE_TIME = 50;
    private static double MIN_AZIMUTH_DIFF = 0.5;

    private Sensor sensorAccelerometer = null;
    private Sensor sensorMagneticField = null;
    private Sensor sensorOrientation = null;
    private final SensorManager sensorManager;
    private Context context;
    private float[] valuesAccelerometer;
    private float[] valuesMagneticField;
    private float[] valuesOrientation;

    private float[] matrixR;
    private float[] matrixI;
    private float[] matrixValues;

    private double pitch = Double.NaN;
    private double roll = Double.NaN;
    private double azimuth = Double.NaN;
    private double lastAzimuth= Double.NaN;
    private AverageAngle azimuthRadiants;

    private long lastChangeDispatchedAt = -1;

    private boolean useOrientationSensors = false;

    /**
     * Default constructor.
     *
     * @param context Application Context
     */
    public GpsCompassLocationProvider(Context context) {
        this(context, false, 10);
    }

    /**
     * @param context Application Context
     * @param useOrientationSensors set to true to use deprecated orientation sensor instead of
     *                              accelerometer/magnetic field sensors.
     * @param azimuthSmoothing the number of measurements used to calculate a mean for the azimuth. Set
     *                      this to 1 for the smallest delay. Setting it to 5-10 to prevents the
     *                      needle from going crazy
     */
    public GpsCompassLocationProvider(Context context, boolean useOrientationSensors, int azimuthSmoothing) {
        super(context);
        this.context = context;

        this.useOrientationSensors = useOrientationSensors;

        sensorManager = (SensorManager) context.getSystemService(Context.SENSOR_SERVICE);

        if (useOrientationSensors) {
            sensorOrientation = sensorManager.getDefaultSensor(Sensor.TYPE_ORIENTATION);
            valuesOrientation = new float[3];
        } else {
            sensorAccelerometer = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
            sensorMagneticField = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
            valuesAccelerometer = new float[3];
            valuesMagneticField = new float[3];

            matrixR = new float[9];
            matrixI = new float[9];
            matrixValues = new float[3];
        }
        
        azimuthRadiants = new AverageAngle(azimuthSmoothing);
    }

    @Override
    public boolean startLocationProvider(UserLocationOverlay myLocationConsumer) {
        boolean result = super.startLocationProvider(myLocationConsumer);

        if(result) {
            if (sensorOrientation != null) {
                sensorManager.registerListener(this,
                        sensorOrientation,
                        SensorManager.SENSOR_DELAY_UI);
            }

            if (sensorAccelerometer != null) {
                sensorManager.registerListener(this,
                        sensorAccelerometer,
                        SensorManager.SENSOR_DELAY_UI);
            }

            if (sensorMagneticField != null) {
                sensorManager.registerListener(this,
                        sensorMagneticField,
                        SensorManager.SENSOR_DELAY_UI);
            }
        }

        return result;
    }

    @Override
    public void stopLocationProvider() {
        super.stopLocationProvider();

        if (sensorAccelerometer != null) {
            sensorManager.unregisterListener(this,
                    sensorAccelerometer);
        }
        if (sensorMagneticField != null) {
            sensorManager.unregisterListener(this,
                    sensorMagneticField);
        }
        if (sensorOrientation != null) {
            sensorManager.unregisterListener(this,
                    sensorOrientation);
        }
    }

    @Override
    public void onSensorChanged(SensorEvent event) {

        switch (event.sensor.getType()) {
            case Sensor.TYPE_ACCELEROMETER:
                System.arraycopy(event.values, 0, valuesAccelerometer, 0, 3);
                break;
            case Sensor.TYPE_MAGNETIC_FIELD:
                System.arraycopy(event.values, 0, valuesMagneticField, 0, 3);
                break;
            case Sensor.TYPE_ORIENTATION:
                System.arraycopy(event.values, 0, valuesOrientation, 0, 3);
        }

        if (useOrientationSensors) {
            this.azimuthRadiants.putValue(Math.toRadians(valuesOrientation[0]));
            azimuth = Math.toDegrees(azimuthRadiants.getAverage());
        } else {
            boolean success = SensorManager.getRotationMatrix(
                    matrixR,
                    matrixI,
                    valuesAccelerometer,
                    valuesMagneticField);

            if (success) {
                SensorManager.getOrientation(matrixR, matrixValues);

                azimuthRadiants.putValue(matrixValues[0]);
                pitch = Math.toDegrees(matrixValues[1]);
                roll = Math.toDegrees(matrixValues[2]);
                azimuth = Math.toDegrees(azimuthRadiants.getAverage());
            }
        }

        // Throttle dispatching the changes a bit
        if(System.currentTimeMillis() - lastChangeDispatchedAt < THROTTLE_TIME) {
            return;
        }
        lastChangeDispatchedAt = System.currentTimeMillis();

        if (Double.isNaN(lastAzimuth) || Math.abs(lastAzimuth - azimuth) > MIN_AZIMUTH_DIFF) {
            lastAzimuth = azimuth;
            updateBearing();
        }
    }

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

    }

    @Override
    public void onLocationChanged(final Location location) {
        if (location != null && !Double.isNaN(azimuth)) {
            location.setBearing((float)getBearingForLocation(location));
        }
        super.onLocationChanged(location);
    }

    private void updateBearing() {
        Location location = getLastKnownLocation();
        if (location != null) {
            Location newLocation = new Location(location.getProvider());
            newLocation.setLongitude(location.getLongitude());
            newLocation.setLatitude(location.getLatitude());
            if(location.hasSpeed()) {
                newLocation.setAltitude(location.getSpeed());
            }
            if(location.hasAltitude()) {
                newLocation.setAltitude(location.getAltitude());
            }
            newLocation.setTime(System.currentTimeMillis());
            if (!Double.isNaN(azimuth)) {
                newLocation.setBearing((float)getBearingForLocation(location));
            }
            newLocation.setAccuracy(location.getAccuracy());

            try {
                Method locationJellyBeanFixMethod = Location.class.getMethod("makeComplete");
                locationJellyBeanFixMethod.invoke(newLocation);
            } catch (NoSuchMethodException e) {
                e.printStackTrace();
            } catch (IllegalAccessException e) {
                e.printStackTrace();
            } catch (InvocationTargetException e) {
                e.printStackTrace();
            }

            super.onLocationChanged(newLocation);
        }
    }

    private double getBearingForLocation(Location location) {
        return azimuth - getGeomagneticField(location).getDeclination();
    }

    private GeomagneticField getGeomagneticField(Location location) {
        GeomagneticField geomagneticField = new GeomagneticField(
                (float)location.getLatitude(),
                (float)location.getLongitude(),
                (float)location.getAltitude(),
                System.currentTimeMillis());
        return geomagneticField;
    }
}