Merge branch 'branch-1.6' into debian

Conflicts:
	ChangeLog
	altoslib/AltosTelemetryReader.java
	configure.ac

1 files changed, 225 insertions, 20 deletions

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 5fce15c4..d363027c 100644
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -19,9 +19,11 @@
  * Track flight state from telemetry or eeprom data stream
  */
 
-package org.altusmetrum.altoslib_5;
+package org.altusmetrum.altoslib_6;
 
-public class AltosState implements Cloneable {
+import java.io.*;
+
+public class AltosState implements Cloneable, Serializable {
 
 	public static final int set_position = 1;
 	public static final int set_gps = 2;
@@ -43,9 +45,9 @@ public class AltosState implements Cloneable {
 	private int	prev_tick;
 	public int	boost_tick;
 
-	class AltosValue {
-		private double	value;
-		private double	prev_value;
+	class AltosValue implements Serializable{
+		double	value;
+		double	prev_value;
 		private double	max_value;
 		private double	set_time;
 		private double	prev_set_time;
@@ -179,9 +181,20 @@ public class AltosState implements Cloneable {
 		}
 	}
 
-	class AltosCValue {
-		AltosValue	measured;
-		AltosValue	computed;
+	class AltosCValue implements Serializable {
+
+		class AltosIValue extends AltosValue implements Serializable {
+			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 +276,8 @@ public class AltosState implements Cloneable {
 		}
 
 		AltosCValue() {
-			measured = new AltosValue();
-			computed = new AltosValue();
+			measured = new AltosIValue();
+			computed = new AltosIValue();
 		}
 	}
 
@@ -301,7 +314,7 @@ public class AltosState implements Cloneable {
 		ground_altitude.set_measured(a, time);
 	}
 
-	class AltosGpsGroundAltitude extends AltosValue {
+	class AltosGpsGroundAltitude extends AltosValue implements Serializable {
 		void set(double a, double t) {
 			super.set(a, t);
 			pad_alt = value();
@@ -325,7 +338,7 @@ public class AltosState implements Cloneable {
 		gps_ground_altitude.set(a, time);
 	}
 
-	class AltosGroundPressure extends AltosCValue {
+	class AltosGroundPressure extends AltosCValue implements Serializable {
 		void set_filtered(double p, double time) {
 			computed.set_filtered(p, time);
 			if (!is_measured())
@@ -348,7 +361,7 @@ public class AltosState implements Cloneable {
 		ground_pressure.set_measured(pressure, time);
 	}
 
-	class AltosAltitude extends AltosCValue {
+	class AltosAltitude extends AltosCValue implements Serializable {
 
 		private void set_speed(AltosValue v) {
 			if (!acceleration.is_measured() || !ascent)
@@ -370,7 +383,7 @@ public class AltosState implements Cloneable {
 
 	private AltosAltitude	altitude;
 
-	class AltosGpsAltitude extends AltosValue {
+	class AltosGpsAltitude extends AltosValue implements Serializable {
 
 		private void set_gps_height() {
 			double	a = value();
@@ -453,7 +466,7 @@ public class AltosState implements Cloneable {
 		return gps_speed.max();
 	}
 
-	class AltosPressure extends AltosValue {
+	class AltosPressure extends AltosValue implements Serializable {
 		void set(double p, double time) {
 			super.set(p, time);
 			if (state == AltosLib.ao_flight_pad)
@@ -523,7 +536,7 @@ public class AltosState implements Cloneable {
 		return AltosLib.MISSING;
 	}
 
-	class AltosSpeed extends AltosCValue {
+	class AltosSpeed extends AltosCValue implements Serializable {
 
 		boolean can_max() {
 			return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
@@ -577,7 +590,7 @@ public class AltosState implements Cloneable {
 		return AltosLib.MISSING;
 	}
 
-	class AltosAccel extends AltosCValue {
+	class AltosAccel extends AltosCValue implements Serializable {
 
 		boolean can_max() {
 			return state < AltosLib.ao_flight_fast || state == AltosLib.ao_flight_stateless;
@@ -600,10 +613,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 +726,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 +746,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 +891,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 +1163,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();