From 75c26a26e905e3d4f141badfcb6ec6995ed58213 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 27 Apr 2017 23:59:27 -0700 Subject: [PATCH] altos: Add 100ms constants for baro Kalman filter MicroPeak v2 uses this rate. Signed-off-by: Keith Packard --- src/kernel/ao_microkalman.c | 32 +++++++++++++++++++++++++++----- src/util/make-kalman | 3 ++- 2 files changed, 29 insertions(+), 6 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 +#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); diff --git a/src/util/make-kalman b/src/util/make-kalman index 580a4515..c630e9cb 100644 --- a/src/util/make-kalman +++ b/src/util/make-kalman @@ -40,4 +40,5 @@ nickle kalman.5c -p AO_BARO -c baro -t 0.01 $SIGMA_BARO nickle kalman.5c -p AO_BARO -c baro -t 0.1 $SIGMA_BARO nickle kalman.5c -p AO_BARO -c baro -t 1 $SIGMA_BARO -nickle kalman_micro.5c -p AO_MK_BARO -c baro -t 0.096 $SIGMA_MICRO \ No newline at end of file +nickle kalman_micro.5c -p AO_MK_BARO -c baro -t 0.096 $SIGMA_MICRO +nickle kalman_micro.5c -p AO_MK2_BARO -c baro -t 0.1 $SIGMA_MICRO -- 2.30.2