X-Git-Url: https://git.gag.com/?a=blobdiff_plain;f=src%2Fao_flight.c;h=88f0544f83a076ce4a702a966c23c0703c3376a9;hb=7ce8c9081e703d1405c2595ab9bda0cfa218c6c4;hp=c65670f0d4d5f8c521a06b23826a32f9e6fe44f7;hpb=7b009b2efe3af8722c358c304c2243652594e0d5;p=fw%2Faltos diff --git a/src/ao_flight.c b/src/ao_flight.c index c65670f0..88f0544f 100644 --- a/src/ao_flight.c +++ b/src/ao_flight.c @@ -135,12 +135,12 @@ static __pdata int32_t ao_k_accel; /* * Above this height, the baro sensor doesn't work */ -#define AO_MAX_BARO_HEIGHT 8000 +#define AO_MAX_BARO_HEIGHT 12000 /* * Above this speed, baro measurements are unreliable */ -#define AO_MAX_BARO_SPEED 300 +#define AO_MAX_BARO_SPEED 200 static void ao_kalman_predict(void) @@ -153,6 +153,11 @@ ao_kalman_predict(void) return; } + if (ao_flight_debug) { + printf ("predict speed %g + (%g * %g) = %g\n", + ao_k_speed / (65536.0 * 16.0), ao_accel / 16.0, AO_K_STEP_100 / 65536.0, + (ao_k_speed + (int32_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0)); + } #endif ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 + (int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4; @@ -168,6 +173,11 @@ static void ao_kalman_err_height(void) { int16_t e; + int16_t height_distrust; +#if HAS_ACCEL + int16_t speed_distrust; +#endif + ao_error_h = ao_raw_height - (int16_t) (ao_k_height >> 16); e = ao_error_h; @@ -175,8 +185,45 @@ ao_kalman_err_height(void) 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 + + height_distrust = ao_raw_height - AO_MAX_BARO_HEIGHT; +#if HAS_ACCEL + /* speed is stored * 16, but we need to ramp between 200 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) + height_distrust = speed_distrust; +#endif + if (height_distrust <= 0) + height_distrust = 0; + + if (height_distrust) { +#ifdef AO_FLIGHT_TEST + int old_ao_error_h = ao_error_h; +#endif + if (height_distrust > 0x100) + height_distrust = 0x100; + ao_error_h = (int16_t) (((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8); +#ifdef AO_FLIGHT_TEST + if (ao_flight_debug) { + printf("over height %g over speed %g distrust: %g height: error %d -> %d\n", + (double) (ao_raw_height - AO_MAX_BARO_HEIGHT), + (ao_speed - AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) / 16.0, + height_distrust / 256.0, + old_ao_error_h, ao_error_h); + } +#endif + } } static void @@ -217,35 +264,37 @@ ao_kalman_correct_both(void) ao_kalman_err_height(); ao_kalman_err_accel(); -#if 0 - /* - * Check to see if things are crazy here -- - * if the computed height is far above the - * measured height, we assume that the flight - * trajectory is not vertical, and so ignore - * the accelerometer for the remainder of the - * flight. - */ - if (ao_error_h_sq_avg > 10) - { - ao_kalman_correct_baro(); - return; - } -#endif - #ifdef AO_FLIGHT_TEST if (ao_flight_tick - ao_flight_prev_tick > 5) { + if (ao_flight_debug) { + printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", + ao_k_speed / (65536.0 * 16.0), + (double) ao_error_h, AO_BOTH_K10_10 / 65536.0, + (double) ao_error_a, AO_BOTH_K11_10 / 65536.0, + (ao_k_speed + + (int32_t) AO_BOTH_K10_10 * ao_error_h + + (int32_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0)); + } ao_k_height += (int32_t) AO_BOTH_K00_10 * ao_error_h + - (int32_t) (AO_BOTH_K01_10 >> 4) * ao_error_a; + (int32_t) AO_BOTH_K01_10 * ao_error_a; ao_k_speed += - ((int32_t) AO_BOTH_K10_10 << 4) * ao_error_h + + (int32_t) AO_BOTH_K10_10 * ao_error_h + (int32_t) AO_BOTH_K11_10 * ao_error_a; ao_k_accel += - ((int32_t) AO_BOTH_K20_10 << 4) * ao_error_h + + (int32_t) AO_BOTH_K20_10 * ao_error_h + (int32_t) AO_BOTH_K21_10 * ao_error_a; return; } + if (ao_flight_debug) { + printf ("correct speed %g + (%g * %g) + (%g * %g) = %g\n", + ao_k_speed / (65536.0 * 16.0), + (double) ao_error_h, AO_BOTH_K10_100 / 65536.0, + (double) ao_error_a, AO_BOTH_K11_100 / 65536.0, + (ao_k_speed + + (int32_t) AO_BOTH_K10_100 * ao_error_h + + (int32_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0)); + } #endif ao_k_height += (int32_t) AO_BOTH_K00_100 * ao_error_h + @@ -258,23 +307,23 @@ ao_kalman_correct_both(void) (int32_t) AO_BOTH_K21_100 * ao_error_a; } +#ifdef FORCE_ACCEL static void ao_kalman_correct_accel(void) { ao_kalman_err_accel(); -#ifdef AO_FLIGHT_TEST if (ao_flight_tick - ao_flight_prev_tick > 5) { ao_k_height +=(int32_t) AO_ACCEL_K0_10 * ao_error_a; ao_k_speed += (int32_t) AO_ACCEL_K1_10 * ao_error_a; ao_k_accel += (int32_t) AO_ACCEL_K2_10 * ao_error_a; return; } -#endif ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a; ao_k_speed += (int32_t) AO_ACCEL_K1_100 * ao_error_a; ao_k_accel += (int32_t) AO_ACCEL_K2_100 * ao_error_a; } +#endif #endif /* HAS_ACCEL */ __xdata int32_t ao_raw_pres_sum; @@ -407,16 +456,13 @@ ao_flight(void) ao_kalman_predict(); #if HAS_ACCEL if (ao_flight_state <= ao_flight_coast) { -#ifndef FORCE_ACCEL - if (/*ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED) &&*/ - ao_raw_alt < AO_MAX_BARO_HEIGHT) - ao_kalman_correct_both(); - else +#ifdef FORCE_ACCEL + ao_kalman_correct_accel(); +#else + ao_kalman_correct_both(); #endif - ao_kalman_correct_accel(); } else #endif - if (ao_raw_alt < AO_MAX_BARO_HEIGHT || ao_flight_state >= ao_flight_drogue) ao_kalman_correct_baro(); ao_height = from_fix(ao_k_height); ao_speed = from_fix(ao_k_speed); @@ -564,23 +610,30 @@ ao_flight(void) if ((ao_accel < AO_MSS_TO_ACCEL(-2.5) && ao_height > AO_M_TO_HEIGHT(100)) || (int16_t) (ao_flight_tick - ao_launch_tick) > BOOST_TICKS_MAX) { +#if HAS_ACCEL ao_flight_state = ao_flight_fast; +#else + ao_flight_state = ao_flight_coast; +#endif ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); break; } break; +#if HAS_ACCEL case ao_flight_fast: /* * This is essentially the same as coast, * but the barometer is being ignored as * it may be unreliable. */ - if (ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) { + if (ao_speed < AO_MS_TO_SPEED(AO_MAX_BARO_SPEED)) + { ao_flight_state = ao_flight_coast; ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); break; } break; +#endif case ao_flight_coast: /* apogee detect: coast to drogue deploy: @@ -591,7 +644,11 @@ ao_flight(void) * the measured altitude reasonably closely; otherwise * we're probably transsonic. */ - if (ao_speed < 0 && (ao_raw_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 100)) + if (ao_speed < 0 +#if !HAS_ACCEL + && (ao_raw_alt >= AO_MAX_BARO_HEIGHT || ao_error_h_sq_avg < 100) +#endif + ) { /* ignite the drogue charge */ ao_ignite(ao_igniter_drogue);