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

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

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