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 <keithp@keithp.com>

3 files changed, 213 insertions, 4 deletions

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 <keithp@keithp.com>
+ *
+ * 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 <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <getopt.h>
+#include <math.h>
+
+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>", 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