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 \
--- /dev/null
+/*
+ * Copyright © 2013 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.
+ */
+
+#ifndef AO_FLIGHT_TEST
+#include <ao.h>
+#endif
+#include <ao_micropeak.h>
+#include <ao_log_micro.h>
+
+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;
+ }
+ }
+}
--- /dev/null
+/*
+ * Copyright © 2013 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.
+ */
+
+#ifndef AO_FLIGHT_TEST
+#include <ao.h>
+#endif
+#include <ao_micropeak.h>
+
+#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 <ao_kalman.h>
+
+/* 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);
+}
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);
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();
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();
#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 */
#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;
/* 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
-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
./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
--- /dev/null
+/*
+ * 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);
+}
--- /dev/null
+#!/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
projects / fw / altos / commitdiff