altoslib: Clear sat data when tick changes

[fw/altos] / altoslib / AltosState.java

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 7817c76ad32d8a37f6cc136191c8ad74df19b90c..e32a1fe5111f4650774013ecb2c707376be226d5 100644 (file)
--- 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;
        }