X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fkernel%2Fao_kalman.c;fp=src%2Fkernel%2Fao_kalman.c;h=f93f5aba94f1daa17d2830d7ce42969e6617111e;hp=aaf0595f01ab7d5e345ab5997983386f78bd4c1e;hb=4370b7e7bc48d3f3f3ec94665449f1fde4e9567c;hpb=a7d8a0f8b26bf62b63f12110d6d66a4f9da76eae

diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c
index aaf0595f..f93f5aba 100644
--- 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)
 {