From 6ee99c1861ef1898a77aead41d80383e697bd248 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 3 Sep 2013 17:38:20 -0600 Subject: [PATCH] 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 --- altoslib/AltosState.java | 48 ++++++++++++++++++++++++++-------------- 1 file changed, 31 insertions(+), 17 deletions(-) diff --git a/altoslib/AltosState.java b/altoslib/AltosState.java index 7817c76a..a920d425 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 */ @@ -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) { -- 2.30.2