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..46771496b69a4745cd6ea24597f9fa048c7243ba 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 & 0x3f) == 0)
+               ao_kalman_reset_accumulate();
+#endif
 }
 
 #if 0