diff options
| -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 |