X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=altoslib%2FAltosState.java;h=e32a1fe5111f4650774013ecb2c707376be226d5;hp=7817c76ad32d8a37f6cc136191c8ad74df19b90c;hb=96a651cc1b81b30f4cbde454e34cf80ed8825945;hpb=528e2e41112cad8a81bccbb89c3bd202b818a506

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 7817c76a..e32a1fe5 100644
--- a/altoslib/AltosState.java
+++ b/altoslib/AltosState.java
@@ -30,7 +30,8 @@ public class AltosState implements Cloneable {
 
 	public int set;
 
-	static final double filter_len = 0.5;
+	static final double ascent_filter_len = 0.1;
+	static final double descent_filter_len = 2.0;
 
 	/* derived data */
 
@@ -61,6 +62,12 @@ public class AltosState implements Cloneable {
 			}
 		}
 
+		void set_filtered(double new_value, double time) {
+			if (prev_value != AltosRecord.MISSING)
+				new_value = (prev_value * 15.0 + new_value) / 16.0;
+			set(new_value, time);
+		}
+
 		double value() {
 			return value;
 		}
@@ -102,36 +109,53 @@ public class AltosState implements Cloneable {
 		}
 
 		void set_derivative(AltosValue in) {
-			double	new_value = in.rate();
+			double	n = in.rate();
 			
-			if (new_value == AltosRecord.MISSING)
+			if (n == AltosRecord.MISSING)
 				return;
 
+			double	p = prev_value;
+			double	pt = prev_set_time;
+
+			if (p == AltosRecord.MISSING) {
+				p = 0;
+				pt = in.time() - 0.01;
+			}
+
 			/* Clip changes to reduce noise */
-			if (prev_value != AltosRecord.MISSING) {
-				double	ddt = in.time() - prev_set_time;
-				double	ddv = (new_value - prev_value) / ddt;
+			double	ddt = in.time() - pt;
+			double	ddv = (n - p) / ddt;
 				
-				/* 100gs */
-				if (Math.abs(ddv) > 1000) {
-					if (new_value > prev_value)
-						new_value = prev_value + ddt * 1000;
-					else
-						new_value = prev_value - ddt * 1000;
-				}
-
-				double f = 1/Math.exp(ddt/ filter_len);
-				new_value = prev_value * f + new_value * (1-f);
+			/* 100gs */
+			if (Math.abs(ddv) > 1000) {
+				if (n > p)
+					n = p + ddt * 1000;
+				else
+					n = p - ddt * 1000;
 			}
 
-			set(new_value, in.time());
+			double filter_len;
+
+			if (ascent)
+				filter_len = ascent_filter_len;
+			else
+				filter_len = descent_filter_len;
+
+			double f = 1/Math.exp(ddt/ filter_len);
+			n = p * f + n * (1-f);
+
+			set(n, in.time());
 		}
 
 		void set_integral(AltosValue in) {
 			double	change = in.integrate();
 
-			if (change != AltosRecord.MISSING)
-				set(prev_value + change, in.time());
+			if (change != AltosRecord.MISSING) {
+				double	prev = prev_value;
+				if (prev == AltosRecord.MISSING)
+					prev = 0;
+				set(prev + change, in.time());
+			}
 		}
 
 		void copy(AltosValue old) {
@@ -264,9 +288,14 @@ public class AltosState implements Cloneable {
 		ground_altitude.set_measured(a, time);
 	}
 
-	class AltosGroundPressure extends AltosValue {
-		void set(double p, double time) {
-			super.set(p, time);
+	class AltosGroundPressure extends AltosCValue {
+		void set_filtered(double p, double time) {
+			computed.set_filtered(p, time);
+			ground_altitude.set_computed(pressure_to_altitude(computed.value()), time);
+		}
+
+		void set_measured(double p, double time) {
+			super.set_measured(p, time);
 			ground_altitude.set_computed(pressure_to_altitude(p), time);
 		}
 	}
@@ -278,7 +307,7 @@ public class AltosState implements Cloneable {
 	}
 
 	public void set_ground_pressure (double pressure) {
-		ground_pressure.set(pressure, time);
+		ground_pressure.set_measured(pressure, time);
 	}
 
 	class AltosAltitude extends AltosCValue {
@@ -326,7 +355,10 @@ public class AltosState implements Cloneable {
 	class AltosPressure extends AltosValue {
 		void set(double p, double time) {
 			super.set(p, time);
-			altitude.set_computed(pressure_to_altitude(p), time);
+			if (state == AltosLib.ao_flight_pad)
+				ground_pressure.set_filtered(p, time);
+			double a = pressure_to_altitude(p);
+			altitude.set_computed(a, time);
 		}
 	}
 
@@ -432,7 +464,7 @@ public class AltosState implements Cloneable {
 
 	public AltosGPS	gps;
 	public AltosGPS	temp_gps;
-	public boolean	temp_gps_clear_sats_pending;
+	public int temp_gps_sat_tick;
 	public boolean	gps_pending;
 	public int gps_sequence;
 
@@ -526,7 +558,7 @@ public class AltosState implements Cloneable {
 
 		gps = null;
 		temp_gps = null;
-		temp_gps_clear_sats_pending = false;
+		temp_gps_sat_tick = 0;
 		gps_sequence = 0;
 		gps_pending = false;
 
@@ -621,7 +653,7 @@ public class AltosState implements Cloneable {
 			temp_gps = old.temp_gps.clone();
 		else
 			temp_gps = null;
-		temp_gps_clear_sats_pending = old.temp_gps_clear_sats_pending;
+		temp_gps_sat_tick = old.temp_gps_sat_tick;
 		gps_sequence = old.gps_sequence;
 		gps_pending = old.gps_pending;
 
@@ -941,11 +973,10 @@ public class AltosState implements Cloneable {
 			temp_gps = new AltosGPS(gps);
 		}
 		gps_pending = true;
-		if (!sats)
-			temp_gps_clear_sats_pending = true;
-		else if (temp_gps_clear_sats_pending) {
-			temp_gps.cc_gps_sat = null;
-			temp_gps_clear_sats_pending = false;
+		if (sats) {
+			if (tick != temp_gps_sat_tick)
+				temp_gps.cc_gps_sat = null;
+			temp_gps_sat_tick = tick;
 		}
 		return temp_gps;
 	}