diff options
| author | Keith Packard <keithp@keithp.com> | 2011-08-02 01:49:35 -0700 |
|---|---|---|
| committer | Keith Packard <keithp@keithp.com> | 2011-08-02 01:53:33 -0700 |
| commit | 6c55bf35b11ae3ddae152795072d69e98184bac1 (patch) | |
| tree | 65e832f873863beb2beda91afd00726fc54e10f4 | |
| parent | e19a117b99e8374ca0e8e35948e23bc672ad1a32 (diff) | |
altos: Reduce height averaging filter time constant
Using the longer time constant could lead to false landing detection
just after apogee, which is definitely not a good idea.
Signed-off-by: Keith Packard <keithp@keithp.com>
| -rw-r--r-- | src/ao_kalman.c | 17 |
1 files changed, 7 insertions, 10 deletions
diff --git a/src/ao_kalman.c b/src/ao_kalman.c index ab97fc34..203d727a 100644 --- a/src/ao_kalman.c +++ b/src/ao_kalman.c @@ -277,18 +277,15 @@ ao_kalman(void) ao_accel = from_fix(ao_k_accel); if (ao_height > ao_max_height) ao_max_height = ao_height; + ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_height; #ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) { - ao_avg_height = ao_height; - } else if (ao_sample_tick - ao_sample_prev_tick > 5) { - ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_height; - ao_avg_height = (ao_avg_height_scaled + 3) >> 2; - } else + if (ao_sample_tick - ao_sample_prev_tick > 50) + ao_avg_height = (ao_avg_height_scaled + 1) >> 1; + else if (ao_sample_tick - ao_sample_prev_tick > 5) + ao_avg_height = (ao_avg_height_scaled + 7) >> 4; + else #endif - { - ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_height; - ao_avg_height = (ao_avg_height_scaled + 15) >> 5; - } + ao_avg_height = (ao_avg_height_scaled + 63) >> 7; #ifdef AO_FLIGHT_TEST ao_sample_prev_tick = ao_sample_tick; #endif |