| 109 | 109 |
/** Hides Runnability, ensures singleton-ness */
|
| 110 | 110 |
private Runnable mRunner;
|
| 111 | 111 |
private static PersistentThread sThread;
|
| 112 | |
|
| 113 | |
/** Transformation matrix to from local frame to absolute **/
|
| 114 | |
private double[][] mTransform = new double[3][3];
|
| 115 | |
|
| 116 | |
/** Holds the timestamp of the last acceleration update **/
|
| 117 | |
private long mLastAccelUpdate = -1;
|
| 118 | |
|
| 119 | |
/** Waits through this many acceleratino updates before calculating velocities **/
|
| 120 | |
private int mAccCount = 0;
|
| 121 | |
|
| 122 | |
/** Holds velocity **/
|
| 123 | |
private double[] mVel = new double[3];
|
| 124 | 112 |
|
| 125 | 113 |
/**
|
| 126 | 114 |
* Initializes the locks, registers application context for runtime use.
|
|
| 445 | 433 |
public void onSensorChanged(SensorEvent event) {
|
| 446 | 434 |
int type = event.sensor.getType();
|
| 447 | 435 |
switch (type) {
|
| 448 | |
//Locks handled in updateField()
|
| 449 | 436 |
case Sensor.TYPE_ACCELEROMETER:
|
| 450 | |
|
| 451 | |
double t3 = getReadingField(AZIMUTH) * Math.PI / 180.0D;
|
| 452 | |
double t1 = getReadingField(PITCH) * Math.PI / 180.0D;
|
| 453 | |
double t2 = getReadingField(ROLL) * Math.PI / 180.0D;
|
| 454 | |
|
| 455 | |
double s1 = Math.sin(t1);
|
| 456 | |
double s2 = Math.sin(t2);
|
| 457 | |
double s3 = Math.sin(t3);
|
| 458 | |
|
| 459 | |
double c1 = Math.cos(t1);
|
| 460 | |
double c2 = Math.cos(t2);
|
| 461 | |
double c3 = Math.cos(t2);
|
| 462 | |
|
| 463 | |
//XYZ
|
| 464 | |
mTransform[0][0] = c2*c3;
|
| 465 | |
mTransform[1][0] = c1*s3 + c3*s1*s2;
|
| 466 | |
mTransform[2][0] = s1*s3 - c1*c3*s2;
|
| 467 | |
|
| 468 | |
mTransform[0][1] = -c2*s3;
|
| 469 | |
mTransform[1][1] = c1*c3 - s1*s2*s3;
|
| 470 | |
mTransform[2][1] = c1*s2*s3 + c3*s1;
|
| 471 | |
|
| 472 | |
mTransform[0][2] = s2;
|
| 473 | |
mTransform[1][2] = -c2*s1;
|
| 474 | |
mTransform[2][2] = c1*c2;
|
| 475 | |
|
| 476 | |
double[] newAcc = new double[3];
|
| 477 | |
newAcc[0] = 0;
|
| 478 | |
newAcc[1] = 0;
|
| 479 | |
newAcc[2] = -SensorManager.GRAVITY_EARTH;
|
| 480 | |
|
| 481 | |
for (int i = 0; i < 3; i++) {
|
| 482 | |
for (int j = 0; j < 3; j++) {
|
| 483 | |
newAcc[i] += (double)event.values[j] * mTransform[j][i];
|
| 484 | |
}
|
| 485 | |
Log.v(TAG, "ACC" + i + ": " + newAcc[i]);
|
| 486 | |
|
| 487 | |
}
|
| 488 | |
Log.v(TAG, "ACC: ");
|
| 489 | |
if (mAccCount < 300) {
|
| 490 | |
mAccCount++;
|
| 491 | |
Log.v(TAG,"MCOUNT is " + mAccCount);
|
| 492 | |
}
|
| 493 | |
else {
|
| 494 | |
if (mLastAccelUpdate==-1) {
|
| 495 | |
mLastAccelUpdate = event.timestamp;
|
| 496 | |
}
|
| 497 | |
else {
|
| 498 | |
for (int i = 0; i < 3; i++) {
|
| 499 | |
mVel[i] += ((double)(event.timestamp - mLastAccelUpdate))/1000000000.0 * newAcc[i];
|
| 500 | |
Log.v(TAG, "VEL" + i + ": " + mVel[i]);
|
| 501 | |
}
|
| 502 | |
Log.v(TAG, "VEL: ");
|
| 503 | |
mLastAccelUpdate = event.timestamp;
|
| 504 | |
}
|
| 505 | |
}
|
| 506 | |
/*
|
| 507 | |
double sumsqd = Math.sqrt(Math.pow(event.values[0], 2.0) +
|
| 508 | |
Math.pow(event.values[1], 2.0) +
|
| 509 | |
Math.pow(event.values[2], 2.0)) -
|
| 510 | |
Math.sqrt(Math.pow(newAcc[0], 2.0) +
|
| 511 | |
Math.pow(newAcc[1], 2.0) +
|
| 512 | |
Math.pow(newAcc[2], 2.0));
|
| 513 | |
*/
|
| 514 | |
//Log.v(TAG, "XACC: " + newAcc[0]);
|
| 515 | |
//Log.v(TAG, "YACC: " + newAcc[1]);
|
| 516 | |
//Log.v(TAG, "ZACC: " + newAcc[2]);
|
| 517 | |
//Log.v(TAG, "DIFFOFSUMOFTDASQUARES: " + sumsqd);
|
| 518 | |
setReadingField(X_ACCEL, newAcc[0]);
|
| 519 | |
setReadingField(Y_ACCEL, newAcc[1]);
|
| 520 | |
setReadingField(Z_ACCEL, newAcc[2]);
|
| 521 | |
|
|
437 |
setReadingField(X_ACCEL, event.values[0]);
|
|
438 |
setReadingField(Y_ACCEL, event.values[1]);
|
|
439 |
setReadingField(Z_ACCEL, event.values[2]);
|
| 522 | 440 |
break;
|
| 523 | 441 |
case Sensor.TYPE_LIGHT:
|
| 524 | 442 |
setReadingField(LIGHT, event.values[0]);
|