From c7406355a6be2ceddaa8ec5347d4c62a2da778d3 Mon Sep 17 00:00:00 2001
From: Andy Spencer <andy753421@gmail.com>
Date: Fri, 7 Mar 2014 10:16:16 +0000
Subject: [PATCH] Integrate quaternions

---
 res/layout/main.xml                  | 70 ++++++++++++++++++++++++++++
 src/edu/ucla/iBeaconNav/CMD.java     |  2 +
 src/edu/ucla/iBeaconNav/Main.java    | 10 ++++
 src/edu/ucla/iBeaconNav/Quat.java    | 70 ++++++++++++++++------------
 src/edu/ucla/iBeaconNav/Sensors.java | 36 ++++++++++++++
 5 files changed, 158 insertions(+), 30 deletions(-)

diff --git a/res/layout/main.xml b/res/layout/main.xml
index 4a797b8..ba0827e 100644
--- a/res/layout/main.xml
+++ b/res/layout/main.xml
@@ -21,6 +21,76 @@
 				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"
diff --git a/src/edu/ucla/iBeaconNav/CMD.java b/src/edu/ucla/iBeaconNav/CMD.java
index 172baa9..ce385d1 100644
--- a/src/edu/ucla/iBeaconNav/CMD.java
+++ b/src/edu/ucla/iBeaconNav/CMD.java
@@ -23,11 +23,13 @@ public class CMD {
 		ACC,
 		MAG,
 		GYR,
+		GYR1HZ,
 		ORIENT,
 		WRDACC,
 		WRDGYR,
 		STPCNT,
 		POSITION,
+		ROTATION,
 		HEADING,
 		STABLE,
 	}
diff --git a/src/edu/ucla/iBeaconNav/Main.java b/src/edu/ucla/iBeaconNav/Main.java
index 3e421c9..e9fef30 100644
--- a/src/edu/ucla/iBeaconNav/Main.java
+++ b/src/edu/ucla/iBeaconNav/Main.java
@@ -111,6 +111,11 @@ public class Main extends Activity
 			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);
@@ -131,6 +136,11 @@ public class Main extends Activity
 			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;
diff --git a/src/edu/ucla/iBeaconNav/Quat.java b/src/edu/ucla/iBeaconNav/Quat.java
index 7483f8d..f919f66 100644
--- a/src/edu/ucla/iBeaconNav/Quat.java
+++ b/src/edu/ucla/iBeaconNav/Quat.java
@@ -1,9 +1,11 @@
+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()
 	{
@@ -11,21 +13,20 @@ class 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);
@@ -42,25 +43,25 @@ class Quat
 		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));
 		}
 	}
 
@@ -77,15 +78,24 @@ class Quat
 		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() {
diff --git a/src/edu/ucla/iBeaconNav/Sensors.java b/src/edu/ucla/iBeaconNav/Sensors.java
index e3d788a..745c6e6 100644
--- a/src/edu/ucla/iBeaconNav/Sensors.java
+++ b/src/edu/ucla/iBeaconNav/Sensors.java
@@ -105,6 +105,13 @@ public class Sensors extends Service implements SensorEventListener
 	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)
@@ -172,6 +179,7 @@ public class Sensors extends Service implements SensorEventListener
 			// Reset heading
 			case RSTHEAD:
 				Util.debug("Sensors: handle - reset heading");
+				this.rotation = new Quat();
 				currentHeading = 0;
 				displayData(CMD.Data.HEADING);
 				break;
@@ -318,6 +326,22 @@ public class Sensors extends Service implements SensorEventListener
 				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;
 
 
@@ -470,11 +494,23 @@ public class Sensors extends Service implements SensorEventListener
 			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;
-- 
2.43.2