From ea6fe21d78744d7e6225a56c369d54f7cd956767 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 26 Aug 2017 19:16:47 -0700 Subject: 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 --- src/kernel/ao_kalman.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/kernel/ao_kalman.c') diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c index 87f1bf66..82315c48 100644 --- 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) { -- cgit v1.2.3