android:glEsVersion="0x00020000"
android:required="true"/>
<uses-sdk
- android:minSdkVersion="18"
+ android:minSdkVersion="16"
android:targetSdkVersion="18" />
<!-- Application -->
</activity>
<activity android:name="Prefs" />
<service android:name="Task" />
+ <service android:name="Sensors" />
<service android:name="com.radiusnetworks.ibeacon.service.IBeaconService"
android:enabled="true"
android:exported="true"
android:layout_width="fill_parent"
android:layout_height="fill_parent">
<LinearLayout
- android:id="@+id/map"
+ android:id="@+id/state"
android:orientation="vertical"
android:layout_width="fill_parent"
android:layout_height="fill_parent">
- <fragment
- xmlns:android="http://schemas.android.com/apk/res/android"
- android:id="@+id/map_fragment"
+ <GridLayout
android:layout_width="match_parent"
- android:layout_height="match_parent"
- android:name="com.google.android.gms.maps.MapFragment"/>
+ android:layout_height="wrap_content"
+ android:columnCount="1" >
+ <TextView
+ android:id="@+id/accText1"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="1"
+ android:text="accValueX"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/accText2"
+ android:layout_column="0"
+ android:layout_gravity="center_horizontal|top"
+ android:layout_row="1"
+ android:text="accValueY"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/accText3"
+ android:layout_column="0"
+ android:layout_gravity="right|top"
+ android:layout_row="1"
+ android:text="accValueZ"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/textView1"
+ android:layout_width="wrap_content"
+ android:layout_height="wrap_content"
+ android:layout_column="0"
+ android:layout_gravity="left|center_vertical"
+ android:layout_row="0"
+ android:text="Accelerometer Info"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ </GridLayout>
+ <GridLayout
+ android:layout_width="match_parent"
+ android:layout_height="wrap_content"
+ android:columnCount="1"
+ android:paddingTop="10dp" >
+ <TextView
+ android:id="@+id/wrdAccText1"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="1"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/wrdAccText2"
+ android:layout_column="0"
+ android:layout_gravity="center_horizontal|top"
+ android:layout_row="1"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/wrdAccText3"
+ android:layout_column="0"
+ android:layout_gravity="right|top"
+ android:layout_row="1"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/textView4"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="0"
+ android:text="World Acceleration Info"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ </GridLayout>
+ <GridLayout
+ android:layout_width="match_parent"
+ android:layout_height="wrap_content"
+ android:columnCount="1"
+ android:paddingTop="10dp" >
+ <TextView
+ android:id="@+id/textView8"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="0"
+ android:text="Gyroscope Info"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/gyrText1"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="1"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/gyrText2"
+ android:layout_column="0"
+ android:layout_gravity="center_horizontal|top"
+ android:layout_row="1"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/gyrText3"
+ android:layout_column="0"
+ android:layout_gravity="right|top"
+ android:layout_row="1"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ </GridLayout>
+ <GridLayout
+ android:layout_width="match_parent"
+ android:layout_height="wrap_content"
+ android:columnCount="1"
+ android:paddingTop="10dp" >
+ <TextView
+ android:id="@+id/orientText1"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="2"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/orientText2"
+ android:layout_column="0"
+ android:layout_gravity="center_horizontal|top"
+ android:layout_row="2"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/orientText3"
+ android:layout_column="0"
+ android:layout_gravity="right|top"
+ android:layout_row="2"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/textView3"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="1"
+ android:text="Orientation Info"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ </GridLayout>
+ <GridLayout
+ android:layout_width="match_parent"
+ android:layout_height="wrap_content"
+ android:columnCount="1"
+ android:paddingTop="10dp" >
+ <TextView
+ android:id="@+id/textView5"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="0"
+ android:text="Step Count"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/stepCntText"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="1"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/calGrvText"
+ android:layout_width="147dp"
+ android:layout_column="0"
+ android:layout_gravity="right|top"
+ android:layout_row="1"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/textView6"
+ android:layout_column="0"
+ android:layout_gravity="right|bottom"
+ android:layout_row="0"
+ android:text="Calculated Gravity"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ </GridLayout>
+ <GridLayout
+ android:layout_width="match_parent"
+ android:layout_height="wrap_content"
+ android:columnCount="1"
+ android:paddingTop="10dp" >
+ <TextView
+ android:id="@+id/textView7"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="0"
+ android:text="Current Position"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/curPosXText"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="1"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/curPosYText"
+ android:layout_width="147dp"
+ android:layout_column="0"
+ android:layout_gravity="right|top"
+ android:layout_row="1"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ </GridLayout>
+ <GridLayout
+ android:layout_width="match_parent"
+ android:layout_height="wrap_content"
+ android:columnCount="1"
+ android:paddingTop="10dp" >
+ <TextView
+ android:id="@+id/textView9"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="1"
+ android:text="Current Heading"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/headingText"
+ android:layout_width="wrap_content"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="2"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/textView2"
+ android:layout_width="146dp"
+ android:layout_column="0"
+ android:layout_gravity="right|top"
+ android:layout_row="1"
+ android:text="World GyroZ"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/wrdGyrText1"
+ android:layout_width="146dp"
+ android:layout_column="0"
+ android:layout_gravity="right|top"
+ android:layout_row="2"
+ android:text="Medium Text"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ </GridLayout>
+ <GridLayout
+ android:layout_width="match_parent"
+ android:layout_height="wrap_content"
+ android:columnCount="1" >
+ <Button
+ android:id="@+id/rstHdBttn"
+ android:layout_width="108dp"
+ android:layout_height="wrap_content"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="0"
+ android:text="Heading Reset" />
+ <Button
+ android:id="@+id/rstDstBttn"
+ android:layout_width="96dp"
+ android:layout_column="0"
+ android:layout_gravity="center_horizontal|top"
+ android:layout_row="0"
+ android:text="Distance Reset" />
+ <TextView
+ android:id="@+id/stableText"
+ android:layout_width="84dp"
+ android:layout_column="0"
+ android:layout_gravity="right|center_vertical"
+ android:layout_row="0"
+ android:text="ifStable"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ </GridLayout>
</LinearLayout>
<LinearLayout
- android:id="@+id/state"
+ android:id="@+id/map"
android:orientation="vertical"
android:layout_width="fill_parent"
android:layout_height="fill_parent">
- <TextView
- android:id="@+id/state_text"
- android:layout_width="fill_parent"
- android:layout_height="wrap_content"
- android:text="State information" />
+ <fragment
+ xmlns:android="http://schemas.android.com/apk/res/android"
+ android:id="@+id/map_fragment"
+ android:layout_width="match_parent"
+ android:layout_height="match_parent"
+ android:name="com.google.android.gms.maps.MapFragment" />
</LinearLayout>
<ScrollView
android:id="@+id/debug_scroll"
package edu.ucla.iBeaconNav;
public class CMD {
- /* Main -> Task Messsages */
+ /* Task Commands */
static enum Command {
REGISTER,
CONNECT,
DISCONNECT,
- };
+ RSTHEAD,
+ RSTDST,
+ }
- /* Task -> Main messages */
+ /* Responses Commands */
static enum Response {
- POSITION,
+ REGISTER_TASK,
+ REGISTER_SENSORS,
NOTIFY,
- };
+ SHOWDATA,
+ }
+
+ /* Show Data Commands */
+ static enum Data {
+ ACC,
+ MAG,
+ GYR,
+ ORIENT,
+ WRDACC,
+ WRDGYR,
+ STPCNT,
+ POSITION,
+ HEADING,
+ STABLE,
+ }
}
private Handler handler;
private Messenger messenger;
private Task task;
+ private Sensors sensors;
private Toast toast;
/* Widgets */
private LinearLayout state;
private TextView debug;
private ScrollView scroll;
+ private Button rstHdBttn;
+ private Button rstDstBttn;
/* Private helper methods */
private void notice(String text)
/* Private handler methods */
private void onRegister(Task task)
{
- Util.debug("Main: onRegister");
+ Util.debug("Main: onRegister_task");
this.task = task;
}
+
+ private void onRegister(Sensors sensors)
+ {
+ Util.debug("Main: onRegister_sensors");
+ this.sensors = sensors;
+ }
private void onPosition()
{
this.toast.show();
}
+ private void onShowData(float[] data){
+ TextView textView1 = null;
+ TextView textView2 = null;
+ TextView textView3 = null;
+ int displayNum = 3;
+ int dataId = (int)(data[0]+0.5);
+ CMD.Data dataType = CMD.Data.values()[dataId];
+ switch (dataType){
+ case ACC:
+ textView1 = (TextView)findViewById(R.id.accText1);
+ textView2 = (TextView)findViewById(R.id.accText2);
+ textView3 = (TextView)findViewById(R.id.accText3);
+ break;
+ case MAG:
+ return;
+ /*textView1 = (TextView)findViewById(R.id.magText1);
+ textView2 = (TextView)findViewById(R.id.magText2);
+ textView3 = (TextView)findViewById(R.id.magText3);
+ break;*/
+ case GYR:
+ textView1 = (TextView)findViewById(R.id.gyrText1);
+ textView2 = (TextView)findViewById(R.id.gyrText2);
+ textView3 = (TextView)findViewById(R.id.gyrText3);
+ break;
+ case ORIENT:
+ textView1 = (TextView)findViewById(R.id.orientText1);
+ textView2 = (TextView)findViewById(R.id.orientText2);
+ textView3 = (TextView)findViewById(R.id.orientText3);
+ break;
+ case WRDACC:
+ textView1 = (TextView)findViewById(R.id.wrdAccText1);
+ textView2 = (TextView)findViewById(R.id.wrdAccText2);
+ textView3 = (TextView)findViewById(R.id.wrdAccText3);
+ break;
+ case STPCNT:
+ textView1 = (TextView)findViewById(R.id.stepCntText);
+ textView2 = (TextView)findViewById(R.id.calGrvText);
+ displayNum = 2;
+ break;
+ case POSITION:
+ textView1 = (TextView)findViewById(R.id.curPosXText);
+ textView2 = (TextView)findViewById(R.id.curPosYText);
+ displayNum = 2;
+ break;
+ case HEADING:
+ textView1 = (TextView)findViewById(R.id.headingText);
+ displayNum = 1;
+ break;
+ case WRDGYR:
+ textView1 = (TextView)findViewById(R.id.wrdGyrText1);
+ displayNum = 1;
+ break;
+ case STABLE:
+ textView1 = (TextView)findViewById(R.id.stableText);
+ displayNum = 1;
+ break;
+ default:
+ Util.debug("Main: Nothing Matches");
+ }
+
+ textView1.setText(Float.toString(data[1]));
+ if (displayNum >1){
+ textView2.setText(Float.toString(data[2]));
+ }
+ if (displayNum >2){
+ textView3.setText(Float.toString(data[3]));
+ }
+ }
+
/* Private service methods */
private void register()
{
startService(new Intent(this, Task.class)
.putExtra("Command", CMD.Command.REGISTER)
.putExtra("Messenger", this.messenger));
+ startService(new Intent(this, Sensors.class)
+ .putExtra("Command", CMD.Command.REGISTER)
+ .putExtra("Messenger", this.messenger));
}
private void connect()
Util.debug("Main: connect");
startService(new Intent(this, Task.class)
.putExtra("Command", CMD.Command.CONNECT));
+ startService(new Intent(this, Sensors.class)
+ .putExtra("Command", CMD.Command.CONNECT));
}
private void disconnect()
Util.debug("Main: disconnect");
startService(new Intent(this, Task.class)
.putExtra("Command", CMD.Command.DISCONNECT));
+ startService(new Intent(this, Sensors.class)
+ .putExtra("Command", CMD.Command.DISCONNECT));
}
private void quit()
this.state = (LinearLayout) findViewById(R.id.state);
this.debug = (TextView) findViewById(R.id.debug);
this.scroll = (ScrollView) findViewById(R.id.debug_scroll);
+ this.rstHdBttn = (Button) findViewById(R.id.rstHdBttn);
+ this.rstDstBttn= (Button) findViewById(R.id.rstDstBttn);
+
+ // TODO - remove these
+ rstHdBttn.setOnClickListener(new View.OnClickListener() {
+ @Override
+ public void onClick(View v) {
+ // TODO Auto-generated method stub
+ startService(new Intent(Main.this, Sensors.class)
+ .putExtra("Command", CMD.Command.RSTHEAD));
+ }
+ });
+
+ rstDstBttn.setOnClickListener(new View.OnClickListener() {
+ @Override
+ public void onClick(View v) {
+ // TODO Auto-generated method stub
+ startService(new Intent(Main.this, Sensors.class)
+ .putExtra("Command", CMD.Command.RSTDST));
+ }
+ });
// Get a handle to the Map Fragment
//GoogleMap map = ((MapFragment)getFragmentManager()
// Add window tabs
this.window.setup();
- this.window.addTab(this.window
- .newTabSpec("map")
- .setIndicator("Map")
- .setContent(R.id.map));
this.window.addTab(this.window
.newTabSpec("state")
.setIndicator("State")
.setContent(R.id.state));
+ this.window.addTab(this.window
+ .newTabSpec("map")
+ .setIndicator("Map")
+ .setContent(R.id.map));
this.window.addTab(this.window
.newTabSpec("debug")
.setIndicator("Debug")
/* Handler class */
class MainHandler extends Handler
{
+ @Override
public void handleMessage(android.os.Message msg)
{
CMD.Response resp = CMD.Response.values()[msg.what];
switch (resp) {
- case POSITION:
- Main.this.onPosition();
+ case REGISTER_TASK:
+ Main.this.onRegister((Task)msg.obj);
+ break;
+ case REGISTER_SENSORS:
+ Main.this.onRegister((Sensors)msg.obj);
break;
case NOTIFY:
Main.this.onNotify((String)msg.obj);
break;
+ case SHOWDATA:
+ Main.this.onShowData((float[])msg.obj);
+ break;
default:
Util.debug("Main: unknown message - " + resp);
break;
--- /dev/null
+package edu.ucla.iBeaconNav;
+
+class Matrix {
+ public float[] mValue = new float[9];
+
+ public Matrix(){
+ for (int i=0; i<9; i++){
+ mValue[i] = 0;
+ }
+ }
+
+ public Matrix(float[] m){
+ for (int i=0; i<9; i++){
+ mValue[i] = m[i];
+ }
+ }
+
+ public Matrix rotateX(float thetaX){
+ float[] rtX = {1, 0, 0,
+ 0, (float)Math.cos(thetaX),(float)-Math.sin(thetaX),
+ 0, (float)Math.sin(thetaX),(float) Math.cos(thetaX)};
+ return new Matrix(rtX);
+ }
+
+ public Matrix rotateY(float thetaY){
+ float[] rtY = {(float) Math.cos(thetaY), 0, (float)Math.sin(thetaY),
+ 0, 1, 0,
+ (float)-Math.sin(thetaY), 0, (float)Math.cos(thetaY)};
+ return new Matrix(rtY);
+ }
+
+ public Matrix rotateZ(float thetaZ){
+ float[] rtZ = {(float)Math.cos(thetaZ),(float)-Math.sin(thetaZ), 0,
+ (float)Math.sin(thetaZ),(float) Math.cos(thetaZ), 0,
+ 0, 0, 1};
+ return new Matrix(rtZ);
+ }
+
+ public Matrix multiple(Matrix m){
+ float[] result = new float[9];
+ for (int k1=0; k1<9; k1++){
+ int i = (int) (k1/3);
+ int j = k1%3;
+ result[k1] = 0;
+ for (int k2=0; k2<3; k2++){
+ result[k1]+=mValue[i*3+k2]*m.mValue[k2*3+j];
+ }
+
+ }
+ return new Matrix(result);
+ }
+
+ public float[] multipleV(float[] v){
+ float[] result = new float[3];
+ for(int i=0; i<3; i++){
+ result[i] = 0;
+ for(int j=0; j<3; j++){
+ result[i]+=mValue[3*i+j]*v[j];
+ }
+ }
+ return result;
+ }
+
+ public Matrix transpose(){
+ float[] result = new float[9];
+ result[0] = mValue[0];
+ result[1] = mValue[3];
+ result[2] = mValue[6];
+ result[4] = mValue[4];
+ result[3] = mValue[1];
+ result[5] = mValue[7];
+ result[6] = mValue[2];
+ result[7] = mValue[5];
+ result[8] = mValue[8];
+ return new Matrix(result);
+ }
+
+}
--- /dev/null
+package edu.ucla.iBeaconNav;
+
+import java.util.LinkedList;
+import java.util.Queue;
+
+import android.app.Notification;
+import android.app.Activity;
+import android.app.Service;
+import android.content.Context;
+import android.content.Intent;
+import android.hardware.Sensor;
+import android.hardware.SensorEvent;
+import android.hardware.SensorEventListener;
+import android.hardware.SensorManager;
+import android.os.Handler;
+import android.os.IBinder;
+import android.os.Message;
+import android.os.Messenger;
+import android.os.SystemClock;
+import android.widget.EditText;
+import edu.ucla.iBeaconNav.R;
+
+public class Sensors extends Service implements SensorEventListener
+{
+ /* Private data */
+ private SensorManager sensorManager;
+ private Sensor accSensor;
+ private Sensor grvSensor;
+ private Sensor magSensor;
+ private Sensor gyrSensor;
+
+ private Messenger messenger;
+ private boolean active;
+ private long lastTime_ms = 0;
+ private float snsInterval_ms = 0;
+ private long lastTime_ns = 0;
+ private long snsInterval_ns = 0;
+
+
+ /* Sensor data */
+ private float[] accValues = new float[3];
+ private float[] magValues = new float[3];
+ private float[] gyrValues = new float[3];
+ private float[] rotationMatrix_R = new float[9];
+ private float[] rtMatrixStable_R = new float[9];
+ private float[] rotationMatrix_I = new float[9];
+ private float[] orientValues_rd = new float[3];
+ private float[] orientValues = new float[3];
+ private float[] accWorldCoord = {0,0,0};
+ private float[] gyrWorldCoord = {0,0,0};
+
+
+ /* Auxiliary Variables for Sensor Processing */
+
+ private final int cycle = 32;
+
+ private LinkedList<Float> accBuffers = new LinkedList<Float>();
+ private LinkedList<Float> magBuffers = new LinkedList<Float>();
+ private LinkedList<Float> gyrBuffers = new LinkedList<Float>();
+ private LinkedList<Float> orientBuffers = new LinkedList<Float>();
+ private LinkedList<Float> rtMtrxBuffers = new LinkedList<Float>();
+ private float[] data = new float[4];
+ private float[] accSum = {0,0,0};
+ private float[] accAvg = {0,0,0};
+ private float[] magSum = {0,0,0};
+ private float[] gyrSum = {0,0,0};
+ private float[] gyrOffset = {0,0,0};
+ private float[] orientSum = {0,0,0};
+ private float[] rtMtrxSum = {0,0,0,
+ 0,0,0,
+ 0,0,0};
+ private float[] accBuffer = {0,0,0};
+ private float[] magBuffer = {0,0,0};
+ private float[] orientBuffer= {0,0,0};
+ private int cnt = 0;
+ private int accCnt = 0;
+ private int magCnt = 0;
+ private int gyrCnt = 0;
+ private int orientCnt = 0;
+ private int rtMtrxCnt = 0;
+
+ private boolean ifGyrOffset = false;
+ private boolean ifGrvOffset = false;
+ private boolean ifStable = false;
+
+ private final float EPSILON = (float)0.01;
+
+ private float accValueTotal = 0;
+ private float calculatedGravity = SensorManager.GRAVITY_EARTH;
+ private float gravityRef = 0;
+ private float gyroscopeRef = 0;
+ private boolean ifSetGrvRef = false;
+
+
+ /* Position Related Stuff */
+ //private float startPosX = 0;
+ //private float startPos = 0;
+ private float currentPosX = 0;
+ private float currentPosY = 0;
+ // rotate around gravity direction, positive when counterclock-wise, initially align with user's first step
+ private float currentHeading = 0;
+ private float headingOffset = 0;
+ private float stepLength = (float)0.5; // in m
+ private int stepCount = 0;
+ private boolean stepStart = false;
+
+
+
+ /* Private methods */
+ private void tellMain(CMD.Response cmd, Object value)
+ {
+ try {
+ android.os.Message msg = android.os.Message.obtain();
+ msg.what = cmd.ordinal();
+ msg.obj = value;
+ this.messenger.send(msg);
+ } catch (Exception e) {
+ Util.debug("Sensors: error sending message", e);
+ }
+ }
+
+ private void notify(String text, int icon)
+ {
+ // Notify Main
+ this.tellMain(CMD.Response.NOTIFY, text);
+
+ // Notification bar
+ //Notification note = new Notification(icon, null, 0);
+ //Intent intent = new Intent(this, Main.class);
+ //PendingIntent pend = PendingIntent.getActivity(this, 0, intent, 0);
+ //note.setLatestEventInfo(this, "iBeaconNav!", text, pend);
+ //PendingIntent pend = PendingIntent.getActivity(this, 0, intent, 0);
+
+ Notification note = new Notification.Builder(this)
+ .setContentTitle("iBeaconNav!")
+ .setContentText("iBeaconNav!")
+ .setSmallIcon(icon)
+ .build();
+
+ this.startForeground(1, note);
+ }
+
+ private void handle(CMD.Command cmd, Messenger mgr)
+ {
+ // Validate messenger
+ if (cmd != CMD.Command.REGISTER && mgr != null && mgr != this.messenger)
+ Util.debug("Sensors: handle - invalid messenger");
+
+ // Handle the command
+ switch (cmd) {
+ // Setup communication with Main
+ case REGISTER:
+ Util.debug("Sensors: handle - register");
+ this.messenger = mgr;
+ break;
+
+ // Create client thread
+ case CONNECT:
+ Util.debug("Sensors: handle - connect");
+ sensorManager.registerListener(this, accSensor, SensorManager.SENSOR_DELAY_FASTEST);
+ sensorManager.registerListener(this, grvSensor, SensorManager.SENSOR_DELAY_FASTEST);
+ sensorManager.registerListener(this, magSensor, SensorManager.SENSOR_DELAY_FASTEST);
+ sensorManager.registerListener(this, gyrSensor, SensorManager.SENSOR_DELAY_FASTEST);
+ break;
+
+ // Stop client thread
+ case DISCONNECT:
+ Util.debug("Sensors: handle - register");
+ // TODO
+ break;
+
+ // Reset heading
+ case RSTHEAD:
+ Util.debug("Sensors: handle - reset heading");
+ currentHeading = 0;
+ displayData(CMD.Data.HEADING);
+ break;
+
+ // Reset distance
+ case RSTDST:
+ Util.debug("Sensors: handle - reset distance");
+ currentPosX = 0;
+ currentPosY = 0;
+ displayData(CMD.Data.POSITION);
+ break;
+ }
+ }
+
+ /* Public methods */
+ public boolean isRunning()
+ {
+ return true; // TODO
+ }
+
+ /* Service Methods */
+ @Override
+ public void onCreate()
+ {
+ Util.debug("Sensors: onCreate");
+ super.onCreate();
+ sensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
+ accSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
+ grvSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
+ magSensor = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
+ gyrSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
+ }
+
+ @Override
+ public void onDestroy()
+ {
+ Util.debug("Sensors: onDestroy");
+ //this.handle(CMD.Response.DISCONNECT, null);
+ }
+
+ @Override
+ public int onStartCommand(Intent intent, int flags, int startId)
+ {
+ Util.debug("Sensors: onStartCommand");
+ int rval = super.onStartCommand(intent, flags, startId);
+ CMD.Command cmd = (CMD.Command)intent.getExtras().get("Command");
+ Messenger mgr = (Messenger)intent.getExtras().get("Messenger");
+ this.handle(cmd, mgr);
+ return rval;
+ }
+
+ @Override
+ public IBinder onBind(Intent intent)
+ {
+ Util.debug("Sensors: onBind");
+ return messenger.getBinder();
+ }
+ class IncomingHandler extends Handler{
+ @Override
+ public void handleMessage(Message msg) {
+ //Util.debug("Sensors: MSG HANDLERRRRRRRRR");
+ super.handleMessage(msg);
+ }
+ }
+
+ @Override
+ public void onAccuracyChanged(Sensor sensor, int accuracy) {
+ // TODO Auto-generated method stub
+ Util.debug("Sensors: onAccuracyChanged");
+
+ }
+
+ @Override
+ public void onSensorChanged(SensorEvent event) {
+ // TODO Auto-generated method stub
+ switch(event.sensor.getType()){
+ case Sensor.TYPE_ACCELEROMETER:
+ accCnt++;
+ System.arraycopy(event.values, 0, accValues, 0, 3);
+ //accCnt = everyAveragedBuffer(accBuffer, accValues, 3, accCnt, cycle, CMD.Data.ACC);
+ forwardAveragedBuffer(accBuffers, accSum, accAvg, accValues, 3, accCnt, cycle, CMD.Data.ACC);
+ toWorldCoordinates(accWorldCoord, accValues, rotationMatrix_R, accCnt, CMD.Data.WRDACC);
+ accValueTotal = accWorldCoord[2];
+ if (ifSetGrvRef && !ifGrvOffset && Math.abs((accValueTotal-gravityRef)/gravityRef)<0.05){
+ rtMatrixStable_R = rotationMatrix_R;
+ calculatedGravity = accValueTotal;
+ ifGrvOffset = true;
+ ifGyrOffset = false;
+ }
+ else if(!ifSetGrvRef && accCnt>400){
+ ifSetGrvRef = true;
+ gravityRef = accValueTotal;
+ }
+ updateOrientation();
+ break;
+ case Sensor.TYPE_MAGNETIC_FIELD:
+ magCnt++;
+ System.arraycopy(event.values, 0, magValues, 0, 3);
+ //magCnt = everyAveragedBuffer(magBuffer, magValues, 3, magCnt, cycle, CMD.Data.MAG);
+ forwardAveragedBuffer(magBuffers, magSum, null, magValues, 3, magCnt, cycle, CMD.Data.MAG);
+ updateOrientation(); // This maybe useless
+ break;
+ case Sensor.TYPE_GYROSCOPE:
+ gyrCnt++;
+ float gyrTemp[] = new float[3];
+ long currentTime_ns = event.timestamp;
+ snsInterval_ns = currentTime_ns-lastTime_ns;
+ lastTime_ns = currentTime_ns;
+
+ int chkIntCnt = 8; //check Interval Count
+ if (gyrCnt%chkIntCnt==0){
+ //long currentTime_ms = SystemClock.elapsedRealtime();
+ //snsInterval_ms = (float)(currentTime_ms-lastTime_ms)/chkIntCnt;
+ if(ifSetGrvRef){
+ ifStable = isStable(accBuffers, 3, cycle/2) && isStable(gyrBuffers, 3, cycle/2);
+ }
+ if (ifStable){
+ ifGrvOffset = false;
+ }
+ //lastTime_ms = SystemClock.elapsedRealtime();
+ //Util.debug("Interal in ms: "+Float.toString(snsInterval_ms));
+ }
+
+ System.arraycopy(event.values, 0, gyrValues, 0, 3);
+ System.arraycopy(gyrValues, 0, gyrTemp, 0, 3);
+ forwardAveragedBuffer(gyrBuffers, gyrSum, null, gyrValues, 3, gyrCnt, 2*cycle, CMD.Data.GYR);
+ System.arraycopy(gyrTemp, 0, gyrValues, 0, 3);
+ if (!ifGyrOffset && ifGrvOffset){
+ //Util.debug("[GYR1] Reset "+Float.toString(gyrValues[2]));
+ gyrOffset[0] = gyrValues[0]; //x
+ gyrOffset[1] = gyrValues[1]; //y
+ gyrOffset[2] = gyrValues[2]; //z
+ ifGyrOffset = true;
+ }
+ depleteOffset(gyrValues, gyrOffset, 3);
+ //cycleFloatArray(gyrValues, 3);
+ toWorldCoordinates(gyrWorldCoord, gyrValues, rtMatrixStable_R, gyrCnt, CMD.Data.WRDGYR);
+
+ float gyrZ = gyrWorldCoord[2];
+ //if (!ifStable && ifGyrOffset && Math.abs(gyrZ)>0.05){
+ if (ifGyrOffset){
+ //currentHeading+=gyrZ*snsInterval_ms/1000*180/Math.PI;
+ currentHeading += gyrZ*snsInterval_ns/1000000000*180/Math.PI;
+ currentHeading%=360;
+ displayData(CMD.Data.HEADING);
+ }
+ break;
+
+
+ }
+ processSensorInfo();
+ }
+
+ private void cycleFloatArray(float[] array, int length){
+ float temp = array[length-1];
+ for(int i=1; i<length; i++){
+ array[i] = array[i-1];
+ }
+ array[0]=temp;
+ }
+
+ private void depleteOffset(float[] values, float[] offset, int len){
+ for(int i=0; i<len; i++){
+ values[i]-=offset[i];
+ }
+ }
+
+ private boolean isStable(LinkedList<Float> buffer, int length, int cycle){
+ int len = buffer.size();
+ float[] avrg = new float[length];
+ float[] devSum = new float[length];
+ float ttDev = 0;
+ for(int i=0; i<length; i++){
+ avrg[length-1-i]=0;
+ for (int j=0; j<cycle; j++){
+ avrg[length-1-i]+=buffer.get(len-1-j*length-i);
+ }
+ avrg[length-1-i]/=cycle;
+ }
+ for (int i=0; i<length; i++){
+ devSum[i] = 0;
+ for(int j=0; j<cycle; j++){
+ devSum[length-1-i]+=(float) Math.pow(buffer.get(len-1-j*length-i)-avrg[length-1-i],2);
+ }
+ devSum[length-1-i] = (float) Math.sqrt(devSum[length-1-i]/cycle);
+ }
+
+ for (int i=0; i<length; i++){
+ ttDev+=devSum[i];
+ }
+
+ if (ttDev>0.1){
+ //Util.debug("[DEV] unStable");
+ displayData(CMD.Data.STABLE);
+ return false;
+ }else{
+ //Util.debug("[DEV] Stable");
+ displayData(CMD.Data.STABLE);
+ return true;
+ }
+ }
+
+
+
+ private void processSensorInfo(){
+ displayData(CMD.Data.STPCNT);
+ float epsl = (float)0.6;
+ // on step count????
+ if (stepStart){
+ //judge if stop
+ if(accValueTotal-calculatedGravity>epsl){
+ stepStart = false;
+ stepCount++;
+ currentPosX += stepLength*Math.sin(currentHeading);
+ currentPosY += stepLength*Math.cos(currentHeading);
+ displayData(CMD.Data.POSITION);
+ }
+
+ }else{
+ //judge if start
+ if (calculatedGravity-accValueTotal>epsl ){
+ stepStart = true;
+ }
+ }
+ }
+
+
+
+ private void updateOrientation(){
+ if (accValues == null || magValues == null){
+ return;
+ }
+ float R[] = new float[9];
+ float I[] = new float[9];
+ boolean success;
+ success = SensorManager.getRotationMatrix(R, I, accValues, magValues);
+ if(success){
+ orientCnt++;
+ rtMtrxCnt++;
+ rotationMatrix_R = R;
+ rotationMatrix_I = I;
+ SensorManager.getOrientation(rotationMatrix_R, orientValues_rd);
+ }
+ // to degree
+ orientValues[0] = (float) (orientValues_rd[0]*180/Math.PI);
+ orientValues[1] = (float) (orientValues_rd[1]*180/Math.PI);
+ orientValues[2] = (float) (orientValues_rd[2]*180/Math.PI);
+
+ //orientCnt=everyAveragedBuffer(orientBuffer, orientValues, 3, orientCnt, 2*cycle, CMD.Data.ORIENT);
+ forwardAveragedBuffer(rtMtrxBuffers,rtMtrxSum, null, rotationMatrix_R, 9, rtMtrxCnt, 2*cycle, null);
+ forwardAveragedBuffer(orientBuffers,orientSum, null, orientValues, 3, orientCnt, 2*cycle, CMD.Data.ORIENT);
+ //displayData(CMD.Data.ORIENT);
+ }
+
+ private void toWorldCoordinates(float[] worldCoord, float[] values, float[] rotationMatrix, int cnt, CMD.Data cmd){
+ float result[] = new float[3];
+ result = new Matrix(rotationMatrix).multipleV(values);
+ worldCoord[0] = result[0];
+ worldCoord[1] = result[1];
+ worldCoord[2] = result[2];
+ if(cnt%32==0){
+ displayData(cmd);
+ }
+ }
+
+ private void displayData(CMD.Data dataType){
+ //Util.debug("Sensors: displayData");
+ float data[] = new float[4];
+ data[0] = dataType.ordinal();
+ switch(dataType) {
+ case ACC:
+ data[1] = accValues[0];
+ data[2] = accValues[1];
+ data[3] = accValues[2];
+ break;
+ case MAG:
+ data[1] = magValues[0];
+ data[2] = magValues[1];
+ data[3] = magValues[2];
+ break;
+ case ORIENT:
+ data[1] = orientValues[0];
+ data[2] = orientValues[1];
+ data[3] = orientValues[2];
+ break;
+ case WRDACC:
+ data[1] = accWorldCoord[0];
+ data[2] = accWorldCoord[1];
+ data[3] = accWorldCoord[2];
+ break;
+ case STPCNT:
+ data[1] = stepCount;
+ data[2] = calculatedGravity;
+ break;
+ case POSITION:
+ data[1] = currentPosX;
+ data[2] = currentPosY;
+ break;
+ case GYR:
+ data[1] = gyrValues[0];
+ data[2] = gyrValues[1];
+ data[3] = gyrValues[2];
+ break;
+ case HEADING:
+ data[1] = currentHeading;
+ break;
+ case WRDGYR:
+ data[1] = gyrWorldCoord[0];
+ break;
+ case STABLE:
+ data[1] = ifStable?1:0;
+ break;
+ default:
+ Util.debug("Bad Data Sending Command!");
+ return;
+ }
+
+ this.tellMain(CMD.Response.SHOWDATA, data);
+ }
+
+ private int everyAveragedBuffer(float[] buffer, float[] values, int length, int cnt, int cycle, CMD.Data cmd){
+ for(int i=0; i<length; i++){
+ buffer[i]+=values[i];
+ }
+ if (cnt == cycle){
+ cnt = 0;
+ for(int i=0; i<length; i++){
+ values[i] = buffer[i]/cycle;
+ buffer[i] = 0;
+ }
+ displayData(cmd);
+ }
+ return cnt;
+ }
+
+ private void forwardAveragedBuffer(LinkedList<Float> buffer, float[] sum, float[] avg,
+ float[] values, int length, int cnt, int cycle, CMD.Data cmd){
+ if (buffer==null||buffer.size()<cycle*length){
+ for(int i=0; i<length; i++){
+ buffer.addLast(values[i]);
+ sum[i]+=values[i];
+ values[i] = sum[i]/buffer.size();
+ }
+ }
+ else{
+ float[] discarded = new float[length];
+ for(int i=0; i<length; i++){
+ discarded[i]= buffer.removeFirst();
+ buffer.addLast(values[i]);
+ sum[i]-=discarded[i];
+ sum[i]+=values[i];
+ values[i]=sum[i]/cycle;
+ }
+ }
+ if(avg!=null){
+ for(int i=0; i<length; i++){
+ avg[i]=sum[i]/cycle;
+ }
+ }
+ if (cnt%32==0 && cmd != null){
+ displayData(cmd);
+ }
+ }
+
+ public void printMatrix(String s, Matrix m){
+ Util.debug("Sensor: ["+s+"] "+m.mValue[0]+" "+m.mValue[1]+" "+m.mValue[2]);
+ Util.debug("Sensor: " +m.mValue[3]+" "+m.mValue[4]+" "+m.mValue[5]);
+ Util.debug("Sensor: " +m.mValue[6]+" "+m.mValue[7]+" "+m.mValue[8]);
+ }
+
+ public void printVector(String s, float[] v){
+ Util.debug("Sensor: ["+s+"] "+v[0]+" "+v[1]+" "+v[2]);
+ }
+}
projects / ~andy / iBeaconNav / commitdiff