From 540309240a8515116120dbd4403902282ed8c27b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 16 Jan 2013 15:15:49 -0800 Subject: [PATCH] altos: Add Kalman filter to MicroPeak This filters altitudes more accurately and also allows tracking of acceleration, which is used to discard height data generated by ejection charge noise Signed-off-by: Keith Packard --- src/micropeak/Makefile | 4 +- src/micropeak/ao_microflight.c | 143 ++++++++++++++++++++++++ src/micropeak/ao_microkalman.c | 74 +++++++++++++ src/micropeak/ao_micropeak.c | 106 +----------------- src/micropeak/ao_micropeak.h | 32 +++++- src/test/Makefile | 11 +- src/test/ao_micropeak_test.c | 192 +++++++++++++++++++++++++++++++++ src/test/plotmicro | 14 +++ 8 files changed, 462 insertions(+), 114 deletions(-) create mode 100644 src/micropeak/ao_microflight.c create mode 100644 src/micropeak/ao_microkalman.c create mode 100644 src/test/ao_micropeak_test.c create mode 100755 src/test/plotmicro diff --git a/src/micropeak/Makefile b/src/micropeak/Makefile index ff0a4499..315b93f6 100644 --- a/src/micropeak/Makefile +++ b/src/micropeak/Makefile @@ -33,7 +33,9 @@ ALTOS_SRC = \ ao_eeprom_tiny.c \ ao_panic.c \ ao_log_micro.c \ - ao_async.c + ao_async.c \ + ao_microflight.c \ + ao_microkalman.c INC=\ ao.h \ diff --git a/src/micropeak/ao_microflight.c b/src/micropeak/ao_microflight.c new file mode 100644 index 00000000..714bb90a --- /dev/null +++ b/src/micropeak/ao_microflight.c @@ -0,0 +1,143 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_FLIGHT_TEST +#include +#endif +#include +#include + +uint32_t pa; +uint32_t pa_ground; +uint32_t pa_min; + +static void +ao_microsample(void) +{ + ao_pa_get(); + ao_microkalman_predict(); + ao_microkalman_correct(); +} + +#define NUM_PA_HIST 16 + +#define SKIP_PA_HIST(i,j) (((i) + (j)) & (NUM_PA_HIST - 1)) + +static uint32_t pa_hist[NUM_PA_HIST]; + +void +ao_microflight(void) +{ + int16_t sample_count; + uint16_t time; + uint32_t pa_interval_min, pa_interval_max; + int32_t pa_diff; + uint8_t h, i; + uint8_t accel_lock = 0; + uint32_t pa_sum = 0; + + /* Wait for motion, averaging values to get ground pressure */ + + time = ao_time(); + ao_pa_get(); + ao_microkalman_init(); + pa_ground = pa; + sample_count = 0; + h = 0; + for (;;) { + time += SAMPLE_SLEEP; + if (sample_count == 0) + ao_led_on(AO_LED_REPORT); + ao_delay_until(time); + ao_microsample(); + if (sample_count == 0) + ao_led_off(AO_LED_REPORT); + pa_hist[h] = pa; + h = SKIP_PA_HIST(h,1); + pa_diff = pa_ground - ao_pa; + + /* Check for a significant pressure change */ + if (pa_diff > BOOST_DETECT) + break; + + if (sample_count < GROUND_AVG * 2) { + if (sample_count < GROUND_AVG) + pa_sum += pa; + ++sample_count; + } else { + pa_ground = pa_sum >> GROUND_AVG_SHIFT; + pa_sum = 0; + sample_count = 0; + } + } + + /* Go back and find the first sample a decent interval above the ground */ + pa_min = pa_ground - LAND_DETECT; + for (i = SKIP_PA_HIST(h,2); i != h; i = SKIP_PA_HIST(i,2)) { + if (pa_hist[i] < pa_min) + break; + } + + /* Log the remaining samples so we get a complete history since leaving the ground */ + for (; i != h; i = SKIP_PA_HIST(i,2)) { + pa = pa_hist[i]; + ao_log_micro_data(); + } + + /* Now sit around until the pressure is stable again and record the max */ + + sample_count = 0; + pa_min = ao_pa; + pa_interval_min = ao_pa; + pa_interval_max = ao_pa; + for (;;) { + time += SAMPLE_SLEEP; + ao_delay_until(time); + if ((sample_count & 3) == 0) + ao_led_on(AO_LED_REPORT); + ao_microsample(); + if ((sample_count & 3) == 0) + ao_led_off(AO_LED_REPORT); + if (sample_count & 1) + ao_log_micro_data(); + + /* If accelerating upwards, don't look for min pressure */ + if (ao_pa_accel < ACCEL_LOCK_PA) + accel_lock = ACCEL_LOCK_TIME; + else if (accel_lock) + --accel_lock; + else if (ao_pa < pa_min) + pa_min = ao_pa; + + if (sample_count == (GROUND_AVG - 1)) { + pa_diff = pa_interval_max - pa_interval_min; + + /* Check to see if the pressure is now stable */ + if (pa_diff < LAND_DETECT) + break; + sample_count = 0; + pa_interval_min = ao_pa; + pa_interval_max = ao_pa; + } else { + if (ao_pa < pa_interval_min) + pa_interval_min = ao_pa; + if (ao_pa > pa_interval_max) + pa_interval_max = ao_pa; + ++sample_count; + } + } +} diff --git a/src/micropeak/ao_microkalman.c b/src/micropeak/ao_microkalman.c new file mode 100644 index 00000000..0684ea2b --- /dev/null +++ b/src/micropeak/ao_microkalman.c @@ -0,0 +1,74 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_FLIGHT_TEST +#include +#endif +#include + +#define FIX_BITS 16 + +#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) +#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) +#define from_fix8(x) ((x) >> 8) +#define from_fix(x) ((x) >> 16) +#define fix8_to_fix16(x) ((x) << 8) +#define fix16_to_fix8(x) ((x) >> 8) + +#include + +/* Basic time step (96ms) */ +#define AO_MK_STEP to_fix16(0.096) +/* step ** 2 / 2 */ +#define AO_MK_STEP_2_2 to_fix16(0.004608) + +uint32_t ao_k_pa; /* 24.8 fixed point */ +int32_t ao_k_pa_speed; /* 16.16 fixed point */ +int32_t ao_k_pa_accel; /* 16.16 fixed point */ + +uint32_t ao_pa; /* integer portion */ +int16_t ao_pa_speed; /* integer portion */ +int16_t ao_pa_accel; /* integer portion */ + +void +ao_microkalman_init(void) +{ + ao_pa = pa; + ao_k_pa = pa << 8; +} + +void +ao_microkalman_predict(void) +{ + ao_k_pa += fix16_to_fix8((int32_t) ao_pa_speed * AO_MK_STEP + (int32_t) ao_pa_accel * AO_MK_STEP_2_2); + ao_k_pa_speed += (int32_t) ao_pa_accel * AO_MK_STEP; +} + +void +ao_microkalman_correct(void) +{ + int16_t e; /* Height error in Pa */ + + 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_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/micropeak/ao_micropeak.c b/src/micropeak/ao_micropeak.c index f361aa26..10f0d192 100644 --- a/src/micropeak/ao_micropeak.c +++ b/src/micropeak/ao_micropeak.c @@ -24,16 +24,10 @@ static struct ao_ms5607_sample sample; static struct ao_ms5607_value value; -uint32_t pa; -uint32_t pa_avg; -uint32_t pa_ground; -uint32_t pa_min; alt_t ground_alt, max_alt; alt_t ao_max_height; -static uint32_t pa_sum; - -static void +void ao_pa_get(void) { ao_ms5607_sample(&sample); @@ -60,21 +54,9 @@ ao_pips(void) ao_delay(AO_MS_TO_TICKS(200)); } -#define NUM_PA_HIST 16 - -#define SKIP_PA_HIST(i,j) (((i) + (j)) & (NUM_PA_HIST - 1)) - -static uint32_t pa_hist[NUM_PA_HIST]; - int main(void) { - int16_t sample_count; - uint16_t time; - uint32_t pa_interval_min, pa_interval_max; - int32_t pa_diff; - uint8_t h, i; - ao_led_init(LEDS_AVAILABLE); ao_timer_init(); @@ -93,93 +75,9 @@ main(void) ao_log_micro_dump(); ao_delay(BOOST_DELAY); - /* Wait for motion, averaging values to get ground pressure */ - time = ao_time(); - ao_pa_get(); - pa_avg = pa_ground = pa << FILTER_SHIFT; - sample_count = 0; - h = 0; - for (;;) { - time += SAMPLE_SLEEP; - if (sample_count == 0) - ao_led_on(AO_LED_REPORT); - ao_delay_until(time); - ao_pa_get(); - if (sample_count == 0) - ao_led_off(AO_LED_REPORT); - pa_hist[h] = pa; - h = SKIP_PA_HIST(h,1); - pa_avg = pa_avg - (pa_avg >> FILTER_SHIFT) + pa; - pa_diff = pa_ground - pa_avg; - - /* Check for a significant pressure change */ - if (pa_diff > (BOOST_DETECT << FILTER_SHIFT)) - break; - - if (sample_count < GROUND_AVG * 2) { - if (sample_count < GROUND_AVG) - pa_sum += pa; - ++sample_count; - } else { - pa_ground = pa_sum >> (GROUND_AVG_SHIFT - FILTER_SHIFT); - pa_sum = 0; - sample_count = 0; - } - } - pa_ground >>= FILTER_SHIFT; + ao_microflight(); - /* Go back and find the first sample a decent interval above the ground */ - pa_min = pa_ground - LAND_DETECT; - for (i = SKIP_PA_HIST(h,2); i != h; i = SKIP_PA_HIST(i,2)) { - if (pa_hist[i] < pa_min) - break; - } - - /* Log the remaining samples so we get a complete history since leaving the ground */ - for (; i != h; i = SKIP_PA_HIST(i,2)) { - pa = pa_hist[i]; - ao_log_micro_data(); - } - - /* Now sit around until the pressure is stable again and record the max */ - - sample_count = 0; - pa_min = pa_avg; - pa_interval_min = pa_avg; - pa_interval_max = pa_avg; - for (;;) { - time += SAMPLE_SLEEP; - ao_delay_until(time); - if ((sample_count & 3) == 0) - ao_led_on(AO_LED_REPORT); - ao_pa_get(); - if ((sample_count & 3) == 0) - ao_led_off(AO_LED_REPORT); - if (sample_count & 1) - ao_log_micro_data(); - pa_avg = pa_avg - (pa_avg >> FILTER_SHIFT) + pa; - if (pa_avg < pa_min) - pa_min = pa_avg; - - if (sample_count == (GROUND_AVG - 1)) { - pa_diff = pa_interval_max - pa_interval_min; - - /* Check to see if the pressure is now stable */ - if (pa_diff < (LAND_DETECT << FILTER_SHIFT)) - break; - sample_count = 0; - pa_interval_min = pa_avg; - pa_interval_max = pa_avg; - } else { - if (pa_avg < pa_interval_min) - pa_interval_min = pa_avg; - if (pa_avg > pa_interval_max) - pa_interval_max = pa_avg; - ++sample_count; - } - } - pa_min >>= FILTER_SHIFT; ao_log_micro_save(); ao_compute_height(); ao_report_altitude(); diff --git a/src/micropeak/ao_micropeak.h b/src/micropeak/ao_micropeak.h index e408d7c5..382b98d9 100644 --- a/src/micropeak/ao_micropeak.h +++ b/src/micropeak/ao_micropeak.h @@ -18,7 +18,6 @@ #ifndef _AO_MICROPEAK_H_ #define _AO_MICROPEAK_H_ -#define FILTER_SHIFT 3 #define SAMPLE_SLEEP AO_MS_TO_TICKS(96) /* 16 sample, or about two seconds worth */ @@ -32,14 +31,11 @@ #define BOOST_DELAY AO_SEC_TO_TICKS(30) /* Pressure change (in Pa) to detect landing */ -#define LAND_DETECT 12 /* 1m at sea level, 1.2m at 2000m */ +#define LAND_DETECT 24 /* 2m at sea level, 2.4m at 2000m */ /* Current sensor pressure value */ extern uint32_t pa; -/* IIR filtered pressure value */ -extern uint32_t pa_avg; - /* Average pressure value on ground */ extern uint32_t pa_ground; @@ -52,5 +48,31 @@ extern alt_t ground_alt, max_alt; /* max_alt - ground_alt */ extern alt_t ao_max_height; +void +ao_pa_get(void); + +void +ao_microflight(void); + +#define ACCEL_LOCK_PA -20 +#define ACCEL_LOCK_TIME 10 + +extern uint32_t ao_k_pa; /* 24.8 fixed point */ +extern int32_t ao_k_pa_speed; /* 16.16 fixed point */ +extern int32_t ao_k_pa_accel; /* 16.16 fixed point */ + +extern uint32_t ao_pa; /* integer portion */ +extern int16_t ao_pa_speed; /* integer portion */ +extern int16_t ao_pa_accel; /* integer portion */ + +void +ao_microkalman_init(void); + +void +ao_microkalman_predict(void); + +void +ao_microkalman_correct(void); + #endif diff --git a/src/test/Makefile b/src/test/Makefile index 092bf360..87bd70fe 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,14 +1,14 @@ -vpath % ..:../core:../drivers:../util +vpath % ..:../core:../drivers:../util:../micropeak PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \ ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test \ - ao_aprs_test + ao_aprs_test ao_micropeak_test INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h KALMAN=make-kalman -CFLAGS=-I.. -I. -I../core -I../drivers -O0 -g -Wall +CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -O0 -g -Wall all: $(PROGS) ao_aprs_data.wav @@ -60,4 +60,7 @@ ao_aprs_data.wav: ao_aprs_test ./ao_aprs_test | sox $(SOX_INPUT_ARGS) - $(SOX_OUTPUT_ARGS) $@ check: ao_fec_test ao_flight_test ao_flight_test_baro run-tests - ./ao_fec_test && ./run-tests \ No newline at end of file + ./ao_fec_test && ./run-tests + +ao_micropeak_test: ao_micropeak_test.c ao_microflight.c + cc $(CFLAGS) -o $@ ao_micropeak_test.c -lm \ No newline at end of file diff --git a/src/test/ao_micropeak_test.c b/src/test/ao_micropeak_test.c new file mode 100644 index 00000000..04686402 --- /dev/null +++ b/src/test/ao_micropeak_test.c @@ -0,0 +1,192 @@ +/* + * Copyright © 2009 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#define _GNU_SOURCE + +#include +#include +#include +#include +#include +#include + +FILE *emulator_in; +char *emulator_app; +char *emulator_name; +char *emulator_info; +uint8_t ao_flight_debug; + +#define AO_FLIGHT_TEST + +typedef int32_t alt_t; + +#define AO_MS_TO_TICKS(ms) ((ms) / 10) + +#define AO_LED_REPORT 0 + +static void ao_led_on(uint8_t led) { +} + +static void ao_led_off(uint8_t led) { +} + +static void ao_delay_until(uint16_t target) { +} + +static uint16_t ao_time(void) { + return 0; +} + +#include "ao_microflight.c" +#include "ao_microkalman.c" +#include "ao_convert_pa.c" + +uint16_t now; +uint8_t running; + +void ao_log_micro_data() { + running = 1; +} + +void +ao_micro_report(void) +{ + if (running) { + alt_t ground = ao_pa_to_altitude(pa_ground); + printf ("%6.2f %10d %10d %10d\n", now / 100.0, + ao_pa_to_altitude(pa) - ground, + ao_pa_to_altitude(ao_pa) - ground, + ao_pa_to_altitude(pa_min) - ground); + } +} + +void +ao_micro_finish(void) +{ + ao_micro_report(); +} + +void +ao_pa_get(void) +{ + char line[4096]; + char *toks[128]; + char *saveptr; + int t, ntok; + static int time_id; + static int pa_id; + double time; + double pressure; + static double last_time; + static int been_here; + static int start_samples; + + if (been_here && start_samples < 100) { + start_samples++; + return; + } + ao_micro_report(); + for (;;) { + if (!fgets(line, sizeof (line), emulator_in)) + exit(0); + for (t = 0; t < 128; t++) { + toks[t] = strtok_r(t ? NULL : line, ", ", &saveptr); + if (!toks[t]) + break; + } + ntok = t; + if (toks[0][0] == '#') { + if (strcmp(toks[0],"#version") == 0) { + for (t = 1; t < ntok; t++) { + if (!strcmp(toks[t], "time")) + time_id = t; + if (!strcmp(toks[t],"pressure")) + pa_id = t; + } + } + continue; + } + time = strtod(toks[time_id],NULL); + pressure = strtod(toks[pa_id],NULL); + if (been_here && time - last_time < 0.1) + continue; + been_here = 1; + last_time = time; + now = floor (time * 100 + 0.5); + pa = pressure; + break; + } +} + +void +ao_dump_state(void) +{ +} + +static const struct option options[] = { + { .name = "summary", .has_arg = 0, .val = 's' }, + { .name = "debug", .has_arg = 0, .val = 'd' }, + { .name = "info", .has_arg = 1, .val = 'i' }, + { 0, 0, 0, 0}, +}; + +void run_flight_fixed(char *name, FILE *f, int summary, char *info) +{ + emulator_name = name; + emulator_in = f; + emulator_info = info; + ao_microflight(); + ao_micro_finish(); +} + +int +main (int argc, char **argv) +{ + int summary = 0; + int c; + int i; + char *info = NULL; + + emulator_app="baro"; + while ((c = getopt_long(argc, argv, "sdi:", options, NULL)) != -1) { + switch (c) { + case 's': + summary = 1; + break; + case 'd': + ao_flight_debug = 1; + break; + case 'i': + info = optarg; + break; + } + } + + if (optind == argc) + run_flight_fixed("", stdin, summary, info); + 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, info); + fclose(f); + } + exit(0); +} diff --git a/src/test/plotmicro b/src/test/plotmicro new file mode 100755 index 00000000..cdfcc581 --- /dev/null +++ b/src/test/plotmicro @@ -0,0 +1,14 @@ +#!/bin/sh +for i in "$@"; do +gnuplot -p << EOF & +set title "$i" +set ylabel "height (m)" +set xlabel "time (s)" +set xtics border out nomirror +set ytics border out nomirror +set y2tics border out nomirror +plot "$i" using 1:2 with lines lt 2 axes x1y1 title "raw height",\ + "$i" using 1:3 with lines lt 4 axes x1y1 title "kalman height",\ + "$i" using 1:4 with lines lt 1 axes x1y1 title "max height" +EOF +done -- 2.30.2