diff options
| author | Keith Packard <keithp@keithp.com> | 2017-04-27 23:59:27 -0700 |
|---|---|---|
| committer | Keith Packard <keithp@keithp.com> | 2017-04-27 23:59:27 -0700 |
| commit | 75c26a26e905e3d4f141badfcb6ec6995ed58213 (patch) | |
| tree | 7c2ea4dc73648527e1a997bf6d55c2b6feec2100 /src/kernel/ao_microkalman.c | |
| parent | 8c755f52db40bb845ab236ff400f35888053e616 (diff) | |
altos: Add 100ms constants for baro Kalman filter
MicroPeak v2 uses this rate.
Signed-off-by: Keith Packard <keithp@keithp.com>
Diffstat (limited to 'src/kernel/ao_microkalman.c')
| -rw-r--r-- | src/kernel/ao_microkalman.c | 32 |
1 files changed, 27 insertions, 5 deletions
diff --git a/src/kernel/ao_microkalman.c b/src/kernel/ao_microkalman.c index ff543cc4..3cfb87cc 100644 --- a/src/kernel/ao_microkalman.c +++ b/src/kernel/ao_microkalman.c @@ -32,10 +32,32 @@ #include <ao_kalman.h> +#ifndef AO_MK_STEP_96MS +#define AO_MK_STEP_96MS 1 + +#endif + +#if AO_MK_STEP_100MS +#define AO_MK_TIME_STEP 0.1 + +#define AO_K0_10 AO_MK2_BARO_K0_10 +#define AO_K1_10 AO_MK2_BARO_K1_10 +#define AO_K2_10 AO_MK2_BARO_K2_10 +#endif + +#if AO_MK_STEP_96MS +#define AO_MK_TIME_STEP 0.096 + +#define AO_K0_10 AO_MK_BARO_K0_10 +#define AO_K1_10 AO_MK_BARO_K1_10 +#define AO_K2_10 AO_MK_BARO_K2_10 +#endif + /* Basic time step (96ms) */ -#define AO_MK_STEP to_fix_v(0.096) +#define AO_MK_STEP to_fix_v(AO_MK_TIME_STEP) + /* step ** 2 / 2 */ -#define AO_MK_STEP_2_2 to_fix_v(0.004608) +#define AO_MK_STEP_2_2 to_fix_v(AO_MK_TIME_STEP * AO_MK_TIME_STEP / 2.0) uint32_t ao_k_pa; /* 24.8 fixed point */ int32_t ao_k_pa_speed; /* 16.16 fixed point */ @@ -66,9 +88,9 @@ ao_microkalman_correct(void) e = pa - from_fix8(ao_k_pa); - ao_k_pa += fix16_to_fix8((int32_t) e * AO_MK_BARO_K0_10); - ao_k_pa_speed += (int32_t) e * AO_MK_BARO_K1_10; - ao_k_pa_accel += (int32_t) e * AO_MK_BARO_K2_10; + ao_k_pa += fix16_to_fix8((int32_t) e * AO_K0_10); + ao_k_pa_speed += (int32_t) e * AO_K1_10; + ao_k_pa_accel += (int32_t) e * AO_K2_10; ao_pa = from_fix8(ao_k_pa); ao_pa_speed = from_fix(ao_k_pa_speed); ao_pa_accel = from_fix(ao_k_pa_accel); |