From 62eae8a17d870e8ac6937ba23da01a5fbc652c6c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 18 Mar 2011 16:53:11 -0700 Subject: [PATCH] altos: Add kalman filters for baro-only boards This adds a baro-only kalman filter to track the state of the rocket, and then uses it to control flight events instead of the existing ad-hoc mechanisms. Signed-off-by: Keith Packard --- src/ao_flight.c | 104 +++++++++++++++++++++++++++++++++++++++++-- src/ao_flight_test.c | 70 +++++++++++++++++++++++++---- src/ao_pins.h | 4 ++ 3 files changed, 165 insertions(+), 13 deletions(-) diff --git a/src/ao_flight.c b/src/ao_flight.c index 8e370c4f..e8130baa 100644 --- a/src/ao_flight.c +++ b/src/ao_flight.c @@ -31,6 +31,10 @@ #error Please define HAS_USB #endif +#ifndef USE_KALMAN +#error Please define USE_KALMAN +#endif + /* Main flight thread. */ __pdata enum ao_flight_state ao_flight_state; /* current flight state */ @@ -150,6 +154,62 @@ __pdata int16_t ao_old_vel_tick; __xdata int32_t ao_raw_accel_sum; #endif +#if USE_KALMAN +__pdata int16_t ao_ground_height; +__pdata int32_t ao_k_max_height; +__pdata int32_t ao_k_height; +__pdata int32_t ao_k_speed; +__pdata int32_t ao_k_accel; + +#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) +#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) + +#define from_fix(x) ((x) >> 16) + +#define AO_K0_100 to_fix16(0.05680323) +#define AO_K1_100 to_fix16(0.16608182) +#define AO_K2_100 to_fix16(0.24279580) + +#define AO_K_STEP_100 to_fix16(0.01) +#define AO_K_STEP_2_2_100 to_fix16(0.00005) + +#define AO_K0_10 to_fix16(0.23772023) +#define AO_K1_10 to_fix16(0.32214149) +#define AO_K2_10 to_fix16(0.21827159) + +#define AO_K_STEP_10 to_fix16(0.1) +#define AO_K_STEP_2_2_10 to_fix16(0.005) + +static void +ao_kalman_baro(void) +{ + int16_t err = ((ao_pres_to_altitude(ao_raw_pres) - ao_ground_height)) + - (int16_t) (ao_k_height >> 16); + +#ifdef AO_FLIGHT_TEST + if (ao_flight_tick - ao_flight_prev_tick > 5) { + ao_k_height += ((ao_k_speed >> 16) * AO_K_STEP_10 + + (ao_k_accel >> 16) * AO_K_STEP_2_2_10); + ao_k_speed += (ao_k_accel >> 16) * AO_K_STEP_10; + + /* correct */ + ao_k_height += (int32_t) AO_K0_10 * err; + ao_k_speed += (int32_t) AO_K1_10 * err; + ao_k_accel += (int32_t) AO_K2_10 * err; + return; + } +#endif + ao_k_height += ((ao_k_speed >> 16) * AO_K_STEP_100 + + (ao_k_accel >> 16) * AO_K_STEP_2_2_100); + ao_k_speed += (ao_k_accel >> 16) * AO_K_STEP_100; + + /* correct */ + ao_k_height += (int32_t) AO_K0_100 * err; + ao_k_speed += (int32_t) AO_K1_100 * err; + ao_k_accel += (int32_t) AO_K2_100 * err; +} +#endif + __xdata int32_t ao_raw_pres_sum; /* Landing is detected by getting constant readings from both pressure and accelerometer @@ -296,6 +356,10 @@ ao_flight(void) ao_flight_vel += (int32_t) ao_vel_change * (int32_t) ticks; #endif +#if USE_KALMAN + if (ao_flight_state > ao_flight_idle) + ao_kalman_baro(); +#endif ao_flight_adc = ao_adc_ring_next(ao_flight_adc); } @@ -333,6 +397,9 @@ ao_flight(void) ao_ground_pres = ao_raw_pres_sum >> 9; ao_min_pres = ao_ground_pres; ao_config_get(); +#if USE_KALMAN + ao_ground_height = ao_pres_to_altitude(ao_ground_pres); +#endif ao_main_pres = ao_altitude_to_pres(ao_pres_to_altitude(ao_ground_pres) + ao_config.main_deploy); #if HAS_ACCEL ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g; @@ -369,7 +436,6 @@ ao_flight(void) { /* Set pad mode - we can fly! */ ao_flight_state = ao_flight_pad; - #if HAS_USB /* Disable the USB controller in flight mode * to save power @@ -421,14 +487,20 @@ ao_flight(void) * the barometer, but we use both to make sure this * transition is detected */ +#if USE_KALMAN + if ((ao_k_accel > to_fix32(20) && + ao_k_speed > to_fix32(5)) || + ao_k_height > to_fix32(20)) +#else if ( #if HAS_ACCEL (ao_flight_accel < ao_ground_accel - ACCEL_BOOST && ao_flight_vel > ACCEL_VEL_BOOST) || #endif ao_flight_pres < ao_ground_pres - BARO_LAUNCH) +#endif { -#if HAS_ACCEL +#if HAS_ACCEL || USE_KALMAN ao_flight_state = ao_flight_boost; #else ao_flight_state = ao_flight_coast; @@ -454,7 +526,7 @@ ao_flight(void) break; } break; -#if HAS_ACCEL +#if HAS_ACCEL || USE_KALMAN case ao_flight_boost: /* boost to fast: @@ -467,8 +539,13 @@ ao_flight(void) * deceleration, or by waiting until the maximum burn duration * (15 seconds) has past. */ +#if USE_KALMAN + if ((ao_k_accel < to_fix32(-10) && ao_k_height > to_fix32(100)) || + (int16_t) (ao_flight_tick - ao_launch_tick) > BOOST_TICKS_MAX) +#else if (ao_flight_accel > ao_ground_accel + ACCEL_COAST || (int16_t) (ao_flight_tick - ao_launch_tick) > BOOST_TICKS_MAX) +#endif { ao_flight_state = ao_flight_fast; ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); @@ -492,20 +569,34 @@ ao_flight(void) * how big a pressure change the mach transition * generates would be useful here. */ +#if USE_KALMAN + if (ao_k_speed < to_fix32(200) || + ao_k_height < ao_k_max_height - to_fix32(500)) +#else if (ao_flight_vel < ACCEL_VEL_MACH || ao_flight_pres > ao_min_pres + BARO_COAST) +#endif { +#if HAS_ACCEL /* set min velocity to current velocity for * apogee detect */ ao_min_vel = abs(ao_flight_vel); +#endif ao_flight_state = ao_flight_coast; ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); } break; -#endif +#endif /* HAS_ACCEL */ case ao_flight_coast: +#if USE_KALMAN + /* apogee detect: coast to drogue deploy: + * + * speed: < 0 + */ + if (ao_k_speed < 0) +#else /* apogee detect: coast to drogue deploy: * * barometer: fall at least 10m @@ -517,6 +608,7 @@ ao_flight(void) * we'll trust to a single sensor for this test */ if (ao_flight_pres > ao_min_pres + BARO_APOGEE) +#endif { /* ignite the drogue charge */ ao_ignite(ao_igniter_drogue); @@ -569,7 +661,11 @@ ao_flight(void) * at that point. Perhaps also use the drogue sense lines * to notice continutity? */ +#if USE_KALMAN + if (from_fix(ao_k_height) < ao_config.main_deploy) +#else if (ao_flight_pres >= ao_main_pres) +#endif { ao_ignite(ao_igniter_main); ao_flight_state = ao_flight_main; diff --git a/src/ao_flight_test.c b/src/ao_flight_test.c index a635803f..16167644 100644 --- a/src/ao_flight_test.c +++ b/src/ao_flight_test.c @@ -21,6 +21,7 @@ #include #include #include +#include #define AO_HERTZ 100 @@ -62,6 +63,7 @@ enum ao_flight_state { struct ao_adc ao_adc_ring[AO_ADC_RING]; uint8_t ao_adc_head; +int ao_summary = 0; #define ao_led_on(l) #define ao_led_off(l) @@ -79,10 +81,13 @@ enum ao_igniter { ao_igniter_main = 1 }; +struct ao_adc ao_adc_static; + void ao_ignite(enum ao_igniter igniter) { - printf ("ignite %s\n", igniter == ao_igniter_drogue ? "drogue" : "main"); + printf ("ignite %s at %7.2f\n", igniter == ao_igniter_drogue ? "drogue" : "main", + (double) ao_adc_static.tick / 100.0); } struct ao_task { @@ -99,8 +104,6 @@ struct ao_task { #define AO_FLIGHT_TEST -struct ao_adc ao_adc_static; - FILE *emulator_in; void @@ -140,20 +143,37 @@ struct ao_config ao_config; #ifndef HAS_ACCEL #define HAS_ACCEL 1 #define HAS_ACCEL_REF 0 +#define USE_KALMAN 0 +#else +#define USE_KALMAN 1 #endif #include "ao_flight.c" +#define to_double(f) ((f) / 65536.0) + void ao_insert(void) { ao_adc_ring[ao_adc_head] = ao_adc_static; ao_adc_head = ao_adc_ring_next(ao_adc_head); + if (ao_summary) + return; if (ao_flight_state != ao_flight_startup) { +#if USE_KALMAN + printf("time %7.2f accel %d pres %d k_height %8.2f k_speed %8.5f k_accel %8.5f\n", + (double) ao_adc_static.tick / 100, + ao_adc_static.accel, + ao_adc_static.pres, + to_double(ao_k_height), + to_double(ao_k_speed), + to_double(ao_k_accel)); +#else printf("time %g accel %d pres %d\n", (double) ao_adc_static.tick / 100, ao_adc_static.accel, ao_adc_static.pres); +#endif } } @@ -269,6 +289,8 @@ ao_dump_state(void) { if (ao_flight_state == ao_flight_startup) return; + if (ao_summary) + return; #if HAS_ACCEL printf ("\t\t\t\t\t%s accel %g vel %g alt %d main %d\n", ao_state_names[ao_flight_state], @@ -286,14 +308,44 @@ ao_dump_state(void) exit(0); } +static const struct option options[] = { + { .name = "summary", .has_arg = 0, .val = 's' }, + { 0, 0, 0, 0}, +}; + +void run_flight_fixed(char *name, FILE *f, int summary) +{ + emulator_in = f; + ao_summary = summary; + ao_flight_init(); + ao_flight(); +} + int main (int argc, char **argv) { - emulator_in = fopen (argv[1], "r"); - if (!emulator_in) { - perror(argv[1]); - exit(1); + int summary = 0; + int c; + int i; + + while ((c = getopt_long(argc, argv, "s", options, NULL)) != -1) { + switch (c) { + case 's': + summary = 1; + break; + } } - ao_flight_init(); - ao_flight(); + + if (optind == argc) + run_flight_fixed("", stdin, summary); + else + for (i = optind; i < argc; i++) { + FILE *f = fopen(argv[i], "r"); + if (!f) { + perror(argv[i]); + continue; + } + run_flight_fixed(argv[i], f, summary); + fclose(f); + } } diff --git a/src/ao_pins.h b/src/ao_pins.h index 0de39970..353b5fd5 100644 --- a/src/ao_pins.h +++ b/src/ao_pins.h @@ -27,6 +27,7 @@ #define HAS_ADC 1 #define HAS_EEPROM 1 #define HAS_DBG 1 + #define USE_KALMAN 0 #define DBG_ON_P1 1 #define DBG_ON_P0 0 #define IGNITE_ON_P2 1 @@ -50,6 +51,7 @@ #define HAS_ADC 1 #define HAS_EEPROM 1 #define HAS_DBG 1 + #define USE_KALMAN 0 #define DBG_ON_P1 1 #define DBG_ON_P0 0 #define IGNITE_ON_P2 1 @@ -98,6 +100,7 @@ #define HAS_ADC 1 #define HAS_EEPROM 1 #define HAS_DBG 0 + #define USE_KALMAN 1 #define IGNITE_ON_P2 0 #define IGNITE_ON_P0 1 #define PACKET_HAS_MASTER 0 @@ -118,6 +121,7 @@ #define HAS_SERIAL_1 1 #define HAS_ADC 1 #define HAS_DBG 0 + #define USE_KALMAN 0 #define HAS_EEPROM 1 #define DBG_ON_P1 0 #define DBG_ON_P0 1 -- 2.30.2