android:orientation="vertical"
android:layout_width="fill_parent"
android:layout_height="fill_parent">
+ <GridLayout
+ android:layout_width="match_parent"
+ android:layout_height="wrap_content"
+ android:columnCount="1" >
+ <TextView
+ android:id="@+id/roll"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="1"
+ android:text="roll"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/pitch"
+ android:layout_column="0"
+ android:layout_gravity="center_horizontal|top"
+ android:layout_row="1"
+ android:text="pitch"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/yaw"
+ android:layout_column="0"
+ android:layout_gravity="right|top"
+ android:layout_row="1"
+ android:text="yaw"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/rotLabel"
+ 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="Rotation (roll/pitch/yaw)"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ </GridLayout>
+ <GridLayout
+ android:layout_width="match_parent"
+ android:layout_height="wrap_content"
+ android:columnCount="1" >
+ <TextView
+ android:id="@+id/gyr1hzx"
+ android:layout_column="0"
+ android:layout_gravity="left|top"
+ android:layout_row="1"
+ android:text="gyr1hz x"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/gyr1hzy"
+ android:layout_column="0"
+ android:layout_gravity="center_horizontal|top"
+ android:layout_row="1"
+ android:text="gyr1hz y"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/gyr1hzz"
+ android:layout_column="0"
+ android:layout_gravity="right|top"
+ android:layout_row="1"
+ android:text="gyr1hz z"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ <TextView
+ android:id="@+id/gyrLabel"
+ 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="One Second Gyro Sums"
+ android:textAppearance="?android:attr/textAppearanceMedium" />
+ </GridLayout>
<GridLayout
android:layout_width="match_parent"
android:layout_height="wrap_content"
ACC,
MAG,
GYR,
+ GYR1HZ,
ORIENT,
WRDACC,
WRDGYR,
STPCNT,
POSITION,
+ ROTATION,
HEADING,
STABLE,
}
textView2 = (TextView)findViewById(R.id.gyrText2);
textView3 = (TextView)findViewById(R.id.gyrText3);
break;
+ case GYR1HZ:
+ textView1 = (TextView)findViewById(R.id.gyr1hzx);
+ textView2 = (TextView)findViewById(R.id.gyr1hzy);
+ textView3 = (TextView)findViewById(R.id.gyr1hzz);
+ break;
case ORIENT:
textView1 = (TextView)findViewById(R.id.orientText1);
textView2 = (TextView)findViewById(R.id.orientText2);
textView2 = (TextView)findViewById(R.id.curPosYText);
displayNum = 2;
break;
+ case ROTATION:
+ textView1 = (TextView)findViewById(R.id.roll);
+ textView2 = (TextView)findViewById(R.id.pitch);
+ textView3 = (TextView)findViewById(R.id.yaw);
+ break;
case HEADING:
textView1 = (TextView)findViewById(R.id.headingText);
displayNum = 1;
+package edu.ucla.iBeaconNav;
+
class Quat
{
- private double x = 0;
- private double y = 0;
- private double z = 0;
- private double w = 1;
+ public double x = 0;
+ public double y = 0;
+ public double z = 0;
+ public double w = 1;
public Quat()
{
public Quat(double roll, double pitch, double yaw)
{
- double euler[] = { roll, pitch, yaw };
- this.set(euler);
+ this.set(new Vect(roll, pitch, yaw));
}
- public void set(double euler[])
+ public void set(Vect rpy)
{
// Calculate sin/cos of roll/pitch/yaw
- double sr = Math.sin(euler[0] * 0.5f);
- double cr = Math.cos(euler[0] * 0.5f);
+ double sr = Math.sin(rpy.x * 0.5f);
+ double cr = Math.cos(rpy.x * 0.5f);
- double sp = Math.sin(euler[1] * 0.5f);
- double cp = Math.cos(euler[1] * 0.5f);
+ double sp = Math.sin(rpy.y * 0.5f);
+ double cp = Math.cos(rpy.y * 0.5f);
- double sy = Math.sin(euler[2] * 0.5f);
- double cy = Math.cos(euler[2] * 0.5f);
+ double sy = Math.sin(rpy.z * 0.5f);
+ double cy = Math.cos(rpy.z * 0.5f);
// Calculate quaternion
double tx = (cr * cp * sy) + (sr * sp * cy);
this.w = tw / dist;
}
- public void get(double[] euler)
+ public void get(Vect rpy)
{
double test = x*y + z*w;
if (test > 0.499) {
// north pole
- euler[0] = 2 * Math.atan2(x, w);
- euler[1] = Math.PI / 2;
- euler[2] = 0;
+ rpy.x = 2 * Math.atan2(x, w);
+ rpy.y = Math.PI / 2;
+ rpy.z = 0;
} else if (test < -0.499) {
// south pole
- euler[0] = -2 * Math.atan2(x, w);
- euler[1] = Math.PI / 2;
- euler[2] = 0;
+ rpy.x = -2 * Math.atan2(x, w);
+ rpy.y = Math.PI / 2;
+ rpy.z = 0;
} else {
// normal
- euler[0] = Math.atan2(2*y*w - 2*x*z, 1 - 2*(y*y) - 2*(z*z));
- euler[1] = Math.asin(2*x*y + 2*z*w);
- euler[2] = Math.atan2(2*x*w - 2*y*z, 1 - 2*(x*x) - 2*(z*z));
+ rpy.x = Math.atan2(2*y*w - 2*x*z, 1 - 2*(y*y) - 2*(z*z));
+ rpy.y = Math.asin(2*x*y + 2*z*w);
+ rpy.z = Math.atan2(2*x*w - 2*y*z, 1 - 2*(x*x) - 2*(z*z));
}
}
this.z = tz;
}
+ public String toString()
+ {
+ Vect rpy = new Vect();
+ this.get(rpy);
+ return String.format("%7.2f %7.2f %7.2f\n",
+ rpy.x*180/Math.PI,
+ rpy.y*180/Math.PI,
+ rpy.z*180/Math.PI);
+ }
+
public void print(String label)
{
- double euler[] = {0, 0, 0};
- this.get(euler);
- System.out.format("%8s rpy - %7.2f %7.2f %7.2f\n",
- label + ":",
- euler[0]*180/Math.PI,
- euler[1]*180/Math.PI,
- euler[2]*180/Math.PI);
+ System.out.format("%-8s rpy - %s", label+":", this);
+ }
+
+ public void debug(String label)
+ {
+ Util.debug(String.format("%-8s rpy - %s", label+":", this));
}
public static void test() {
private boolean stepStart = false;
+ /* Integrated positions */
+ private Vect position = new Vect();
+ private Quat rotation = new Quat();
+
+ /* Test data */
+ private long time1hz = 0;
+ private Vect gyr1hz = new Vect();
/* Private methods */
private void tellMain(CMD.Response cmd, Object value)
// Reset heading
case RSTHEAD:
Util.debug("Sensors: handle - reset heading");
+ this.rotation = new Quat();
currentHeading = 0;
displayData(CMD.Data.HEADING);
break;
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;
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;
projects / ~andy / iBeaconNav / commitdiff