X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=altoslib%2FAltosState.java;h=758fd6366b7b7f40d1b3967bcc24bdcb8f4d9313;hb=324073d14ed8215b74d9df51f2229827cd4401d0;hp=134aeb4e34eaa3929a86b6e91cd49076a6b2aa95;hpb=053d092b3dca4ebb98e97ec0fe24f5f5e1c31f88;p=fw%2Faltos

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 134aeb4e..758fd636 100644
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -299,6 +299,30 @@ public class AltosState implements Cloneable {
 		ground_altitude.set_measured(a, time);
 	}
 
+	class AltosGpsGroundAltitude extends AltosValue {
+		void set(double a, double t) {
+			super.set(a, t);
+			pad_alt = value();
+			gps_altitude.set_gps_height();
+		}
+
+		void set_filtered(double a, double t) {
+			super.set_filtered(a, t);
+			pad_alt = value();
+			gps_altitude.set_gps_height();
+		}
+	}
+
+	private AltosGpsGroundAltitude gps_ground_altitude;
+
+	public double gps_ground_altitude() {
+		return gps_ground_altitude.value();
+	}
+
+	public void set_gps_ground_altitude(double a) {
+		gps_ground_altitude.set(a, time);
+	}
+
 	class AltosGroundPressure extends AltosCValue {
 		void set_filtered(double p, double time) {
 			computed.set_filtered(p, time);
@@ -344,26 +368,56 @@ public class AltosState implements Cloneable {
 
 	private AltosAltitude	altitude;
 
+	class AltosGpsAltitude extends AltosValue {
+
+		private void set_gps_height() {
+			double	a = value();
+			double	g = gps_ground_altitude.value();
+
+			if (a != AltosLib.MISSING && g != AltosLib.MISSING)
+				gps_height = a - g;
+			else
+				gps_height = AltosLib.MISSING;
+		}
+
+		void set(double a, double t) {
+			super.set(a, t);
+			set_gps_height();
+		}
+	}
+
+	private AltosGpsAltitude	gps_altitude;
+
 	public double altitude() {
 		double a = altitude.value();
 		if (a != AltosLib.MISSING)
 			return a;
-		if (gps != null)
-			return gps.alt;
-		return AltosLib.MISSING;
+		return gps_altitude.value();
 	}
 
 	public double max_altitude() {
 		double a = altitude.max();
 		if (a != AltosLib.MISSING)
 			return a;
-		return AltosLib.MISSING;
+		return gps_altitude.max();
 	}
 
 	public void set_altitude(double new_altitude) {
 		altitude.set_measured(new_altitude, time);
 	}
 
+	public double gps_altitude() {
+		return gps_altitude.value();
+	}
+
+	public double max_gps_altitude() {
+		return gps_altitude.max();
+	}
+
+	public void set_gps_altitude(double new_gps_altitude) {
+		gps_altitude.set(new_gps_altitude, time);
+	}
+
 	class AltosPressure extends AltosValue {
 		void set(double p, double time) {
 			super.set(p, time);
@@ -408,6 +462,24 @@ public class AltosState implements Cloneable {
 		return AltosLib.MISSING;
 	}
 
+	public double gps_height() {
+		double a = gps_altitude();
+		double g = gps_ground_altitude();
+
+		if (a != AltosLib.MISSING && g != AltosLib.MISSING)
+			return a - g;
+		return AltosLib.MISSING;
+	}
+
+	public double max_gps_height() {
+		double a = gps_altitude.max();
+		double g = gps_ground_altitude();
+
+		if (a != AltosLib.MISSING && g != AltosLib.MISSING)
+			return a - g;
+		return AltosLib.MISSING;
+	}
+
 	class AltosSpeed extends AltosCValue {
 		
 		void set_accel() {
@@ -608,6 +680,9 @@ public class AltosState implements Cloneable {
 		pad_lon = AltosLib.MISSING;
 		pad_alt = AltosLib.MISSING;
 
+		gps_altitude = new AltosGpsAltitude();
+		gps_ground_altitude = new AltosGpsGroundAltitude();
+
 		speak_tick = AltosLib.MISSING;
 		speak_altitude = AltosLib.MISSING;
 
@@ -730,6 +805,10 @@ public class AltosState implements Cloneable {
 		range = old.range;
 
 		gps_height = old.gps_height;
+
+		gps_altitude.copy(old.gps_altitude);
+		gps_ground_altitude.copy(old.gps_ground_altitude);
+
 		pad_lat = old.pad_lat;
 		pad_lon = old.pad_lon;
 		pad_alt = old.pad_alt;
@@ -759,7 +838,6 @@ public class AltosState implements Cloneable {
 	void update_gps() {
 		elevation = 0;
 		range = -1;
-		gps_height = 0;
 
 		if (gps == null)
 			return;
@@ -771,14 +849,15 @@ public class AltosState implements Cloneable {
 				if (pad_lat != AltosLib.MISSING) {
 					pad_lat = (pad_lat * 31 + gps.lat) / 32;
 					pad_lon = (pad_lon * 31 + gps.lon) / 32;
-					pad_alt = (pad_alt * 31 + gps.alt) / 32;
+					gps_ground_altitude.set_filtered(gps.alt, time);
 				}
 			}
 			if (pad_lat == AltosLib.MISSING) {
 				pad_lat = gps.lat;
 				pad_lon = gps.lon;
-				pad_alt = gps.alt;
+				gps_ground_altitude.set(gps.alt, time);
 			}
+			gps_altitude.set(gps.alt, time);
 		}
 		if (gps.lat != 0 && gps.lon != 0 &&
 		    pad_lat != AltosLib.MISSING &&
@@ -791,7 +870,6 @@ public class AltosState implements Cloneable {
 			from_pad = new AltosGreatCircle(pad_lat, pad_lon, 0, gps.lat, gps.lon, h);
 			elevation = from_pad.elevation;
 			range = from_pad.range;
-			gps_height = gps.alt - pad_alt;
 		}
 	}
 
@@ -949,14 +1027,8 @@ public class AltosState implements Cloneable {
 	}
 
 	void update_accel() {
-		double	ground = ground_accel;
-
-		if (ground == AltosLib.MISSING)
-			ground = ground_accel_avg;
 		if (accel == AltosLib.MISSING)
 			return;
-		if (ground == AltosLib.MISSING)
-			return;
 		if (accel_plus_g == AltosLib.MISSING)
 			return;
 		if (accel_minus_g == AltosLib.MISSING)
@@ -964,7 +1036,7 @@ public class AltosState implements Cloneable {
 
 		double counts_per_g = (accel_minus_g - accel_plus_g) / 2.0;
 		double counts_per_mss = counts_per_g / 9.80665;
-		acceleration.set_measured((ground - accel) / counts_per_mss, time);
+		acceleration.set_measured((accel_plus_g - accel) / counts_per_mss, time);
 	}
 
 	public void set_accel_g(double accel_plus_g, double accel_minus_g) {
@@ -976,10 +1048,8 @@ public class AltosState implements Cloneable {
 	}
 
 	public void set_ground_accel(double ground_accel) {
-		if (ground_accel != AltosLib.MISSING) {
+		if (ground_accel != AltosLib.MISSING)
 			this.ground_accel = ground_accel;
-			update_accel();
-		}
 	}
 
 	public void set_accel(double accel) {