From: Keith Packard <keithp@keithp.com>
Date: Sun, 11 Oct 2020 19:31:17 +0000 (-0700)
Subject: altos: De-bias height/speed data while on pad
X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=d55d7c56b81dba7043857a763d0749e93cf7823a

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

Save speed/height values from 128 samples ago and subtract them from
the current value. This reduces the effect of systematic accelerometer
error causing these values to slowly creep when there's no barometric
sensor to keep them in check.

Signed-off-by: Keith Packard <keithp@keithp.com>
---

diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index 4b6bfd8f..3f06535f 100644
--- a/src/kernel/ao_flight.c
+++ b/src/kernel/ao_flight.c
@@ -251,10 +251,7 @@ ao_flight(void)
 			if (ao_height > AO_M_TO_HEIGHT(20)
 #if HAS_ACCEL
 			    || (ao_accel > AO_MSS_TO_ACCEL(20)
-#if HAS_BARO
-				&& ao_speed > AO_MS_TO_SPEED(5)
-#endif
-)
+				&& ao_speed > AO_MS_TO_SPEED(5))
 #endif
 				)
 			{
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)
 {
diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c
index 91cf113e..a7f50065 100644
--- 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
diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h
index 04e97e7b..475b3f63 100644
--- a/src/kernel/ao_sample.h
+++ b/src/kernel/ao_sample.h
@@ -199,4 +199,8 @@ extern ao_v_t			ao_error_a;
 
 void ao_kalman(void);
 
+#if !HAS_BARO
+void ao_kalman_reset_accumulate(void);
+#endif
+
 #endif /* _AO_SAMPLE_H_ */