diff options
| -rw-r--r-- | src/kernel/ao_flight.c | 4 | ||||
| -rw-r--r-- | src/kernel/ao_kalman.c | 8 | ||||
| -rw-r--r-- | src/kernel/ao_sample.h | 7 |
3 files changed, 13 insertions, 6 deletions
diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index b0d911ed..f06125cd 100644 --- 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 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) { diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h index da40187b..f89d6a4c 100644 --- 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) |