altoslib: Make Ascent/descent use different filter values. Always filter.

In derivative code, use a shorter filter during ascent as the baro
sensor is cleaner then. Then, make sure to always filter the values as
the very first few baro samples can be noisy, which generates a bad
starting speed.

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

diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java
index 7817c76ad32d8a37f6cc136191c8ad74df19b90c..a920d4250caaf0eceeeb0ebe42b8e4d8b356a417 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 */
 
@@ -102,29 +103,42 @@ 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) {