altoslib: Compute tilt angle from eeprom data

This copies the computation of tilt angle from the firmware so that
post-flight analysis can also show the data.

This change also renames all of the imu values to make them easier to
understand:

	accel	gyro	axis

	along	roll	length of the board
	across	pitch	across the board
	through	yaw	through the board.

Signed-off-by: Keith Packard <keithp@keithp.com>

14 files changed, 603 insertions, 183 deletions

diff --git a/altoslib/AltosCSV.java b/altoslib/AltosCSV.java
index 4a9278d9..2357dbc7 100644
--- a/altoslib/AltosCSV.java
+++ b/altoslib/AltosCSV.java
@@ -162,17 +162,10 @@ public class AltosCSV implements AltosWriter {
 	}
 
 	void write_advanced(AltosState state) {
-		AltosIMU	imu = state.imu;
-		AltosMag	mag = state.mag;
-
-		if (imu == null)
-			imu = new AltosIMU();
-		if (mag == null)
-			mag = new AltosMag();
 		out.printf("%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f,%7.2f",
-			   imu.accel_x, imu.accel_y, imu.accel_z,
-			   imu.gyro_x, imu.gyro_y, imu.gyro_z,
-			   mag.x, mag.y, mag.z);
+			   state.accel_along(), state.accel_across(), state.accel_through(),
+			   state.gyro_roll(), state.gyro_pitch(), state.gyro_yaw(),
+			   state.mag_along(), state.mag_across(), state.mag_through());
 	}
 
 	void write_gps_header() {
@@ -381,7 +374,7 @@ public class AltosCSV implements AltosWriter {
 				has_basic = true;
 			if (state.battery_voltage != AltosLib.MISSING)
 				has_battery = true;
-			if (state.imu != null || state.mag != null)
+			if (state.accel_across() != AltosLib.MISSING)
 				has_advanced = true;
 			if (state.gps != null) {
 				has_gps = true;
diff --git a/altoslib/AltosConfigData.java b/altoslib/AltosConfigData.java
index fc1f2442..a9e863c7 100644
--- a/altoslib/AltosConfigData.java
+++ b/altoslib/AltosConfigData.java
@@ -90,6 +90,9 @@ public class AltosConfigData implements Iterable<String> {
 	public int	tracker_motion;
 	public int	tracker_interval;
 
+	/* HAS_GYRO */
+	public int	accel_zero_along, accel_zero_across, accel_zero_through;
+
 	public static String get_string(String line, String label) throws  ParseException {
 		if (line.startsWith(label)) {
 			String	quoted = line.substring(label.length()).trim();
@@ -266,6 +269,10 @@ public class AltosConfigData implements Iterable<String> {
 		storage_size = -1;
 		storage_erase_unit = -1;
 		stored_flight = 0;
+
+		accel_zero_along = -1;
+		accel_zero_across = -1;
+		accel_zero_through = -1;
 	}
 
 	public void parse_line(String line) {
@@ -361,6 +368,18 @@ public class AltosConfigData implements Iterable<String> {
 
 		/* Log listing replies */
 		try { get_int(line, "flight"); stored_flight++; }  catch (Exception e) {}
+
+		/* HAS_GYRO */
+		try {
+			if (line.startsWith("IMU call along")) {
+				String[] bits = line.split("\\s+");
+				if (bits.length >= 8) {
+					accel_zero_along = Integer.parseInt(bits[3]);
+					accel_zero_across = Integer.parseInt(bits[5]);
+					accel_zero_through = Integer.parseInt(bits[7]);
+				}
+			}
+		} catch (Exception e) {}
 	}
 
 	public AltosConfigData() {
@@ -637,6 +656,9 @@ public class AltosConfigData implements Iterable<String> {
 		if (tracker_motion >= 0 && tracker_interval >= 0)
 			link.printf("c t %d %d\n", tracker_motion, tracker_interval);
 
+		/* HAS_GYRO */
+		/* UI doesn't support accel cal */
+
 		link.printf("c w\n");
 		link.flush_output();
 	}
diff --git a/altoslib/AltosEepromHeader.java b/altoslib/AltosEepromHeader.java
index 71030655..0414c37e 100644
--- a/altoslib/AltosEepromHeader.java
+++ b/altoslib/AltosEepromHeader.java
@@ -25,7 +25,7 @@ public class AltosEepromHeader extends AltosEeprom {
 
 	public int	cmd;
 	public String	data;
-	public int	config_a, config_b;
+	public int	config_a, config_b, config_c;
 	public boolean	last;
 	public boolean	valid;
 
@@ -93,6 +93,9 @@ public class AltosEepromHeader extends AltosEeprom {
 			state.make_baro();
 			state.baro.crc = config_a;
 			break;
+		case AltosLib.AO_LOG_IMU_CAL:
+			state.set_accel_zero(config_a, config_b, config_c);
+			break;
 		case AltosLib.AO_LOG_SOFTWARE_VERSION:
 			state.set_firmware_version(data);
 			break;
@@ -100,7 +103,10 @@ public class AltosEepromHeader extends AltosEeprom {
 		case AltosLib.AO_LOG_APOGEE_LOCKOUT:
 		case AltosLib.AO_LOG_RADIO_RATE:
 		case AltosLib.AO_LOG_IGNITE_MODE:
+			break;
 		case AltosLib.AO_LOG_PAD_ORIENTATION:
+			state.set_pad_orientation(config_a);
+			break;
 		case AltosLib.AO_LOG_RADIO_ENABLE:
 		case AltosLib.AO_LOG_AES_KEY:
 		case AltosLib.AO_LOG_APRS:
@@ -177,11 +183,17 @@ public class AltosEepromHeader extends AltosEeprom {
 		case AltosLib.AO_LOG_BARO_CRC:
 			out.printf ("# Baro crc: %d\n", config_a);
 			break;
+		case AltosLib.AO_LOG_IMU_CAL:
+			out.printf ("# IMU cal: %d %d %d\n", config_a, config_b, config_c);
+			break;
 		case AltosLib.AO_LOG_FREQUENCY:
 		case AltosLib.AO_LOG_APOGEE_LOCKOUT:
 		case AltosLib.AO_LOG_RADIO_RATE:
 		case AltosLib.AO_LOG_IGNITE_MODE:
+			break;
 		case AltosLib.AO_LOG_PAD_ORIENTATION:
+			out.printf("# Pad orientation: %d\n", config_a);
+			break;
 		case AltosLib.AO_LOG_RADIO_ENABLE:
 		case AltosLib.AO_LOG_AES_KEY:
 		case AltosLib.AO_LOG_APRS:
@@ -273,6 +285,14 @@ public class AltosEepromHeader extends AltosEeprom {
 					cmd = AltosLib.AO_LOG_INVALID;
 					data = tokens[2];
 				}
+			} else if (tokens[0].equals("IMU") && tokens[1].equals("cal")) {
+				cmd = AltosLib.AO_LOG_IMU_CAL;
+				config_a = Integer.parseInt(tokens[3]);
+				config_b = Integer.parseInt(tokens[5]);
+				config_c = Integer.parseInt(tokens[7]);
+			} else if (tokens[0].equals("Pad") && tokens[1].equals("orientation:")) {
+				cmd = AltosLib.AO_LOG_PAD_ORIENTATION;
+				config_a = Integer.parseInt(tokens[2]);
 			} else
 				valid = false;
 		} catch (Exception e) {
diff --git a/altoslib/AltosEepromMega.java b/altoslib/AltosEepromMega.java
index aaff2410..f9bfb4a7 100644
--- a/altoslib/AltosEepromMega.java
+++ b/altoslib/AltosEepromMega.java
@@ -149,6 +149,12 @@ public class AltosEepromMega extends AltosEeprom {
 			state.set_flight(flight());
 			state.set_ground_accel(ground_accel());
 			state.set_ground_pressure(ground_pres());
+			state.set_accel_ground(ground_accel_along(),
+					       ground_accel_across(),
+					       ground_accel_through());
+			state.set_gyro_zero(ground_roll() / 512.0,
+					    ground_pitch() / 512.0,
+					    ground_yaw() / 512.0);
 			break;
 		case AltosLib.AO_LOG_STATE:
 			state.set_tick(tick);
@@ -158,22 +164,21 @@ public class AltosEepromMega extends AltosEeprom {
 			state.set_tick(tick);
 			state.set_ms5607(pres(), temp());
 
-			AltosIMU imu = new AltosIMU();
-			imu.accel_x = AltosIMU.convert_accel(accel_x());
-			imu.accel_y = AltosIMU.convert_accel(accel_y());
-			imu.accel_z = AltosIMU.convert_accel(accel_z());
+			AltosIMU	imu = new AltosIMU(accel_y(),	/* along */
+							   accel_x(),	/* across */
+							   accel_z(),	/* through */
+							   gyro_y(),	/* roll */
+							   gyro_x(),	/* pitch */
+							   gyro_z());	/* yaw */
 
-			imu.gyro_x = AltosIMU.convert_gyro(gyro_x());
-			imu.gyro_y = AltosIMU.convert_gyro(gyro_y());
-			imu.gyro_z = AltosIMU.convert_gyro(gyro_z());
-			state.imu = imu;
+			if (log_format == AltosLib.AO_LOG_FORMAT_TELEMEGA_OLD)
+				state.check_imu_wrap(imu);
 
-			AltosMag mag = new AltosMag();
-			mag.x = AltosMag.convert_gauss(mag_x());
-			mag.y = AltosMag.convert_gauss(mag_y());
-			mag.z = AltosMag.convert_gauss(mag_z());
+			state.set_imu(imu);
 
-			state.mag = mag;
+			state.set_mag(new AltosMag(mag_x(),
+						   mag_y(),
+						   mag_z()));
 
 			state.set_accel(accel());
 
diff --git a/altoslib/AltosIMU.java b/altoslib/AltosIMU.java
index 89d7def4..d7373f3c 100644
--- a/altoslib/AltosIMU.java
+++ b/altoslib/AltosIMU.java
@@ -20,36 +20,24 @@ package org.altusmetrum.altoslib_5;
 import java.util.concurrent.*;
 
 public class AltosIMU implements Cloneable {
-	public double		accel_x;
-	public double		accel_y;
-	public double		accel_z;
+	public int		accel_along;
+	public int		accel_across;
+	public int		accel_through;
 
-	public double		gyro_x;
-	public double		gyro_y;
-	public double		gyro_z;
+	public int		gyro_roll;
+	public int		gyro_pitch;
+	public int		gyro_yaw;
 
-/*
- * XXX use ground measurements to adjust values
-
-	public double		ground_accel_x;
-	public double		ground_accel_y;
-	public double		ground_accel_z;
-
-	public double		ground_gyro_x;
-	public double		ground_gyro_y;
-	public double		ground_gyro_z;
-*/
-
-	public static int	counts_per_g = 2048;
+	public static double	counts_per_g = 2048.0;
 
-	public static double convert_accel(int counts) {
-		return (double) counts / (double) counts_per_g * (-AltosConvert.GRAVITATIONAL_ACCELERATION);
+	public static double convert_accel(double counts) {
+		return counts / counts_per_g * (-AltosConvert.GRAVITATIONAL_ACCELERATION);
 	}
 
 	public static double	counts_per_degsec = 16.4;
 
-	public static double convert_gyro(int counts) {
-		return (double) counts / counts_per_degsec;
+	public static double convert_gyro(double counts) {
+		return counts / counts_per_degsec;
 	}
 
 	public boolean parse_string(String line) {
@@ -59,12 +47,12 @@ public class AltosIMU implements Cloneable {
 		String[] items = line.split("\\s+");
 
 		if (items.length >= 8) {
-			accel_x = convert_accel(Integer.parseInt(items[1]));
-			accel_y = convert_accel(Integer.parseInt(items[2]));
-			accel_z = convert_accel(Integer.parseInt(items[3]));
-			gyro_x = convert_gyro(Integer.parseInt(items[5]));
-			gyro_y = convert_gyro(Integer.parseInt(items[6]));
-			gyro_z = convert_gyro(Integer.parseInt(items[7]));
+			accel_along = Integer.parseInt(items[1]);
+			accel_across = Integer.parseInt(items[2]);
+			accel_through = Integer.parseInt(items[3]);
+			gyro_roll = Integer.parseInt(items[5]);
+			gyro_pitch = Integer.parseInt(items[6]);
+			gyro_yaw = Integer.parseInt(items[7]);
 		}
 		return true;
 	}
@@ -72,13 +60,13 @@ public class AltosIMU implements Cloneable {
 	public AltosIMU clone() {
 		AltosIMU	n = new AltosIMU();
 
-		n.accel_x = accel_x;
-		n.accel_y = accel_y;
-		n.accel_z = accel_z;
+		n.accel_along = accel_along;
+		n.accel_across = accel_across;
+		n.accel_through = accel_through;
 
-		n.gyro_x = gyro_x;
-		n.gyro_y = gyro_y;
-		n.gyro_z = gyro_z;
+		n.gyro_roll = gyro_roll;
+		n.gyro_pitch = gyro_pitch;
+		n.gyro_yaw = gyro_yaw;
 		return n;
 	}
 
@@ -93,13 +81,25 @@ public class AltosIMU implements Cloneable {
 	}
 
 	public AltosIMU() {
-		accel_x = AltosLib.MISSING;
-		accel_y = AltosLib.MISSING;
-		accel_z = AltosLib.MISSING;
+		accel_along = AltosLib.MISSING;
+		accel_across = AltosLib.MISSING;
+		accel_through = AltosLib.MISSING;
+
+		gyro_roll = AltosLib.MISSING;
+		gyro_pitch = AltosLib.MISSING;
+		gyro_yaw = AltosLib.MISSING;
+	}
+
+	public AltosIMU(int accel_along, int accel_across, int accel_through,
+			int gyro_roll, int gyro_pitch, int gyro_yaw) {
+
+		this.accel_along = accel_along;
+		this.accel_across = accel_across;
+		this.accel_through = accel_through;
 
-		gyro_x = AltosLib.MISSING;
-		gyro_y = AltosLib.MISSING;
-		gyro_z = AltosLib.MISSING;
+		this.gyro_roll = gyro_roll;
+		this.gyro_pitch = gyro_pitch;
+		this.gyro_yaw = gyro_yaw;
 	}
 
 	public AltosIMU(AltosLink link) throws InterruptedException, TimeoutException {
diff --git a/altoslib/AltosLib.java b/altoslib/AltosLib.java
index 92a22ec7..0edc0b43 100644
--- a/altoslib/AltosLib.java
+++ b/altoslib/AltosLib.java
@@ -74,6 +74,7 @@ public class AltosLib {
 	public static final int AO_LOG_BARO_TREF = 3006;
 	public static final int AO_LOG_BARO_TEMPSENS = 3007;
 	public static final int AO_LOG_BARO_CRC = 3008;
+	public static final int AO_LOG_IMU_CAL = 3009;
 
 	public static final int AO_LOG_SOFTWARE_VERSION = 9999;
 
diff --git a/altoslib/AltosMag.java b/altoslib/AltosMag.java
index 690241f1..1fa8877b 100644
--- a/altoslib/AltosMag.java
+++ b/altoslib/AltosMag.java
@@ -20,19 +20,19 @@ package org.altusmetrum.altoslib_5;
 import java.util.concurrent.*;
 
 public class AltosMag implements Cloneable {
-	public double		x;
-	public double		y;
-	public double		z;
+	public int		along;
+	public int		across;
+	public int		through;
 
 	public static double counts_per_gauss = 1090;
 
-	public static double convert_gauss(int counts) {
-		return (double) counts / counts_per_gauss;
+	public static double convert_gauss(double counts) {
+		return counts / counts_per_gauss;
 	}
 
 	public boolean parse_string(String line) {
 //		if (line.startsWith("Syntax error")) {
-//			x = y = z = 0;
+//			along = across = through = 0;
 //			return true;
 //		}
 
@@ -42,9 +42,9 @@ public class AltosMag implements Cloneable {
 		String[] items = line.split("\\s+");
 
 		if (items.length >= 6) {
-			x = convert_gauss(Integer.parseInt(items[1]));
-			y = convert_gauss(Integer.parseInt(items[3]));
-			z = convert_gauss(Integer.parseInt(items[5]));
+			along = Integer.parseInt(items[1]);
+			across = Integer.parseInt(items[3]);
+			through = Integer.parseInt(items[5]);
 		}
 		return true;
 	}
@@ -52,16 +52,22 @@ public class AltosMag implements Cloneable {
 	public AltosMag clone() {
 		AltosMag n = new AltosMag();
 
-		n.x = x;
-		n.y = y;
-		n.z = z;
+		n.along = along;
+		n.across = across;
+		n.through = through;
 		return n;
 	}
 
 	public AltosMag() {
-		x = AltosLib.MISSING;
-		y = AltosLib.MISSING;
-		z = AltosLib.MISSING;
+		along = AltosLib.MISSING;
+		across = AltosLib.MISSING;
+		through = AltosLib.MISSING;
+	}
+
+	public AltosMag(int along, int across, int through) {
+		this.along = along;
+		this.across = across;
+		this.through = through;
 	}
 
 	static public void update_state(AltosState state, AltosLink link, AltosConfigData config_data) throws InterruptedException {
diff --git a/altoslib/AltosQuaternion.java b/altoslib/AltosQuaternion.java
new file mode 100644
index 00000000..a7125100
--- /dev/null
+++ b/altoslib/AltosQuaternion.java
@@ -0,0 +1,150 @@
+/*
+ * Copyright © 2014 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+package org.altusmetrum.altoslib_5;
+
+public class AltosQuaternion {
+	double	r;		/* real bit */
+	double	x, y, z;	/* imaginary bits */
+
+	public AltosQuaternion multiply(AltosQuaternion b) {
+		return new AltosQuaternion(
+			this.r * b.r - this.x * b.x - this.y * b.y - this.z * b.z,
+			this.r * b.x + this.x * b.r + this.y * b.z - this.z * b.y,
+			this.r * b.y - this.x * b.z + this.y * b.r + this.z * b.x,
+			this.r * b.z + this.x * b.y - this.y * b.x + this.z * b.r);
+	}
+
+	public AltosQuaternion conjugate() {
+		return new AltosQuaternion(
+			this.r,
+			-this.x,
+			-this.y,
+			-this.z);
+	}
+
+	public double normal() {
+		return (this.r * this.r +
+			this.x * this.x +
+			this.y * this.y +
+			this.z * this.z);
+	}
+
+	public AltosQuaternion scale(double b) {
+		return new AltosQuaternion(
+			this.r * b,
+			this.x * b,
+			this.y * b,
+			this.z * b);
+	}
+
+	public AltosQuaternion normalize() {
+		double	n = normal();
+		if (n <= 0)
+			return this;
+		return scale(1/Math.sqrt(n));
+	}
+
+	public double dot(AltosQuaternion b) {
+		return (this.r * b.r +
+			this.x * b.x +
+			this.y * b.y +
+			this.z * b.z);
+	}
+
+	public AltosQuaternion rotate(AltosQuaternion b) {
+		return (b.multiply(this)).multiply(b.conjugate());
+	}
+
+	public AltosQuaternion vectors_to_rotation(AltosQuaternion b) {
+		/*
+		 * The cross product will point orthogonally to the two
+		 * vectors, forming our rotation axis. The length will be
+		 * sin(θ), so these values are already multiplied by that.
+		 */
+
+		double x = this.y * b.z - this.z * b.y;
+		double y = this.z * b.x - this.x * b.z;
+		double z = this.x * b.y - this.y * b.x;
+
+		double s_2 = x*x + y*y + z*z;
+		double s = Math.sqrt(s_2);
+
+		/* cos(θ) = a · b / (|a| |b|).
+		 *
+		 * a and b are both unit vectors, so the divisor is one
+		 */
+		double c = this.x*b.x + this.y*b.y + this.z*b.z;
+
+		double c_half = Math.sqrt ((1 + c) / 2);
+		double s_half = Math.sqrt ((1 - c) / 2);
+
+		/*
+		 * Divide out the sine factor from the
+		 * cross product, then multiply in the
+		 * half sine factor needed for the quaternion
+		 */
+		double s_scale = s_half / s;
+
+		AltosQuaternion r = new AltosQuaternion(c_half,
+							x * s_scale,
+							y * s_scale,
+							z * s_scale);
+		return r.normalize();
+	}
+
+	public AltosQuaternion(double r, double x, double y, double z) {
+		this.r = r;
+		this.x = x;
+		this.y = y;
+		this.z = z;
+	}
+
+	public AltosQuaternion(AltosQuaternion q) {
+		this.r = q.r;
+		this.x = q.x;
+		this.y = q.y;
+		this.z = q.z;
+	}
+
+	static public AltosQuaternion vector(double x, double y, double z) {
+		return new AltosQuaternion(0, x, y, z);
+	}
+
+	static public AltosQuaternion rotation(double x, double y, double z,
+					       double s, double c) {
+		return new AltosQuaternion(c,
+					   s*x,
+					   s*y,
+					   s*z);
+	}
+
+	static public AltosQuaternion zero_rotation() {
+		return new AltosQuaternion(1, 0, 0, 0);
+	}
+
+	static public AltosQuaternion half_euler(double x, double y, double z) {
+		double	s_x = Math.sin(x), c_x = Math.cos(x);
+		double	s_y = Math.sin(y), c_y = Math.cos(y);
+		double	s_z = Math.sin(z), c_z = Math.cos(z);;
+
+		return new AltosQuaternion(c_x * c_y * c_z + s_x * s_y * s_z,
+					   s_x * c_y * c_z - c_x * s_y * s_z,
+					   c_x * s_y * c_z + s_x * c_y * s_z,
+					   c_x * c_y * s_z - s_x * s_y * c_z);
+	}
+}
diff --git a/altoslib/AltosRotation.java b/altoslib/AltosRotation.java
new file mode 100644
index 00000000..ad5aca8f
--- /dev/null
+++ b/altoslib/AltosRotation.java
@@ -0,0 +1,50 @@
+/*
+ * Copyright © 2014 Keith Packard <keithp@keithp.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+package org.altusmetrum.altoslib_5;
+
+public class AltosRotation {
+	private AltosQuaternion		rotation;
+
+	public double tilt() {
+		double	rotz = rotation.z * rotation.z - rotation.y * rotation.y - rotation.x * rotation.x + rotation.r * rotation.r;
+
+		double tilt = Math.acos(rotz) * 180.0 / Math.PI;
+		return tilt;
+	}
+
+	public void rotate(double dt, double x, double y, double z) {
+		AltosQuaternion	rot = AltosQuaternion.half_euler(x * dt, y * dt, z * dt);
+		rotation = rot.multiply(rotation).normalize();
+	}
+
+	/* Clone an existing rotation value */
+	public AltosRotation (AltosRotation old) {
+		this.rotation = new AltosQuaternion(old.rotation);
+	}
+
+	/* Create a new rotation value given an acceleration vector pointing down */
+	public AltosRotation(double x,
+			     double y,
+			     double z,
+			     int pad_orientation) {
+		AltosQuaternion	orient = AltosQuaternion.vector(x, y, z).normalize();
+		double sky = pad_orientation == 0 ? 1 : -1;
+		AltosQuaternion	up = new AltosQuaternion(0, 0, 0, sky);
+		rotation = up.vectors_to_rotation(orient);
+	}
+}
diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 5fce15c4..830e95f3 100644
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -44,8 +44,8 @@ public class AltosState implements Cloneable {
 	public int	boost_tick;
 
 	class AltosValue {
-		private double	value;
-		private double	prev_value;
+		double	value;
+		double	prev_value;
 		private double	max_value;
 		private double	set_time;
 		private double	prev_set_time;
@@ -180,8 +180,19 @@ public class AltosState implements Cloneable {
 	}
 
 	class AltosCValue {
-		AltosValue	measured;
-		AltosValue	computed;
+
+		class AltosIValue extends AltosValue {
+			boolean can_max() {
+				return c_can_max();
+			}
+		};
+
+		public AltosIValue	measured;
+		public AltosIValue	computed;
+
+		boolean can_max() { return true; }
+
+		boolean c_can_max() { return can_max(); }
 
 		double value() {
 			double v = measured.value();
@@ -263,8 +274,8 @@ public class AltosState implements Cloneable {
 		}
 
 		AltosCValue() {
-			measured = new AltosValue();
-			computed = new AltosValue();
+			measured = new AltosIValue();
+			computed = new AltosIValue();
 		}
 	}
 
@@ -600,10 +611,10 @@ public class AltosState implements Cloneable {
 		return acceleration.max();
 	}
 
-	public AltosValue	orient;
+	public AltosCValue	orient;
 
 	public void set_orient(double new_orient) {
-		orient.set(new_orient, time);
+		orient.set_measured(new_orient, time);
 	}
 
 	public double orient() {
@@ -713,7 +724,7 @@ public class AltosState implements Cloneable {
 		pressure = new AltosPressure();
 		speed = new AltosSpeed();
 		acceleration = new AltosAccel();
-		orient = new AltosValue();
+		orient = new AltosCValue();
 
 		temperature = AltosLib.MISSING;
 		battery_voltage = AltosLib.MISSING;
@@ -733,7 +744,24 @@ public class AltosState implements Cloneable {
 		gps_pending = false;
 
 		imu = null;
+		last_imu_time = AltosLib.MISSING;
+		rotation = null;
+		ground_rotation = null;
+
 		mag = null;
+		accel_zero_along = AltosLib.MISSING;
+		accel_zero_across = AltosLib.MISSING;
+		accel_zero_through = AltosLib.MISSING;
+
+		accel_ground_along = AltosLib.MISSING;
+		accel_ground_across = AltosLib.MISSING;
+		accel_ground_through = AltosLib.MISSING;
+
+		pad_orientation = AltosLib.MISSING;
+
+		gyro_zero_roll = AltosLib.MISSING;
+		gyro_zero_pitch = AltosLib.MISSING;
+		gyro_zero_yaw = AltosLib.MISSING;
 
 		set_npad(0);
 		ngps = 0;
@@ -861,6 +889,27 @@ public class AltosState implements Cloneable {
 			imu = old.imu.clone();
 		else
 			imu = null;
+		last_imu_time = old.last_imu_time;
+
+		if (old.rotation != null)
+			rotation = new AltosRotation (old.rotation);
+
+		if (old.ground_rotation != null) {
+			ground_rotation = new AltosRotation(old.ground_rotation);
+		}
+
+		accel_zero_along = old.accel_zero_along;
+		accel_zero_across = old.accel_zero_across;
+		accel_zero_through = old.accel_zero_through;
+
+		accel_ground_along = old.accel_ground_along;
+		accel_ground_across = old.accel_ground_across;
+		accel_ground_through = old.accel_ground_through;
+		pad_orientation = old.pad_orientation;
+
+		gyro_zero_roll = old.gyro_zero_roll;
+		gyro_zero_pitch = old.gyro_zero_pitch;
+		gyro_zero_yaw = old.gyro_zero_yaw;
 
 		if (old.mag != null)
 			mag = old.mag.clone();
@@ -1112,16 +1161,170 @@ public class AltosState implements Cloneable {
 		}
 	}
 
+
+	public double	accel_zero_along;
+	public double	accel_zero_across;
+	public double	accel_zero_through;
+
+	public AltosRotation	rotation;
+	public AltosRotation	ground_rotation;
+
+	public void set_accel_zero(double zero_along, double zero_across, double zero_through) {
+		if (zero_along != AltosLib.MISSING) {
+			accel_zero_along = zero_along;
+			accel_zero_across = zero_across;
+			accel_zero_through = zero_through;
+		}
+	}
+
+	public int pad_orientation;
+
+	public double	accel_ground_along, accel_ground_across, accel_ground_through;
+
+	void update_pad_rotation() {
+		if (pad_orientation != AltosLib.MISSING && accel_ground_along != AltosLib.MISSING) {
+			rotation = new AltosRotation(AltosIMU.convert_accel(accel_ground_across - accel_zero_across),
+						     AltosIMU.convert_accel(accel_ground_through - accel_zero_through),
+						     AltosIMU.convert_accel(accel_ground_along - accel_zero_along),
+						     pad_orientation);
+			ground_rotation = rotation;
+			orient.set_computed(rotation.tilt(), time);
+		}
+	}
+
+	public void set_accel_ground(double ground_along, double ground_across, double ground_through) {
+		accel_ground_along = ground_along;
+		accel_ground_across = ground_across;
+		accel_ground_through = ground_through;
+		update_pad_rotation();
+	}
+
+	public void set_pad_orientation(int pad_orientation) {
+		this.pad_orientation = pad_orientation;
+		update_pad_rotation();
+	}
+
+	public double	gyro_zero_roll;
+	public double	gyro_zero_pitch;
+	public double	gyro_zero_yaw;
+
+	public void set_gyro_zero(double roll, double pitch, double yaw) {
+		if (roll != AltosLib.MISSING) {
+			gyro_zero_roll = roll;
+			gyro_zero_pitch = pitch;
+			gyro_zero_yaw = yaw;
+		}
+	}
+
+	public double	last_imu_time;
+
+	private double radians(double degrees) {
+		if (degrees == AltosLib.MISSING)
+			return AltosLib.MISSING;
+		return degrees * Math.PI / 180.0;
+	}
+
+	private void update_orient() {
+		if (last_imu_time != AltosLib.MISSING) {
+			double	t = time - last_imu_time;
+
+			double	pitch = radians(gyro_pitch());
+			double	yaw = radians(gyro_yaw());
+			double	roll = radians(gyro_roll());
+
+			if (t > 0 & pitch != AltosLib.MISSING && rotation != null) {
+				rotation.rotate(t, pitch, yaw, roll);
+				orient.set_computed(rotation.tilt(), time);
+			}
+		}
+		last_imu_time = time;
+	}
+
 	public void set_imu(AltosIMU imu) {
 		if (imu != null)
 			imu = imu.clone();
 		this.imu = imu;
+		update_orient();
+	}
+
+	private double gyro_zero_overflow(double first) {
+		double v = first / 128.0;
+		if (v < 0)
+			v = Math.ceil(v);
+		else
+			v = Math.floor(v);
+		return v * 128.0;
+	}
+
+	public void check_imu_wrap(AltosIMU imu) {
+		if (this.imu == null) {
+			gyro_zero_roll += gyro_zero_overflow(imu.gyro_roll);
+			gyro_zero_pitch += gyro_zero_overflow(imu.gyro_pitch);
+			gyro_zero_yaw += gyro_zero_overflow(imu.gyro_yaw);
+		}
+	}
+
+	public double accel_along() {
+		if (imu != null && accel_zero_along != AltosLib.MISSING)
+			return AltosIMU.convert_accel(imu.accel_along - accel_zero_along);
+		return AltosLib.MISSING;
+	}
+
+	public double accel_across() {
+		if (imu != null && accel_zero_across != AltosLib.MISSING)
+			return AltosIMU.convert_accel(imu.accel_across - accel_zero_across);
+		return AltosLib.MISSING;
+	}
+
+	public double accel_through() {
+		if (imu != null && accel_zero_through != AltosLib.MISSING)
+			return AltosIMU.convert_accel(imu.accel_through - accel_zero_through);
+		return AltosLib.MISSING;
+	}
+
+	public double gyro_roll() {
+		if (imu != null && gyro_zero_roll != AltosLib.MISSING) {
+			return AltosIMU.convert_gyro(imu.gyro_roll - gyro_zero_roll);
+		}
+		return AltosLib.MISSING;
+	}
+
+	public double gyro_pitch() {
+		if (imu != null && gyro_zero_pitch != AltosLib.MISSING) {
+			return AltosIMU.convert_gyro(imu.gyro_pitch - gyro_zero_pitch);
+		}
+		return AltosLib.MISSING;
+	}
+
+	public double gyro_yaw() {
+		if (imu != null && gyro_zero_yaw != AltosLib.MISSING) {
+			return AltosIMU.convert_gyro(imu.gyro_yaw - gyro_zero_yaw);
+		}
+		return AltosLib.MISSING;
 	}
 
 	public void set_mag(AltosMag mag) {
 		this.mag = mag.clone();
 	}
 
+	public double mag_along() {
+		if (mag != null)
+			return AltosMag.convert_gauss(mag.along);
+		return AltosLib.MISSING;
+	}
+
+	public double mag_across() {
+		if (mag != null)
+			return AltosMag.convert_gauss(mag.across);
+		return AltosLib.MISSING;
+	}
+
+	public double mag_through() {
+		if (mag != null)
+			return AltosMag.convert_gauss(mag.through);
+		return AltosLib.MISSING;
+	}
+
 	public AltosMs5607 make_baro() {
 		if (baro == null)
 			baro = new AltosMs5607();
diff --git a/altoslib/AltosTelemetryMegaSensor.java b/altoslib/AltosTelemetryMegaSensor.java
index 1b568c88..bae18d1c 100644
--- a/altoslib/AltosTelemetryMegaSensor.java
+++ b/altoslib/AltosTelemetryMegaSensor.java
@@ -66,24 +66,13 @@ public class AltosTelemetryMegaSensor extends AltosTelemetryStandard {
 
 		state.set_orient(orient);
 
-		AltosIMU imu = new AltosIMU();
-
-		imu.accel_x = AltosIMU.convert_accel(accel_x);
-		imu.accel_y = AltosIMU.convert_accel(accel_y);
-		imu.accel_z = AltosIMU.convert_accel(accel_z);
-
-		imu.gyro_x = AltosIMU.convert_gyro(gyro_x);
-		imu.gyro_y = AltosIMU.convert_gyro(gyro_y);
-		imu.gyro_z = AltosIMU.convert_gyro(gyro_z);
-
-		state.imu = imu;
-
-		AltosMag mag = new AltosMag();
-
-		mag.x = AltosMag.convert_gauss(mag_x);
-		mag.y = AltosMag.convert_gauss(mag_y);
-		mag.z = AltosMag.convert_gauss(mag_z);
-
-		state.mag = mag;
+		state.set_imu(new AltosIMU(accel_y,	/* along */
+					   accel_x,	/* across */
+					   accel_z,	/* through */
+					   gyro_y,	/* along */
+					   gyro_x,	/* across */
+					   gyro_z));	/* through */
+
+		state.set_mag(new AltosMag(mag_x, mag_y, mag_z));
 	}
 }
diff --git a/altoslib/Makefile.am b/altoslib/Makefile.am
index 0ad9b7da..2805742e 100644
--- a/altoslib/Makefile.am
+++ b/altoslib/Makefile.am
@@ -124,7 +124,9 @@ altoslib_JAVA = \
 	AltosLatitude.java \
 	AltosLongitude.java \
 	AltosPyro.java \
-	AltosWriter.java
+	AltosWriter.java \
+	AltosQuaternion.java \
+	AltosRotation.java
 
 JAR=altoslib_$(ALTOSLIB_VERSION).jar
 
diff --git a/altosuilib/AltosGraph.java b/altosuilib/AltosGraph.java
index 522eea1e..2e6d428d 100644
--- a/altosuilib/AltosGraph.java
+++ b/altosuilib/AltosGraph.java
@@ -410,58 +410,58 @@ public class AltosGraph extends AltosUIGraph {
 		}
 
 		if (stats.has_imu) {
-			addSeries("Acceleration X",
-				  AltosGraphDataPoint.data_accel_x,
+			addSeries("Acceleration Along",
+				  AltosGraphDataPoint.data_accel_along,
 				  AltosConvert.accel,
 				  accel_x_color,
 				  false,
 				  accel_axis);
-			addSeries("Acceleration Y",
-				  AltosGraphDataPoint.data_accel_y,
+			addSeries("Acceleration Across",
+				  AltosGraphDataPoint.data_accel_across,
 				  AltosConvert.accel,
 				  accel_y_color,
 				  false,
 				  accel_axis);
-			addSeries("Acceleration Z",
-				  AltosGraphDataPoint.data_accel_z,
+			addSeries("Acceleration Through",
+				  AltosGraphDataPoint.data_accel_through,
 				  AltosConvert.accel,
 				  accel_z_color,
 				  false,
 				  accel_axis);
-			addSeries("Rotation Rate X",
-				  AltosGraphDataPoint.data_gyro_x,
+			addSeries("Roll Rate",
+				  AltosGraphDataPoint.data_gyro_roll,
 				  gyro_units,
 				  gyro_x_color,
 				  false,
 				  gyro_axis);
-			addSeries("Rotation Rate Y",
-				  AltosGraphDataPoint.data_gyro_y,
+			addSeries("Pitch Rate",
+				  AltosGraphDataPoint.data_gyro_pitch,
 				  gyro_units,
 				  gyro_y_color,
 				  false,
 				  gyro_axis);
-			addSeries("Rotation Rate Z",
-				  AltosGraphDataPoint.data_gyro_z,
+			addSeries("Yaw Rate",
+				  AltosGraphDataPoint.data_gyro_yaw,
 				  gyro_units,
 				  gyro_z_color,
 				  false,
 				  gyro_axis);
 		}
 		if (stats.has_mag) {
-			addSeries("Magnetometer X",
-				  AltosGraphDataPoint.data_mag_x,
+			addSeries("Magnetometer Along",
+				  AltosGraphDataPoint.data_mag_along,
 				  mag_units,
 				  mag_x_color,
 				  false,
 				  mag_axis);
-			addSeries("Magnetometer Y",
-				  AltosGraphDataPoint.data_mag_y,
+			addSeries("Magnetometer Across",
+				  AltosGraphDataPoint.data_mag_across,
 				  mag_units,
 				  mag_y_color,
 				  false,
 				  mag_axis);
-			addSeries("Magnetometer Z",
-				  AltosGraphDataPoint.data_mag_z,
+			addSeries("Magnetometer Through",
+				  AltosGraphDataPoint.data_mag_through,
 				  mag_units,
 				  mag_z_color,
 				  false,
diff --git a/altosuilib/AltosGraphDataPoint.java b/altosuilib/AltosGraphDataPoint.java
index 56dadb8b..a9f4f431 100644
--- a/altosuilib/AltosGraphDataPoint.java
+++ b/altosuilib/AltosGraphDataPoint.java
@@ -40,15 +40,15 @@ public class AltosGraphDataPoint implements AltosUIDataPoint {
 	public static final int data_range = 14;
 	public static final int data_distance = 15;
 	public static final int data_pressure = 16;
-	public static final int data_accel_x = 17;
-	public static final int data_accel_y = 18;
-	public static final int data_accel_z = 19;
-	public static final int data_gyro_x = 20;
-	public static final int data_gyro_y = 21;
-	public static final int data_gyro_z = 22;
-	public static final int data_mag_x = 23;
-	public static final int data_mag_y = 24;
-	public static final int data_mag_z = 25;
+	public static final int data_accel_along = 17;
+	public static final int data_accel_across = 18;
+	public static final int data_accel_through = 19;
+	public static final int data_gyro_roll = 20;
+	public static final int data_gyro_pitch = 21;
+	public static final int data_gyro_yaw = 22;
+	public static final int data_mag_along = 23;
+	public static final int data_mag_across = 24;
+	public static final int data_mag_through = 25;
 	public static final int data_orient = 26;
 	public static final int data_gps_course = 27;
 	public static final int data_gps_ground_speed = 28;
@@ -128,53 +128,32 @@ public class AltosGraphDataPoint implements AltosUIDataPoint {
 			y = state.pressure();
 			break;
 
-		case data_accel_x:
-		case data_accel_y:
-		case data_accel_z:
-		case data_gyro_x:
-		case data_gyro_y:
-		case data_gyro_z:
-			AltosIMU	imu = state.imu;
-			if (imu == null)
-				break;
-			switch (index) {
-			case data_accel_x:
-				y = imu.accel_x;
-				break;
-			case data_accel_y:
-				y = imu.accel_y;
-				break;
-			case data_accel_z:
-				y = imu.accel_z;
-				break;
-			case data_gyro_x:
-				y = imu.gyro_x;
-				break;
-			case data_gyro_y:
-				y = imu.gyro_y;
-				break;
-			case data_gyro_z:
-				y = imu.gyro_z;
-				break;
-			}
+		case data_accel_along:
+			y = state.accel_along();
 			break;
-		case data_mag_x:
-		case data_mag_y:
-		case data_mag_z:
-			AltosMag	mag = state.mag;
-			if (mag == null)
-				break;
-			switch (index) {
-			case data_mag_x:
-				y = mag.x;
-				break;
-			case data_mag_y:
-				y = mag.y;
-				break;
-			case data_mag_z:
-				y = mag.z;
-				break;
-			}
+		case data_accel_across:
+			y = state.accel_across();
+			break;
+		case data_accel_through:
+			y = state.accel_through();
+			break;
+		case data_gyro_roll:
+			y = state.gyro_roll();
+			break;
+		case data_gyro_pitch:
+			y = state.gyro_pitch();
+			break;
+		case data_gyro_yaw:
+			y = state.gyro_yaw();
+			break;
+		case data_mag_along:
+			y = state.mag_along();
+			break;
+		case data_mag_across:
+			y = state.mag_across();
+			break;
+		case data_mag_through:
+			y = state.mag_through();
 			break;
 		case data_orient:
 			y = state.orient();