altos: De-bias height/speed data while on pad

Save speed/height values from 128 samples ago and subtract them from
the current value. This reduces the effect of systematic accelerometer
error causing these values to slowly creep when there's no barometric
sensor to keep them in check.

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

diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index 4b6bfd8f7109be78233e25f13f2aa9317410bbbb..3f06535ffb3795601b5da8e1ff743225783e5c3a 100644 (file)
--- a/src/kernel/ao_flight.c
+++ b/src/kernel/ao_flight.c
@@ -251,10 +251,7 @@ ao_flight(void)
                        if (ao_height > AO_M_TO_HEIGHT(20)
 #if HAS_ACCEL
                            || (ao_accel > AO_MSS_TO_ACCEL(20)
-#if HAS_BARO
-                               && ao_speed > AO_MS_TO_SPEED(5)
-#endif
-)
+                               && ao_speed > AO_MS_TO_SPEED(5))
 #endif
                                )
                        {

diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c
index aaf0595f01ab7d5e345ab5997983386f78bd4c1e..f93f5aba94f1daa17d2830d7ce42969e6617111e 100644 (file)
--- a/src/kernel/ao_kalman.c
+++ b/src/kernel/ao_kalman.c
@@ -275,6 +275,25 @@ ao_kalman_correct_accel(void)
 #endif /* else FORCE_ACCEL */
 #endif /* HAS_ACCEL */
 
+#if !HAS_BARO
+static ao_k_t  ao_k_height_prev;
+static ao_k_t  ao_k_speed_prev;
+
+/*
+ * While in pad mode without a barometric sensor, remove accumulated
+ * speed and height values to reduce the effect of systematic sensor
+ * error
+ */
+void
+ao_kalman_reset_accumulate(void)
+{
+       ao_k_height -= ao_k_height_prev;
+       ao_k_speed -= ao_k_speed_prev;
+       ao_k_height_prev = ao_k_height;
+       ao_k_speed_prev = ao_k_speed;
+}
+#endif
+
 void
 ao_kalman(void)
 {

diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c
index 91cf113e3a6ae58880be274bec43653154e485fe..a7f5006590264a0a6d1e6dd348c87fb18e2136d7 100644 (file)
--- a/src/kernel/ao_sample.c
+++ b/src/kernel/ao_sample.c
@@ -322,6 +322,10 @@ ao_sample_preflight_update(void)
                ++nsamples;
        else
                ao_sample_preflight_set();
+#if !HAS_BARO
+       if ((nsamples & 0x7f) == 0)
+               ao_kalman_reset_accumulate();
+#endif
 }
 
 #if 0

diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h
index 04e97e7b29f0c309b1a1a6710cad314d18731dbb..475b3f6326ef8fc57301fcf1172c0310a14d262b 100644 (file)
--- a/src/kernel/ao_sample.h
+++ b/src/kernel/ao_sample.h
@@ -199,4 +199,8 @@ extern ao_v_t                       ao_error_a;
 
 void ao_kalman(void);
 
+#if !HAS_BARO
+void ao_kalman_reset_accumulate(void);
+#endif
+
 #endif /* _AO_SAMPLE_H_ */