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 2142546c9b0737b186cca6f6dce65fa51c8e30ec..3c1067cb44e9e079675d863a2eb38368e096e733 100644 (file)
--- 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 aaf0595f01ab7d5e345ab5997983386f78bd4c1e..f93f5aba94f1daa17d2830d7ce42969e6617111e 100644 (file)
--- 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 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

diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h
index 04e97e7b29f0c309b1a1a6710cad314d18731dbb..475b3f6326ef8fc57301fcf1172c0310a14d262b 100644 (file)
--- 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_ */