altos: Don't eliminate baro above mach speed, just trust it less

Instead of completely eliminating the baro sensor above mach speed,
just derate it a bit so that the accel will dominate for speed
computation and keep the device from false-triggering across mach
transitions.

When we completely ignored the baro sensor above mach, and the flight
spent considerable time in that speed range, then the estimated height
could be far from the real value. When the estimated speed dropped
back down and the baro values were brought back into the computation,
then the resulting rapid shift in estimated speed could trigger
accidental apogee detection.

By mixing in a bit of baro data even above mach, we keep the estimated
height closer to the baro value and prevent this error, at least in
flights measured so far.

The flight known to have this problem is:

	2015-09-26-serial-2093-flight-0012.eeprom

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

diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index b0d911ed7f2663ac5b820cc0146d4944da9d2d85..f06125cd1329f361f7e91e92afcaadc2be420b38 100644 (file)
--- a/src/kernel/ao_flight.c
+++ b/src/kernel/ao_flight.c
@@ -282,9 +282,11 @@ ao_flight(void)
                         * the measured altitude reasonably closely; otherwise
                         * we're probably transsonic.
                         */
+#define AO_ERROR_BOUND 100
+
                        if (ao_speed < 0
 #if !HAS_ACCEL
-                           && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 100)
+                           && (ao_sample_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < AO_ERROR_BOUND)
 #endif
                                )
                        {

diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c
index 87f1bf66f6147e372edad8a94513802aa11c1774..82315c485de086b7f3b0cb64a462a878ac620119 100644 (file)
--- a/src/kernel/ao_kalman.c
+++ b/src/kernel/ao_kalman.c
@@ -103,13 +103,13 @@ ao_kalman_err_height(void)
                return;
        height_distrust = ao_sample_alt - AO_MAX_BARO_HEIGHT;
 #if HAS_ACCEL
-       /* speed is stored * 16, but we need to ramp between 200 and 328, so
+       /* speed is stored * 16, but we need to ramp between 248 and 328, so
         * we want to multiply by 2. The result is a shift by 3.
         */
        speed_distrust = (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) >> (4 - 1);
-       if (speed_distrust <= 0)
-               speed_distrust = 0;
-       else if (speed_distrust > height_distrust)
+       if (speed_distrust > AO_MAX_SPEED_DISTRUST)
+               speed_distrust = AO_MAX_SPEED_DISTRUST;
+       if (speed_distrust > height_distrust)
                height_distrust = speed_distrust;
 #endif
        if (height_distrust > 0) {

diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h
index da40187b0d7effd77aff60a49e63518c57e05007..f89d6a4ced254e0c7723eb002503987590b73d3d 100644 (file)
--- a/src/kernel/ao_sample.h
+++ b/src/kernel/ao_sample.h
@@ -95,7 +95,12 @@ typedef int16_t                      ao_v_t;
 /*
  * Above this speed, baro measurements are unreliable
  */
-#define AO_MAX_BARO_SPEED      200
+#define AO_MAX_BARO_SPEED      248
+
+/* The maximum amount (in a range of 0-256) to de-rate the
+ * baro sensor data based on speed.
+ */
+#define AO_MAX_SPEED_DISTRUST  160
 
 #define ACCEL_NOSE_UP  (ao_accel_2g >> 2)