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

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