diff options
Diffstat (limited to 'src/ao_kalman.c')
| -rw-r--r-- | src/ao_kalman.c | 14 |
1 files changed, 14 insertions, 0 deletions
diff --git a/src/ao_kalman.c b/src/ao_kalman.c index 4e6cbb06..ab97fc34 100644 --- a/src/ao_kalman.c +++ b/src/ao_kalman.c @@ -38,6 +38,8 @@ __pdata int16_t ao_height; __pdata int16_t ao_speed; __pdata int16_t ao_accel; __pdata int16_t ao_max_height; +static __pdata int32_t ao_avg_height_scaled; +__pdata int16_t ao_avg_height; __pdata int16_t ao_error_h; __pdata int16_t ao_error_h_sq_avg; @@ -276,6 +278,18 @@ ao_kalman(void) if (ao_height > ao_max_height) ao_max_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 +#endif + { + ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_height; + ao_avg_height = (ao_avg_height_scaled + 15) >> 5; + } +#ifdef AO_FLIGHT_TEST ao_sample_prev_tick = ao_sample_tick; #endif } |