diff options
| author | Keith Packard <keithp@keithp.com> | 2011-08-01 22:33:38 -0700 |
|---|---|---|
| committer | Keith Packard <keithp@keithp.com> | 2011-08-02 00:30:08 -0700 |
| commit | e19a117b99e8374ca0e8e35948e23bc672ad1a32 (patch) | |
| tree | c079f4bf51fbca4ff00dd5232698cd6ecd247436 /src/ao_kalman.c | |
| parent | 146a0ab223e8d9b376125d1e59f597f6d7851a9b (diff) | |
altos: Average height values for landing detection
Instead of using the direct output of the kalman filter and hoping
that is quiet enough to detect landing, filter that with a long
exponential decay filter and then check to make sure that doesn't
change more than 2m in 5 seconds as a trigger for landing detection.
Tested with existing telemetrum flight logs and it correctly detects
landing in all cases.
Signed-off-by: Keith Packard <keithp@keithp.com>
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 } |