package system;
import java.util.ArrayList;
import java.util.List;
import actions.ActionUseCameraAngles2;
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.os.Handler;
import android.util.FloatMath;
/**
* @author Paul Smith code@uvwxy.de
*
*/
public class StepManager implements SensorEventListener {
private SensorManager sensorManager;
private List<OnStepListener> listeners;
private int minTimeBetweenSteps = 866;
private double minStepPeakSize = 0.9;
private double stepLengthInMeter = 0.9;
private Handler handler = new Handler();
private long handler_delay_millis = 1000 / 30;
boolean handler_is_running = false;
private float[] last_acc_events = { 0f, 0f, 0f };
private long last_step_ms;
private float orientation = 0.0f;
private static final int vhSize = 6;
private static final String LOG_TAG = "StepManager";
private float[][] stepDetecWindow = new float[vhSize][];
private int vhPointer = 0;
public interface OnStepListener {
public void onStep(double compassAngle, double steplength);
}
private void registerSensors(Context context) {
// register acceleraion sensor
sensorManager = (SensorManager) context
.getSystemService(Context.SENSOR_SERVICE);
Sensor magnetSensor = sensorManager
.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
sensorManager.registerListener(this, magnetSensor,
SensorManager.SENSOR_DELAY_GAME);
Sensor accelSensor = sensorManager
.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
sensorManager.registerListener(this, accelSensor,
SensorManager.SENSOR_DELAY_GAME);
}
private void start() {
handler_is_running = true;
handler.removeCallbacks(handlerStepDetection);
handler.postDelayed(handlerStepDetection, handler_delay_millis);
}
private void stop() {
handler.removeCallbacks(handlerStepDetection);
handler_is_running = false;
}
private void unregisterSensors() {
sensorManager.unregisterListener(this);
}
public void registerStepListener(Context context, OnStepListener l) {
if (listeners == null) {
listeners = new ArrayList<OnStepListener>();
registerSensors(context);
start();
}
listeners.add(l);
}
public void unRegisterStepListener(OnStepListener l) {
listeners.remove(l);
if (listeners.isEmpty()) {
stop();
unregisterSensors();
listeners = null;
}
}
private void addCurrentSensorData() {
stepDetecWindow[vhPointer % vhSize] = last_acc_events;
vhPointer++;
vhPointer = vhPointer % vhSize;
}
private boolean checkIfStepHappend() {
// Add value to values_history
int lookahead = 5;
for (int t = 1; t <= lookahead; t++) {
int x0 = (vhPointer - 1 - t + vhSize + vhSize) % vhSize;
int x1 = (vhPointer - 1 + vhSize) % vhSize;
if (stepDetecWindow[(vhPointer - 1 - t + vhSize + vhSize) % vhSize] != null) {
double check = FloatMath
.sqrt((stepDetecWindow[x0][0] - stepDetecWindow[x1][0])
* (stepDetecWindow[x0][0] - stepDetecWindow[x1][0])
+ (stepDetecWindow[x0][1] - stepDetecWindow[x1][1])
* (stepDetecWindow[x0][1] - stepDetecWindow[x1][1])
+ (stepDetecWindow[x0][2] - stepDetecWindow[x1][2])
* (stepDetecWindow[x0][2] - stepDetecWindow[x1][2]));
if (check >= minStepPeakSize) {
// Log.i(LOG_TAG, "Detected step with t = " + t +
// ", peakSize = " + minStepPeakSize + " < " + check);
return true;
}
}
}
return false;
}
// Handler code
/**
* Takes a location and updates its position according to the step
*
* @param l
* @param d
* @param bearing
* @return
*/
public static Location newLocationOneStepFurther(Location l, double d,
double bearing) {
bearing = Math.toRadians(bearing);
double R = 6378100; // m equatorial radius
double lat1 = Math.toRadians(l.getLatitude());
double lon1 = Math.toRadians(l.getLongitude());
double dr = d / R;
double lat2 = Math.asin(Math.sin(lat1) * Math.cos(dr) + Math.cos(lat1)
* Math.sin(dr) * Math.cos(bearing));
double lon2 = lon1
+ Math.atan2(
Math.sin(bearing) * Math.sin(d / R) * Math.cos(lat1),
Math.cos(d / R) - Math.sin(lat1) * Math.sin(lat2));
Location ret = new Location("LOCMOV");
ret.setLatitude(Math.toDegrees(lat2));
ret.setLongitude(Math.toDegrees(lon2));
ret.setAccuracy((float) (2.0f * d));
ret.setAltitude(0);
ret.setTime(System.currentTimeMillis());
return ret;
}
private Runnable handlerStepDetection = new Runnable() {
public void run() {
// if start is called twice: we have "two" threads, i.e clean this
handler.removeCallbacks(handlerStepDetection);
addCurrentSensorData();
long t = System.currentTimeMillis();
if (t - last_step_ms > minTimeBetweenSteps && checkIfStepHappend()) {
for (int i = 0; i < listeners.size(); i++) {
listeners.get(i).onStep(orientation, stepLengthInMeter);
}
last_step_ms = t;
}
// no movements if no steps/jumps detected at all, or not in the
// standing_timeout_ms intervall
if (handler_is_running)
handler.postDelayed(this, handler_delay_millis);
}
};
@Override
public void onAccuracyChanged(Sensor sensor, int accuracy) {
}
final float alpha = 0.8f;
float[] bufferedAccel = { .0f, .0f, .0f };
final float magnetoalpha = 0.8f;
float[] bufferedMagneto = { .0f, .0f, .0f };
ActionUseCameraAngles2 compassAzimuthCalcer = new ActionUseCameraAngles2() {
@Override
public void onAnglesUpdated(float pitch, float roll,
float compassAzimuth) {
orientation = compassAzimuth;
}
};
@Override
public void onSensorChanged(SensorEvent event) {
switch (event.sensor.getType()) {
case Sensor.TYPE_ACCELEROMETER:
bufferedAccel[0] = alpha * bufferedAccel[0] + (1 - alpha)
* event.values[0];
bufferedAccel[1] = alpha * bufferedAccel[1] + (1 - alpha)
* event.values[1];
bufferedAccel[2] = alpha * bufferedAccel[2] + (1 - alpha)
* event.values[2];
float x = event.values[0] - bufferedAccel[0];
float y = event.values[1] - bufferedAccel[1];
float z = event.values[2] - bufferedAccel[2];
float[] b = { x, y, z };
last_acc_events = b;
compassAzimuthCalcer.onAccelChanged(bufferedAccel);
break;
case Sensor.TYPE_MAGNETIC_FIELD:
bufferedMagneto[0] = magnetoalpha * bufferedMagneto[0]
+ (1 - magnetoalpha) * event.values[0];
bufferedMagneto[1] = magnetoalpha * bufferedMagneto[1]
+ (1 - magnetoalpha) * event.values[1];
bufferedMagneto[2] = magnetoalpha * bufferedMagneto[2]
+ (1 - magnetoalpha) * event.values[2];
compassAzimuthCalcer.onMagnetChanged(bufferedMagneto);
break;
}
}
public void setMinTimeBetweenSteps(int minTimeBetweenSteps) {
this.minTimeBetweenSteps = minTimeBetweenSteps;
}
public void setMinStepPeakSize(double minStepPeakSize) {
this.minStepPeakSize = minStepPeakSize;
}
public void setStepLengthInMeter(double stepLengthInMeter) {
this.stepLengthInMeter = stepLengthInMeter;
}
public int getMinTimeBetweenSteps() {
return minTimeBetweenSteps;
}
public double getMinStepPeakSize() {
return minStepPeakSize;
}
public double getStepLengthInMeter() {
return stepLengthInMeter;
}
}