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

[fw/altos] / src / kernel / ao_kalman.c

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)
 {