X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fkernel%2Fao_kalman.c;h=5f32eb7275de8357d8bb50a80680644dee4817e9;hb=8a95afdd417119da6089403f870959a726b0c548;hp=69a1b3de6ddc9a32350605e7e31f19b9b4c198da;hpb=1085ec5d57e0ed5d132f2bbdac1a0b6a32c0ab4a;p=fw%2Faltos diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c index 69a1b3de..5f32eb72 100644 --- a/src/kernel/ao_kalman.c +++ b/src/kernel/ao_kalman.c @@ -24,9 +24,9 @@ #include "ao_sample.h" #include "ao_kalman.h" -static __pdata ao_k_t ao_k_height; -static __pdata ao_k_t ao_k_speed; -static __pdata ao_k_t ao_k_accel; +static ao_k_t ao_k_height; +static ao_k_t ao_k_speed; +static ao_k_t ao_k_accel; #define AO_K_STEP_100 to_fix_v(0.01) #define AO_K_STEP_2_2_100 to_fix_v(0.00005) @@ -37,32 +37,38 @@ static __pdata ao_k_t ao_k_accel; #define AO_K_STEP_1 to_fix_v(1) #define AO_K_STEP_2_2_1 to_fix_v(0.5) -__pdata ao_v_t ao_height; -__pdata ao_v_t ao_speed; -__pdata ao_v_t ao_accel; -__xdata ao_v_t ao_max_height; -static __pdata ao_k_t ao_avg_height_scaled; -__xdata ao_v_t ao_avg_height; +ao_v_t ao_height; +ao_v_t ao_speed; +ao_v_t ao_accel; +ao_v_t ao_max_height; +static ao_k_t ao_avg_height_scaled; +ao_v_t ao_avg_height; -__pdata ao_v_t ao_error_h; -__pdata ao_v_t ao_error_h_sq_avg; +ao_v_t ao_error_h; +#if !HAS_ACCEL || AO_FLIGHT_TEST +#define AO_ERROR_H_SQ_AVG 1 +#endif + +#if AO_ERROR_H_SQ_AVG +ao_v_t ao_error_h_sq_avg; +#endif #if HAS_ACCEL -__pdata ao_v_t ao_error_a; +ao_v_t ao_error_a; #endif static void ao_kalman_predict(void) { #ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) { + if ((AO_TICK_SIGNED) (ao_sample_tick - ao_sample_prev_tick) > 50) { ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_1 + (ao_k_t) ao_accel * AO_K_STEP_2_2_1) >> 4; ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_1; return; } - if (ao_sample_tick - ao_sample_prev_tick > 5) { + if ((AO_TICK_SIGNED) (ao_sample_tick - ao_sample_prev_tick) > 5) { ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_10 + (ao_k_t) ao_accel * AO_K_STEP_2_2_10) >> 4; ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_10; @@ -80,10 +86,13 @@ ao_kalman_predict(void) ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_100; } +#if HAS_BARO static void ao_kalman_err_height(void) { +#if AO_ERROR_H_SQ_AVG ao_v_t e; +#endif ao_v_t height_distrust; #if HAS_ACCEL ao_v_t speed_distrust; @@ -91,15 +100,12 @@ ao_kalman_err_height(void) ao_error_h = ao_sample_height - (ao_v_t) (ao_k_height >> 16); +#if AO_ERROR_H_SQ_AVG e = ao_error_h; if (e < 0) e = -e; if (e > 127) e = 127; -#if HAS_ACCEL - ao_error_h_sq_avg -= ao_error_h_sq_avg >> 2; - ao_error_h_sq_avg += (e * e) >> 2; -#else ao_error_h_sq_avg -= ao_error_h_sq_avg >> 4; ao_error_h_sq_avg += (e * e) >> 4; #endif @@ -108,13 +114,13 @@ ao_kalman_err_height(void) return; height_distrust = ao_sample_alt - AO_MAX_BARO_HEIGHT; #if HAS_ACCEL - /* speed is stored * 16, but we need to ramp between 200 and 328, so + /* speed is stored * 16, but we need to ramp between 248 and 328, so * we want to multiply by 2. The result is a shift by 3. */ speed_distrust = (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) >> (4 - 1); - if (speed_distrust <= 0) - speed_distrust = 0; - else if (speed_distrust > height_distrust) + if (speed_distrust > AO_MAX_SPEED_DISTRUST) + speed_distrust = AO_MAX_SPEED_DISTRUST; + if (speed_distrust > height_distrust) height_distrust = speed_distrust; #endif if (height_distrust > 0) { @@ -135,19 +141,21 @@ ao_kalman_err_height(void) #endif } } +#endif +#if HAS_BARO static void ao_kalman_correct_baro(void) { ao_kalman_err_height(); #ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) { + if ((AO_TICK_SIGNED) (ao_sample_tick - ao_sample_prev_tick) > 50) { ao_k_height += (ao_k_t) AO_BARO_K0_1 * ao_error_h; ao_k_speed += (ao_k_t) AO_BARO_K1_1 * ao_error_h; ao_k_accel += (ao_k_t) AO_BARO_K2_1 * ao_error_h; return; } - if (ao_sample_tick - ao_sample_prev_tick > 5) { + if ((AO_TICK_SIGNED) (ao_sample_tick - ao_sample_prev_tick) > 5) { ao_k_height += (ao_k_t) AO_BARO_K0_10 * ao_error_h; ao_k_speed += (ao_k_t) AO_BARO_K1_10 * ao_error_h; ao_k_accel += (ao_k_t) AO_BARO_K2_10 * ao_error_h; @@ -158,6 +166,7 @@ ao_kalman_correct_baro(void) ao_k_speed += (ao_k_t) AO_BARO_K1_100 * ao_error_h; ao_k_accel += (ao_k_t) AO_BARO_K2_100 * ao_error_h; } +#endif #if HAS_ACCEL @@ -169,10 +178,10 @@ ao_kalman_err_accel(void) accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale; /* Can't use ao_accel here as it is the pre-prediction value still */ - ao_error_a = (accel - ao_k_accel) >> 16; + ao_error_a = (ao_v_t) ((accel - ao_k_accel) >> 16); } -#ifndef FORCE_ACCEL +#if !defined(FORCE_ACCEL) && HAS_BARO static void ao_kalman_correct_both(void) { @@ -180,7 +189,7 @@ ao_kalman_correct_both(void) ao_kalman_err_accel(); #ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) { + if ((AO_TICK_SIGNED) (ao_sample_tick - ao_sample_prev_tick) > 50) { if (ao_flight_debug) { printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", ao_k_speed / (65536.0 * 16.0), @@ -201,7 +210,7 @@ ao_kalman_correct_both(void) (ao_k_t) AO_BOTH_K21_1 * ao_error_a; return; } - if (ao_sample_tick - ao_sample_prev_tick > 5) { + if ((AO_TICK_SIGNED) (ao_sample_tick - ao_sample_prev_tick) > 5) { if (ao_flight_debug) { printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", ao_k_speed / (65536.0 * 16.0), @@ -250,12 +259,14 @@ ao_kalman_correct_accel(void) { ao_kalman_err_accel(); - if (ao_sample_tick - ao_sample_prev_tick > 5) { +#ifdef AO_FLIGHT_TEST + if ((AO_TICK_SIGNED) (ao_sample_tick - ao_sample_prev_tick) > 5) { ao_k_height +=(ao_k_t) AO_ACCEL_K0_10 * ao_error_a; ao_k_speed += (ao_k_t) AO_ACCEL_K1_10 * ao_error_a; ao_k_accel += (ao_k_t) AO_ACCEL_K2_10 * ao_error_a; return; } +#endif ao_k_height += (ao_k_t) AO_ACCEL_K0_100 * ao_error_a; ao_k_speed += (ao_k_t) AO_ACCEL_K1_100 * ao_error_a; ao_k_accel += (ao_k_t) AO_ACCEL_K2_100 * ao_error_a; @@ -264,10 +275,30 @@ ao_kalman_correct_accel(void) #endif /* else FORCE_ACCEL */ #endif /* HAS_ACCEL */ +#if !HAS_BARO +static ao_k_t ao_k_height_prev; +static ao_k_t ao_k_speed_prev; + +/* + * While in pad mode without a barometric sensor, remove accumulated + * speed and height values to reduce the effect of systematic sensor + * error + */ +void +ao_kalman_reset_accumulate(void) +{ + ao_k_height -= ao_k_height_prev; + ao_k_speed -= ao_k_speed_prev; + ao_k_height_prev = ao_k_height; + ao_k_speed_prev = ao_k_speed; +} +#endif + void ao_kalman(void) { ao_kalman_predict(); +#if HAS_BARO #if HAS_ACCEL if (ao_flight_state <= ao_flight_coast) { #ifdef FORCE_ACCEL @@ -278,18 +309,27 @@ ao_kalman(void) } else #endif ao_kalman_correct_baro(); - ao_height = from_fix(ao_k_height); - ao_speed = from_fix(ao_k_speed); - ao_accel = from_fix(ao_k_accel); +#else +#if HAS_ACCEL + ao_kalman_correct_accel(); +#endif +#endif + ao_height = (ao_v_t) from_fix(ao_k_height); + ao_speed = (ao_v_t) from_fix(ao_k_speed); + ao_accel = (ao_v_t) from_fix(ao_k_accel); if (ao_height > ao_max_height) ao_max_height = ao_height; +#if HAS_BARO ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_sample_height; +#else + ao_avg_height_scaled = ao_avg_height_scaled - ao_avg_height + ao_height; +#endif #ifdef AO_FLIGHT_TEST - if (ao_sample_tick - ao_sample_prev_tick > 50) + if ((AO_TICK_SIGNED) (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) + else if ((AO_TICK_SIGNED) (ao_sample_tick - ao_sample_prev_tick) > 5) ao_avg_height = (ao_avg_height_scaled + 7) >> 4; else #endif - ao_avg_height = (ao_avg_height_scaled + 63) >> 7; + ao_avg_height = (ao_v_t) ((ao_avg_height_scaled + 63) >> 7); }