From: Keith Packard <keithp@keithp.com>
Date: Sun, 11 Oct 2020 05:00:56 +0000 (-0700)
Subject: altos: De-bias height/speed data while on pad
X-Git-Tag: 1.9.6~1^2~22
X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=4370b7e7bc48d3f3f3ec94665449f1fde4e9567c

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

Save speed/height values from 64 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 2142546c..3c1067cb 100644
--- a/src/kernel/ao_flight.c
+++ b/src/kernel/ao_flight.c
@@ -102,6 +102,10 @@ uint8_t			ao_flight_force_idle;
 
 #define abs(a)	((a) < 0 ? -(a) : (a))
 
+#if !HAS_BARO
+// #define DEBUG_ACCEL_ONLY	1
+#endif
+
 void
 ao_flight(void)
 {
@@ -127,8 +131,8 @@ ao_flight(void)
 #if HAS_ACCEL
 			if (ao_config.accel_plus_g == 0 ||
 			    ao_config.accel_minus_g == 0 ||
-			    ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP ||
-			    ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP
+			    ao_ground_accel < (accel_t) ao_config.accel_plus_g - ACCEL_NOSE_UP ||
+			    ao_ground_accel > (accel_t) ao_config.accel_minus_g + ACCEL_NOSE_UP
 #if HAS_BARO
 			    || ao_ground_height < -1000 ||
 			    ao_ground_height > 7000
@@ -200,6 +204,15 @@ ao_flight(void)
 			ao_wakeup(&ao_flight_state);
 
 			break;
+
+#if DEBUG_ACCEL_ONLY
+		case ao_flight_invalid:
+		case ao_flight_idle:
+			printf("+g %d ga %d sa %d accel %ld speed %ld\n",
+			       ao_config.accel_plus_g, ao_ground_accel, ao_sample_accel, ao_accel, ao_speed);
+			break;
+#endif
+
 		case ao_flight_pad:
 			/* pad to boost:
 			 *
@@ -216,8 +229,8 @@ ao_flight(void)
 			 */
 			if (ao_height > AO_M_TO_HEIGHT(20)
 #if HAS_ACCEL
-			    || (ao_accel > AO_MSS_TO_ACCEL(20) &&
-				ao_speed > AO_MS_TO_SPEED(5))
+			    || (ao_accel > AO_MSS_TO_ACCEL(20)
+				&& 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..46771496 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 & 0x3f) == 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_ */