altos: Add Kalman filter to MicroPeak

[fw/altos] / src / test / ao_micropeak_test.c

diff --git a/src/test/ao_micropeak_test.c b/src/test/ao_micropeak_test.c
new file mode 100644 (file)
index 0000000..0468640
--- /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);
+}