From 4775b1d9b50a8732d66a0ad3b73ff74901a8cb7f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 Aug 2017 00:15:43 -0700 Subject: [PATCH] altos: Don't compute filtered average of height error when HAS_ACCEL We only use this for baro-only devices to avoid firing drogue charges at mach transitions; we trust the combination of accel+baro to do the right thing when available. Signed-off-by: Keith Packard --- src/kernel/ao_kalman.c | 6 ++++++ src/kernel/ao_sample.h | 2 ++ 2 files changed, 8 insertions(+) diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c index 82315c48..ac41085d 100644 --- a/src/kernel/ao_kalman.c +++ b/src/kernel/ao_kalman.c @@ -45,7 +45,9 @@ static __pdata ao_k_t ao_avg_height_scaled; __xdata ao_v_t ao_avg_height; __pdata ao_v_t ao_error_h; +#if !HAS_ACCEL __pdata ao_v_t ao_error_h_sq_avg; +#endif #if HAS_ACCEL __pdata ao_v_t ao_error_a; @@ -83,7 +85,9 @@ ao_kalman_predict(void) static void ao_kalman_err_height(void) { +#if !HAS_ACCEL ao_v_t e; +#endif ao_v_t height_distrust; #if HAS_ACCEL ao_v_t speed_distrust; @@ -91,6 +95,7 @@ ao_kalman_err_height(void) ao_error_h = ao_sample_height - (ao_v_t) (ao_k_height >> 16); +#if !HAS_ACCEL e = ao_error_h; if (e < 0) e = -e; @@ -98,6 +103,7 @@ ao_kalman_err_height(void) e = 127; ao_error_h_sq_avg -= ao_error_h_sq_avg >> 4; ao_error_h_sq_avg += (e * e) >> 4; +#endif if (ao_flight_state >= ao_flight_drogue) return; diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h index f89d6a4c..fbef031d 100644 --- a/src/kernel/ao_sample.h +++ b/src/kernel/ao_sample.h @@ -180,7 +180,9 @@ extern __xdata ao_v_t ao_max_height; /* max of ao_height */ extern __xdata ao_v_t ao_avg_height; /* running average of height */ extern __pdata ao_v_t ao_error_h; +#if !HAS_ACCEL extern __pdata ao_v_t ao_error_h_sq_avg; +#endif #if HAS_ACCEL extern __pdata ao_v_t ao_error_a; -- 2.30.2