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
+public class Sensors implements SensorEventListener
{
/* Private data */
+ private Task task;
+
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 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[] accAvg = {0,0,0};
private float[] magSum = {0,0,0};
private float[] gyrSum = {0,0,0};
private float[] gyrOffset = {0,0,0};
private float gyroscopeRef = 0;
private boolean ifSetGrvRef = false;
-
/* Position Related Stuff */
//private float startPosX = 0;
//private float startPos = 0;
private boolean stepStart = false;
+ /* Integrated positions */
+ private Vect position = new Vect();
+ private Quat rotation = new Quat();
- /* 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;
- }
- }
+ /* Test data */
+ private long time1hz = 0;
+ private Vect gyr1hz = new Vect();
/* Public methods */
- public boolean isRunning()
+ public Sensors(Task task)
{
- return true; // TODO
+ Util.debug("Sensors: constructor");
+ this.task = task;
+ this.sensorManager = (SensorManager)task.getSystemService(Context.SENSOR_SERVICE);
+
+ // Get sensors
+ this.accSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
+ this.grvSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
+ this.magSensor = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
+ this.gyrSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
}
-
- /* 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);
+
+ public void connect() {
+ // Connect sensor listeners
+ 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);
}
- @Override
- public void onDestroy()
- {
- Util.debug("Sensors: onDestroy");
- //this.handle(CMD.Response.DISCONNECT, null);
+ public void disconnect() {
+ // Remove sensor listeners
+ sensorManager.unregisterListener(this, accSensor);
+ sensorManager.unregisterListener(this, grvSensor);
+ sensorManager.unregisterListener(this, magSensor);
+ sensorManager.unregisterListener(this, gyrSensor);
}
- @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;
+ public void reset_heading() {
+ Util.debug("Sensors: handle - reset heading");
+ this.rotation = new Quat();
+ currentHeading = 0;
+ displayData(CMD.Data.HEADING);
}
- @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);
- }
+ public void reset_distance() {
+ Util.debug("Sensors: handle - reset distance");
+ currentPosX = 0;
+ currentPosY = 0;
+ displayData(CMD.Data.POSITION);
}
+ /* Sensor Event Listener functions */
@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++;
currentHeading%=360;
displayData(CMD.Data.HEADING);
}
+
+ // Integrate rotations
+ Vect rpy = new Vect(event.values[1], event.values[0], -event.values[2]);
+ rpy.mul((double)snsInterval_ns/1E9);
+ Quat rot = new Quat(rpy.x, rpy.y, rpy.z);
+ this.rotation.mul(rot);
+ displayData(CMD.Data.ROTATION);
+
+ // Calculate 1 second gryo data
+ this.gyr1hz.add(rpy);
+ if (currentTime_ns > time1hz+1E9) {
+ displayData(CMD.Data.GYR1HZ);
+ this.gyr1hz.set(0, 0, 0);
+ time1hz = currentTime_ns;
+ }
+
break;
processSensorInfo();
}
+ /* Private helper functions */
private void cycleFloatArray(float[] array, int length){
float temp = array[length-1];
for(int i=1; i<length; i++){
}
}
-
-
private void processSensorInfo(){
displayData(CMD.Data.STPCNT);
float epsl = (float)0.6;
}
}
-
-
private void updateOrientation(){
if (accValues == null || magValues == null){
return;
data[1] = currentPosX;
data[2] = currentPosY;
break;
+ case ROTATION:
+ Vect rpy = new Vect();
+ this.rotation.get(rpy);
+ data[1] = (float)(rpy.x * (180/Math.PI));
+ data[2] = (float)(rpy.y * (180/Math.PI));
+ data[3] = (float)(rpy.z * (180/Math.PI));
+ break;
case GYR:
data[1] = gyrValues[0];
data[2] = gyrValues[1];
data[3] = gyrValues[2];
break;
+ case GYR1HZ:
+ data[1] = (float)(gyr1hz.x * (180/Math.PI) * 60 * 60);
+ data[2] = (float)(gyr1hz.y * (180/Math.PI) * 60 * 60);
+ data[3] = (float)(gyr1hz.z * (180/Math.PI) * 60 * 60);
+ break;
case HEADING:
data[1] = currentHeading;
break;
return;
}
- this.tellMain(CMD.Response.SHOWDATA, data);
+ this.task.tellMain(CMD.Response.SHOWDATA, data);
}
private int everyAveragedBuffer(float[] buffer, float[] values, int length, int cnt, int cycle, CMD.Data cmd){
}
}
- public void printMatrix(String s, Matrix m){
+ private 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){
+ private void printVector(String s, float[] v){
Util.debug("Sensor: ["+s+"] "+v[0]+" "+v[1]+" "+v[2]);
}
}
projects / ~andy / iBeaconNav / blobdiff