summaryrefslogtreecommitdiff
path: root/src/test
diff options
context:
space:
mode:
Diffstat (limited to 'src/test')
-rw-r--r--src/test/.gitignore6
-rw-r--r--src/test/Makefile48
-rw-r--r--src/test/ao_aes_test.c52
-rw-r--r--src/test/ao_aprs_test.c132
-rw-r--r--src/test/ao_fat_test.c565
-rw-r--r--src/test/ao_flight_test.c451
-rw-r--r--src/test/ao_gps_test.c8
-rw-r--r--src/test/ao_gps_test_skytraq.c10
-rw-r--r--src/test/ao_gps_test_ublox.c409
-rw-r--r--src/test/ao_micropeak_test.c220
-rwxr-xr-xsrc/test/plotmicro16
-rwxr-xr-xsrc/test/plotmm11
-rwxr-xr-xsrc/test/run-mm41
13 files changed, 1919 insertions, 50 deletions
diff --git a/src/test/.gitignore b/src/test/.gitignore
index 5d528ab9..90af6517 100644
--- a/src/test/.gitignore
+++ b/src/test/.gitignore
@@ -1,3 +1,5 @@
+ao_aprs_data.wav
+ao_aprs_test
ao_flight_test
ao_flight_test_baro
ao_flight_test_accel
@@ -5,5 +7,9 @@ ao_gps_test
ao_gps_test_skytraq
ao_convert_test
ao_convert_pa_test
+ao_fat_test
ao_fec_test
+ao_flight_test_mm
ao_flight_test_noisy_accel
+ao_micropeak_test
+ao_aes_test
diff --git a/src/test/Makefile b/src/test/Makefile
index db3cc04b..169c1dc6 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -1,36 +1,46 @@
-vpath % ..:../core:../drivers:../util
+vpath % ..:../core:../drivers:../util:../micropeak:../aes
-PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_gps_test ao_gps_test_skytraq ao_convert_test ao_convert_pa_test ao_fec_test
+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_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \
+ ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_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 -O3 -g -Wall
+CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -O0 -g -Wall
-all: $(PROGS)
+all: $(PROGS) ao_aprs_data.wav
clean:
- rm -f $(PROGS) run-out.baro run-out.full
+ rm -f $(PROGS) ao_aprs_data.wav run-out.baro run-out.full
install:
-ao_flight_test: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h ao_kalman.h
+ao_flight_test: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h $(INCS)
cc $(CFLAGS) -o $@ $<
-ao_flight_test_noisy_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h ao_kalman.h
+ao_flight_test_noisy_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS)
cc -DNOISY_ACCEL=1 $(CFLAGS) -o $@ $<
-ao_flight_test_baro: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h ao_kalman.h
+ao_flight_test_baro: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS)
cc $(CFLAGS) -o $@ -DHAS_ACCEL=0 ao_flight_test.c
-ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c altitude.h ao_kalman.h
+ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS)
cc $(CFLAGS) -o $@ -DFORCE_ACCEL=1 ao_flight_test.c
+ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS)
+ cc -DTELEMEGA=1 $(CFLAGS) -o $@ $< -lm
+
ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h
cc $(CFLAGS) -o $@ $<
ao_gps_test_skytraq: ao_gps_test_skytraq.c ao_gps_skytraq.c ao_gps_print.c ao_host.h
cc $(CFLAGS) -o $@ $<
+ao_gps_test_ublox: ao_gps_test_ublox.c ao_gps_ublox.c ao_gps_print.c ao_host.h ao_gps_ublox.h
+ cc $(CFLAGS) -o $@ $<
+
ao_convert_test: ao_convert_test.c ao_convert.c altitude.h
cc $(CFLAGS) -o $@ $<
@@ -43,5 +53,23 @@ ao_kalman.h: $(KALMAN)
ao_fec_test: ao_fec_test.c ao_fec_tx.c ao_fec_rx.c
cc $(CFLAGS) -DAO_FEC_DEBUG=1 -o $@ ao_fec_test.c ../core/ao_fec_tx.c ../core/ao_fec_rx.c -lm
+ao_aprs_test: ao_aprs_test.c ao_aprs.c
+ cc $(CFLAGS) -o $@ ao_aprs_test.c
+
+SOX_INPUT_ARGS=--type raw --encoding unsigned-integer -b 8 -c 1 -r 9600
+SOX_OUTPUT_ARGS=--type wav
+
+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 ao_kalman.h
+ cc $(CFLAGS) -o $@ ao_micropeak_test.c -lm
+
+ao_fat_test: ao_fat_test.c ao_fat.c ao_bufio.c
+ cc $(CFLAGS) -o $@ ao_fat_test.c -lssl
+
+ao_aes_test: ao_aes_test.c ao_aes.c ao_aes_tables.c
+ cc $(CFLAGS) -o $@ ao_aes_test.c
diff --git a/src/test/ao_aes_test.c b/src/test/ao_aes_test.c
new file mode 100644
index 00000000..dcedbfca
--- /dev/null
+++ b/src/test/ao_aes_test.c
@@ -0,0 +1,52 @@
+/*
+ * 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.
+ */
+
+#define __pdata
+#define __data
+#define __xdata
+#define __code
+#define __reentrant
+
+#include <string.h>
+#include <stdio.h>
+
+#define AO_AES_TEST 1
+
+#include "../aes/ao_aes_tables.c"
+#include "../aes/ao_aes.c"
+
+static uint8_t key[16];
+static uint8_t text[16];
+static uint8_t cbc[16];
+
+int
+main (int argc, char **argv)
+{
+ int i;
+
+ ao_aes_init();
+ ao_aes_set_mode(ao_aes_mode_cbc_mac);
+ ao_aes_set_key(key);
+ ao_aes_zero_iv();
+ ao_aes_run(text, cbc);
+
+ printf ("CBC");
+ for (i = 0; i < sizeof (cbc); i++)
+ printf (" %02x", cbc[i]);
+ printf ("\n");
+ return 0;
+}
diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c
new file mode 100644
index 00000000..69147786
--- /dev/null
+++ b/src/test/ao_aprs_test.c
@@ -0,0 +1,132 @@
+/*
+ * Copyright © 2012 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.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdint.h>
+#include <stdarg.h>
+
+#include <ao_telemetry.h>
+
+struct ao_telemetry_location ao_gps_data;
+
+#define AO_APRS_TEST
+
+typedef int16_t (*ao_radio_fill_func)(uint8_t *buffer, int16_t len);
+
+#define DEBUG 0
+#if DEBUG
+void
+ao_aprs_bit(uint8_t bit)
+{
+ static int seq = 0;
+ printf ("%6d %d\n", seq++, bit ? 1 : 0);
+}
+#else
+void
+ao_aprs_bit(uint8_t bit)
+{
+ putchar (bit ? 0xc0 : 0x40);
+}
+#endif
+
+void
+ao_radio_send_aprs(ao_radio_fill_func fill);
+
+#include <ao_aprs.c>
+
+/*
+ * @section copyright_sec Copyright
+ *
+ * Copyright (c) 2001-2009 Michael Gray, KD7LMO
+
+
+ *
+ *
+ * @section gpl_sec GNU General Public License
+ *
+ * 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; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * 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
+ *
+
+ */
+
+static void
+audio_gap(int secs)
+{
+#if !DEBUG
+ int samples = secs * 9600;
+
+ while (samples--)
+ ao_aprs_bit(0);
+#endif
+}
+
+// This is where we go after reset.
+int main(int argc, char **argv)
+{
+ audio_gap(1);
+
+ ao_gps_data.latitude = (45.0 + 28.25 / 60.0) * 10000000;
+ ao_gps_data.longitude = (-(122 + 44.2649 / 60.0)) * 10000000;
+ ao_gps_data.altitude = 84;
+
+ /* Transmit one packet */
+ ao_aprs_send();
+
+ tncBuffer[strlen((char *) tncBuffer) - 2] = '\0';
+ fprintf(stderr, "packet: %s\n", tncBuffer);
+
+ exit(0);
+}
+
+void
+ao_radio_send_aprs(ao_radio_fill_func fill)
+{
+ int16_t len;
+ uint8_t done = 0;
+ uint8_t buf[16], *b, c;
+ uint8_t bit;
+
+ while (!done) {
+ len = (*fill)(buf, sizeof (buf));
+ if (len < 0) {
+ done = 1;
+ len = -len;
+ }
+ b = buf;
+ while (len--) {
+ c = *b++;
+ for (bit = 0; bit < 8; bit++) {
+ ao_aprs_bit(c & 0x80);
+ c <<= 1;
+ }
+ }
+ }
+}
diff --git a/src/test/ao_fat_test.c b/src/test/ao_fat_test.c
new file mode 100644
index 00000000..d1309024
--- /dev/null
+++ b/src/test/ao_fat_test.c
@@ -0,0 +1,565 @@
+/*
+ * 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.
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <string.h>
+#include <getopt.h>
+#include <math.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <openssl/md5.h>
+
+#define AO_FAT_TEST
+
+void
+ao_mutex_get(uint8_t *mutex)
+{
+}
+
+void
+ao_mutex_put(uint8_t *mutex)
+{
+}
+
+void
+ao_panic(uint8_t panic)
+{
+ printf ("panic %d\n", panic);
+ abort();
+}
+
+#define AO_PANIC_BUFIO 15
+
+#define ao_cmd_success 0
+
+uint8_t ao_cmd_status;
+uint32_t ao_cmd_lex_u32;
+
+void
+ao_cmd_decimal()
+{
+}
+
+#define ao_cmd_register(x)
+
+struct ao_cmds {
+ void (*func)(void);
+ const char *help;
+};
+
+int fs_fd;
+
+uint64_t total_reads, total_writes;
+
+uint8_t
+ao_sdcard_read_block(uint32_t block, uint8_t *data)
+{
+ ++total_reads;
+ lseek(fs_fd, block * 512, 0);
+ return read(fs_fd, data, 512) == 512;
+}
+
+uint8_t
+ao_sdcard_write_block(uint32_t block, uint8_t *data)
+{
+ ++total_writes;
+ lseek(fs_fd, block * 512, 0);
+ return write(fs_fd, data, 512) == 512;
+}
+
+struct fs_param {
+ int fat;
+ int blocks;
+} fs_params[] = {
+ { .fat = 16, .blocks = 16384 },
+ { .fat = 32, .blocks = 16384 },
+ { .fat = 16, .blocks = 65536 },
+ { .fat = 32, .blocks = 65536 },
+ { .fat = 16, .blocks = 1048576 },
+ { .fat = 32, .blocks = 1048576 },
+ { .fat = 0, .blocks = 0 },
+};
+
+char *fs = "fs.fat";
+struct fs_param *param;
+
+void
+ao_sdcard_init(void)
+{
+ char cmd[1024];
+
+ if (fs_fd) {
+ close(fs_fd);
+ fs_fd = 0;
+ }
+ snprintf(cmd, sizeof(cmd), "rm -f %s && mkfs.vfat -F %d -C %s %d",
+ fs, param->fat, fs, param->blocks);
+ if (system (cmd) != 0) {
+ fprintf(stderr, "'%s' failed\n", cmd);
+ exit(1);
+ }
+ fs_fd = open(fs, 2);
+ if (fs_fd < 0) {
+ perror (fs);
+ exit(1);
+ }
+}
+
+#include "ao_bufio.c"
+void
+check_bufio(char *where)
+{
+ int b;
+
+ for (b = 0; b < AO_NUM_BUF; b++) {
+ if (ao_bufio[b].busy) {
+ printf ("%s: buffer %d busy. block %d seqno %u\n",
+ where, b, ao_bufio[b].block, ao_bufio[b].seqno & 0xffff);
+ abort();
+ }
+ }
+}
+
+
+void
+check_fat(void);
+
+#include "ao_fat.c"
+
+/* Get the next cluster entry in the chain */
+static cluster_t
+ao_fat_entry_raw_read(cluster_t cluster, uint8_t fat)
+{
+ sector_t sector;
+ cluster_offset_t offset;
+ uint8_t *buf;
+ cluster_t ret;
+
+ if (fat32)
+ cluster <<= 2;
+ else
+ cluster <<= 1;
+ sector = cluster >> SECTOR_SHIFT;
+ offset = cluster & SECTOR_MASK;
+ buf = _ao_fat_sector_get(fat_start + fat * sectors_per_fat + sector);
+ if (!buf)
+ return 0;
+ if (fat32)
+ ret = get_u32(buf + offset);
+ else
+ ret = get_u16(buf + offset);
+ _ao_fat_sector_put(buf, 0);
+ return ret;
+}
+
+void
+dump_fat(void)
+{
+ int e;
+
+ printf ("\n **** FAT ****\n\n");
+ for (e = 0; e < number_cluster; e++) {
+ if ((e & 0xf) == 0x0)
+ printf ("%04x: ", e);
+ if (fat32)
+ printf (" %08x", ao_fat_entry_raw_read(e, 0));
+ else
+ printf (" %04x", ao_fat_entry_raw_read(e, 0));
+ if ((e & 0xf) == 0xf)
+ putchar ('\n');
+ }
+ if (e & 0xf)
+ putchar('\n');
+}
+
+void
+fat_list(void)
+{
+ dirent_t entry = 0;
+ struct ao_fat_dirent dirent;
+
+ printf (" **** Root directory ****\n");
+ while (ao_fat_readdir(&entry, &dirent)) {
+ printf ("%04x: %-8.8s.%-3.3s %02x %04x %d\n",
+ entry,
+ dirent.name,
+ dirent.name + 8,
+ dirent.attr,
+ dirent.cluster,
+ dirent.size);
+ }
+
+ printf (" **** End of root directory ****\n");
+}
+
+void
+fatal(char *msg, ...)
+{
+// dump_fat();
+// fat_list();
+
+ va_list l;
+ va_start(l, msg);
+ vfprintf(stderr, msg, l);
+ va_end(l);
+
+ abort();
+}
+
+void
+check_fat(void)
+{
+ cluster_t e;
+ int f;
+
+ for (e = 0; e < number_cluster; e++) {
+ cluster_t v = ao_fat_entry_raw_read(e, 0);
+ for (f = 1; f < number_fat; f++) {
+ cluster_t o = ao_fat_entry_raw_read(e, f);
+ if (o != v)
+ fatal ("fats differ at %08x (0 %08x %d %08x)\n", e, v, f, o);
+ }
+ }
+}
+
+cluster_t
+check_file(dirent_t dent, cluster_t first_cluster, dirent_t *used)
+{
+ cluster_t clusters = 0;
+ cluster_t cluster;
+
+ if (!first_cluster)
+ return 0;
+
+ for (cluster = first_cluster;
+ fat32 ? !AO_FAT_IS_LAST_CLUSTER(cluster) : !AO_FAT_IS_LAST_CLUSTER16(cluster);
+ cluster = ao_fat_entry_raw_read(cluster, 0))
+ {
+ if (!_ao_fat_cluster_valid(cluster))
+ fatal("file %d: invalid cluster %08x\n", dent, cluster);
+ if (used[cluster])
+ fatal("file %d: duplicate cluster %08x also in file %d\n", dent, cluster, used[cluster]-1);
+ used[cluster] = dent;
+ clusters++;
+ }
+ return clusters;
+}
+
+void
+check_fs(void)
+{
+ dirent_t r;
+ cluster_t cluster, chain;
+ dirent_t *used;
+ uint8_t *dent;
+
+ check_fat();
+
+ used = calloc(sizeof (dirent_t), number_cluster);
+
+ for (r = 0; (dent = _ao_fat_root_get(r)); r++) {
+ cluster_t clusters;
+ offset_t size;
+ cluster_t first_cluster;
+ char name[11];
+
+ if (!dent)
+ fatal("cannot map dent %d\n", r);
+ memcpy(name, dent+0, 11);
+ first_cluster = get_u16(dent + 0x1a);
+ if (fat32)
+ first_cluster |= (cluster_t) get_u16(dent + 0x14) << 16;
+ size = get_u32(dent + 0x1c);
+ _ao_fat_root_put(dent, r, 0);
+
+ if (name[0] == AO_FAT_DENT_END) {
+ break;
+ }
+
+ clusters = check_file(r + 1, first_cluster, used);
+ if (size == 0) {
+ if (clusters != 0)
+ fatal("file %d: zero sized, but %d clusters\n", clusters);
+ } else {
+ if (size > clusters * bytes_per_cluster)
+ fatal("file %d: size %u beyond clusters %d (%u)\n",
+ r, size, clusters, clusters * bytes_per_cluster);
+ if (size <= (clusters - 1) * bytes_per_cluster)
+ fatal("file %d: size %u too small clusters %d (%u)\n",
+ r, size, clusters, clusters * bytes_per_cluster);
+ }
+ }
+ if (!fat32) {
+ for (; r < root_entries; r++) {
+ uint8_t *dent = _ao_fat_root_get(r);
+ if (!dent)
+ fatal("cannot map dent %d\n", r);
+ if (dent[0] != AO_FAT_DENT_END)
+ fatal("found non-zero dent past end %d\n", r);
+ _ao_fat_root_put(dent, r, 0);
+ }
+ } else {
+ check_file((dirent_t) -1, root_cluster, used);
+ }
+
+ for (cluster = 0; cluster < 2; cluster++) {
+ chain = ao_fat_entry_raw_read(cluster, 0);
+
+ if (fat32) {
+ if ((chain & 0xffffff8) != 0xffffff8)
+ fatal("cluster %d: not marked busy\n", cluster);
+ } else {
+ if ((chain & 0xfff8) != 0xfff8)
+ fatal("cluster %d: not marked busy\n", cluster);
+ }
+ }
+ for (; cluster < number_cluster; cluster++) {
+ chain = ao_fat_entry_raw_read(cluster, 0);
+
+ if (chain != 0) {
+ if (used[cluster] == 0)
+ fatal("cluster %d: marked busy, but not in any file\n", cluster);
+ } else {
+ if (used[cluster] != 0)
+ fatal("cluster %d: marked free, but found in file %d\n", cluster, used[cluster]-1);
+ }
+ }
+}
+
+#define NUM_FILES 100
+#define LINES_FILE 500000
+
+uint32_t sizes[NUM_FILES];
+
+unsigned char md5[NUM_FILES][MD5_DIGEST_LENGTH];
+
+void
+micro_test_fs(void)
+{
+ int8_t fd;
+ char name[] = "FOO ";
+ char buf[512];
+ int len;
+
+ printf ("write once\n");
+ if ((fd = ao_fat_creat(name)) >= 0) {
+ ao_fat_write(fd, "hello world\n", 12);
+ ao_fat_close(fd);
+ }
+
+ printf ("read first\n");
+ if ((fd = ao_fat_open(name, AO_FAT_OPEN_READ)) >= 0) {
+ len = ao_fat_read(fd, buf, sizeof(buf));
+ write (1, buf, len);
+ ao_fat_close(fd);
+ }
+
+ printf ("write again\n");
+ if ((fd = ao_fat_creat(name)) >= 0) {
+ ao_fat_write(fd, "hi\n", 3);
+ ao_fat_close(fd);
+ }
+
+ printf ("read again\n");
+ if ((fd = ao_fat_open(name, AO_FAT_OPEN_READ)) >= 0) {
+ len = ao_fat_read(fd, buf, sizeof(buf));
+ write (1, buf, len);
+ ao_fat_close(fd);
+ }
+
+ printf ("write 3\n");
+ if ((fd = ao_fat_creat(name)) >= 0) {
+ int l;
+ char c;
+
+ for (l = 0; l < 10; l++) {
+ for (c = ' '; c < '~'; c++)
+ ao_fat_write(fd, &c, 1);
+ c = '\n';
+ ao_fat_write(fd, &c, 1);
+ }
+ ao_fat_close(fd);
+ }
+
+ printf ("read 3\n");
+ if ((fd = ao_fat_open(name, AO_FAT_OPEN_READ)) >= 0) {
+ while ((len = ao_fat_read(fd, buf, sizeof(buf))) > 0)
+ write (1, buf, len);
+ ao_fat_close(fd);
+ }
+
+ check_fs();
+ printf ("all done\n");
+}
+
+
+void
+short_test_fs(void)
+{
+ int len;
+ int8_t fd;
+ char buf[345];
+
+ if ((fd = ao_fat_open("HELLO TXT",AO_FAT_OPEN_READ)) >= 0) {
+ printf ("File contents for HELLO.TXT\n");
+ while ((len = ao_fat_read(fd, buf, sizeof(buf))))
+ write(1, buf, len);
+ ao_fat_close(fd);
+ }
+
+ if ((fd = ao_fat_creat("NEWFILE TXT")) >= 0) {
+ printf ("Create new file\n");
+ for (len = 0; len < 2; len++)
+ ao_fat_write(fd, "hello, world!\n", 14);
+ ao_fat_seek(fd, 0, AO_FAT_SEEK_SET);
+ printf ("read new file\n");
+ while ((len = ao_fat_read(fd, buf, sizeof (buf))))
+ write (1, buf, len);
+ ao_fat_close(fd);
+ }
+
+ check_fs();
+}
+
+void
+long_test_fs(void)
+{
+ char name[12];
+ int id;
+ MD5_CTX ctx;
+ unsigned char md5_check[MD5_DIGEST_LENGTH];
+ char buf[337];
+ int len;
+ int8_t fd;
+ uint64_t total_file_size = 0;
+
+ total_reads = total_writes = 0;
+
+ printf (" **** Creating %d files\n", NUM_FILES);
+
+ memset(sizes, '\0', sizeof (sizes));
+ for (id = 0; id < NUM_FILES; id++) {
+ sprintf(name, "D%07dTXT", id);
+ if ((id % ((NUM_FILES+49)/50)) == 0) {
+ printf ("."); fflush(stdout);
+ }
+ if ((fd = ao_fat_creat(name)) >= 0) {
+ int j;
+ char line[64];
+ check_bufio("file created");
+ MD5_Init(&ctx);
+ for (j = 0; j < LINES_FILE; j++) {
+ int len, ret;
+ sprintf (line, "Hello, world %d %d\r\n", id, j);
+ len = strlen(line);
+ ret = ao_fat_write(fd, line, len);
+ if (ret <= 0)
+ break;
+ total_file_size += ret;
+ MD5_Update(&ctx, line, ret);
+ sizes[id] += ret;
+ if (ret != len)
+ printf ("write failed %d\n", ret);
+ }
+ ao_fat_close(fd);
+ MD5_Final(&md5[id][0], &ctx);
+ check_bufio("file written");
+ }
+ }
+
+ printf ("\n **** Write IO: read %llu write %llu data sectors %llu\n", total_reads, total_writes, (total_file_size + 511) / 512);
+
+ check_bufio("all files created");
+ printf (" **** All done creating files\n");
+ check_fs();
+
+ total_reads = total_writes = 0;
+
+ printf (" **** Comparing %d files\n", NUM_FILES);
+
+ for (id = 0; id < NUM_FILES; id++) {
+ uint32_t size;
+ sprintf(name, "D%07dTXT", id);
+ size = 0;
+ if ((id % ((NUM_FILES+49)/50)) == 0) {
+ printf ("."); fflush(stdout);
+ }
+ if ((fd = ao_fat_open(name, AO_FAT_OPEN_READ)) >= 0) {
+ MD5_Init(&ctx);
+ while ((len = ao_fat_read(fd, buf, sizeof(buf))) > 0) {
+ MD5_Update(&ctx, buf, len);
+ size += len;
+ }
+ ao_fat_close(fd);
+ MD5_Final(md5_check, &ctx);
+ if (size != sizes[id])
+ fatal("file %d: size differs %d written %d read\n",
+ id, sizes[id], size);
+ if (memcmp(md5_check, &md5[id][0], sizeof (md5_check)) != 0)
+ fatal ("file %d: checksum failed\n", id);
+ check_bufio("file shown");
+ }
+ }
+ printf ("\n **** Read IO: read %llu write %llu\n", total_reads, total_writes);
+}
+
+char *params[] = {
+ "-F 16 -C %s 16384",
+ "-F 32 -C %s 16384",
+ "-F 16 -C %s 65536",
+ "-F 32 -C %s 65536",
+ "-F 16 -C %s 1048576",
+ "-F 32 -C %s 1048576",
+ NULL
+};
+
+void
+do_test(void (*test)(void))
+{
+ ao_fat_init();
+
+ check_bufio("top");
+ _ao_fat_setup();
+
+ check_fs();
+ check_bufio("after setup");
+ (*test)();
+ ao_fat_unmount();
+}
+
+int
+main(int argc, char **argv)
+{
+ int p;
+
+ if (argv[1])
+ fs = argv[1];
+
+ for (p = 0; fs_params[p].fat; p++) {
+ param = &fs_params[p];
+
+ do_test(micro_test_fs);
+ do_test(short_test_fs);
+ do_test(long_test_fs);
+ }
+ unlink (fs);
+
+ return 0;
+}
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index b9e291ce..99bed7ee 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -35,6 +35,21 @@
#define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16))
#define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16))
+#if TELEMEGA
+#define AO_ADC_NUM_SENSE 6
+#define HAS_MS5607 1
+#define HAS_MPU6000 1
+#define HAS_MMA655X 1
+
+struct ao_adc {
+ int16_t sense[AO_ADC_NUM_SENSE];
+ int16_t v_batt;
+ int16_t v_pbatt;
+ int16_t accel_ref;
+ int16_t accel;
+ int16_t temp;
+};
+#else
/*
* One set of samples read from the A/D converter
*/
@@ -48,6 +63,13 @@ struct ao_adc {
int16_t sense_m; /* main continuity sense */
};
+#ifndef HAS_ACCEL
+#define HAS_ACCEL 1
+#define HAS_ACCEL_REF 0
+#endif
+
+#endif
+
#define __pdata
#define __data
#define __xdata
@@ -58,12 +80,9 @@ struct ao_adc {
#define HAS_IGNITE 1
#define HAS_USB 1
#define HAS_GPS 1
-#ifndef HAS_ACCEL
-#define HAS_ACCEL 1
-#define HAS_ACCEL_REF 0
-#endif
#include <ao_data.h>
+#include <ao_log.h>
#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
@@ -72,7 +91,6 @@ struct ao_adc {
/*
* Above this height, the baro sensor doesn't work
*/
-#define AO_MAX_BARO_HEIGHT 12000
#define AO_BARO_SATURATE 13000
#define AO_MIN_BARO_VALUE ao_altitude_to_pres(AO_BARO_SATURATE)
@@ -83,19 +101,6 @@ struct ao_adc {
#define ACCEL_NOSE_UP (ao_accel_2g >> 2)
-enum ao_flight_state {
- ao_flight_startup = 0,
- ao_flight_idle = 1,
- ao_flight_pad = 2,
- ao_flight_boost = 3,
- ao_flight_fast = 4,
- ao_flight_coast = 5,
- ao_flight_drogue = 6,
- ao_flight_main = 7,
- ao_flight_landed = 8,
- ao_flight_invalid = 9
-};
-
extern enum ao_flight_state ao_flight_state;
#define FALSE 0
@@ -190,7 +195,14 @@ struct ao_cmds {
#define ao_xmemcmp(d,s,c) memcmp(d,s,c)
#define AO_NEED_ALTITUDE_TO_PRES 1
+#if TELEMEGA
+#include "ao_convert_pa.c"
+#include <ao_ms5607.h>
+struct ao_ms5607_prom ms5607_prom;
+#include "ao_ms5607_convert.c"
+#else
#include "ao_convert.c"
+#endif
struct ao_config {
uint16_t main_deploy;
@@ -218,16 +230,20 @@ typedef int16_t accel_t;
extern uint16_t ao_sample_tick;
-extern int16_t ao_sample_height;
+extern alt_t ao_sample_height;
extern accel_t ao_sample_accel;
extern int32_t ao_accel_scale;
-extern int16_t ao_ground_height;
-extern int16_t ao_sample_alt;
+extern alt_t ao_ground_height;
+extern alt_t ao_sample_alt;
+
+double ao_sample_qangle;
int ao_sample_prev_tick;
uint16_t prev_tick;
+
#include "ao_kalman.c"
+#include "ao_sqrt.c"
#include "ao_sample.c"
#include "ao_flight.c"
@@ -248,6 +264,10 @@ static int landed_set;
static double landed_time;
static double landed_height;
+#if HAS_MPU6000
+static struct ao_mpu6000_sample ao_ground_mpu6000;
+#endif
+
void
ao_test_exit(void)
{
@@ -285,6 +305,20 @@ ao_test_exit(void)
exit(0);
}
+#if HAS_MPU6000
+static double
+ao_mpu6000_accel(int16_t sensor)
+{
+ return sensor / 32767.0 * MPU6000_ACCEL_FULLSCALE * GRAVITY;
+}
+
+static double
+ao_mpu6000_gyro(int32_t sensor)
+{
+ return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE;
+}
+#endif
+
void
ao_insert(void)
{
@@ -293,9 +327,20 @@ ao_insert(void)
ao_data_ring[ao_data_head] = ao_data_static;
ao_data_head = ao_data_ring_next(ao_data_head);
if (ao_flight_state != ao_flight_startup) {
- double height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height;
- double accel = ((ao_flight_ground_accel - ao_data_static.adc.accel) * GRAVITY * 2.0) /
+#if HAS_ACCEL
+ double accel = ((ao_flight_ground_accel - ao_data_accel_cook(&ao_data_static)) * GRAVITY * 2.0) /
(ao_config.accel_minus_g - ao_config.accel_plus_g);
+#else
+ double accel = 0.0;
+#endif
+#if TELEMEGA
+ double height;
+
+ ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked);
+ height = ao_pa_to_altitude(ao_data_static.ms5607_cooked.pres) - ao_ground_height;
+#else
+ double height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height;
+#endif
if (!tick_offset)
tick_offset = -ao_data_static.tick;
@@ -327,10 +372,26 @@ ao_insert(void)
}
if (!ao_summary) {
- printf("%7.2f height %8.2f accel %8.3f state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n",
+ printf("%7.2f height %8.2f accel %8.3f "
+#if TELEMEGA
+ "roll %8.3f angle %8.3f qangle %8.3f "
+ "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f "
+#endif
+ "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n",
time,
height,
accel,
+#if TELEMEGA
+ ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0,
+ ao_mpu6000_gyro(ao_sample_angle) / 100.0,
+ ao_sample_qangle,
+ ao_mpu6000_accel(ao_data_static.mpu6000.accel_x),
+ ao_mpu6000_accel(ao_data_static.mpu6000.accel_y),
+ ao_mpu6000_accel(ao_data_static.mpu6000.accel_z),
+ ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x),
+ ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y),
+ ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z),
+#endif
ao_state_names[ao_flight_state],
ao_k_height / 65536.0,
ao_k_speed / 65536.0 / 16.0,
@@ -469,7 +530,6 @@ union ao_telemetry_all {
uint16_t
uint16(uint8_t *bytes, int off)
{
- off++;
return (uint16_t) bytes[off] | (((uint16_t) bytes[off+1]) << 8);
}
@@ -479,6 +539,223 @@ int16(uint8_t *bytes, int off)
return (int16_t) uint16(bytes, off);
}
+uint32_t
+uint32(uint8_t *bytes, int off)
+{
+ return (uint32_t) bytes[off] | (((uint32_t) bytes[off+1]) << 8) |
+ (((uint32_t) bytes[off+2]) << 16) |
+ (((uint32_t) bytes[off+3]) << 24);
+}
+
+int32_t
+int32(uint8_t *bytes, int off)
+{
+ return (int32_t) uint32(bytes, off);
+}
+
+static int log_format;
+
+#if TELEMEGA
+
+static double
+ao_vec_norm(double x, double y, double z)
+{
+ return x*x + y*y + z*z;
+}
+
+static void
+ao_vec_normalize(double *x, double *y, double *z)
+{
+ double scale = 1/sqrt(ao_vec_norm(*x, *y, *z));
+
+ *x *= scale;
+ *y *= scale;
+ *z *= scale;
+}
+
+struct ao_quat {
+ double q0, q1, q2, q3;
+};
+
+static void
+ao_quat_mul(struct ao_quat *r, struct ao_quat *a, struct ao_quat *b)
+{
+ r->q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3;
+ r->q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2;
+ r->q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1;
+ r->q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0;
+}
+
+#if 0
+static void
+ao_quat_scale(struct ao_quat *r, struct ao_quat *a, double s)
+{
+ r->q0 = a->q0 * s;
+ r->q1 = a->q1 * s;
+ r->q2 = a->q2 * s;
+ r->q3 = a->q3 * s;
+}
+#endif
+
+static void
+ao_quat_conj(struct ao_quat *r, struct ao_quat *a)
+{
+ r->q0 = a->q0;
+ r->q1 = -a->q1;
+ r->q2 = -a->q2;
+ r->q3 = -a->q3;
+}
+
+static void
+ao_quat_rot(struct ao_quat *r, struct ao_quat *a, struct ao_quat *q)
+{
+ struct ao_quat t;
+ struct ao_quat c;
+ ao_quat_mul(&t, q, a);
+ ao_quat_conj(&c, q);
+ ao_quat_mul(r, &t, &c);
+}
+
+static void
+ao_quat_from_angle(struct ao_quat *r,
+ double x_rad,
+ double y_rad,
+ double z_rad)
+{
+ double angle = sqrt (x_rad * x_rad + y_rad * y_rad + z_rad * z_rad);
+ double s = sin(angle/2);
+ double c = cos(angle/2);
+
+ r->q0 = c;
+ r->q1 = x_rad * s / angle;
+ r->q2 = y_rad * s / angle;
+ r->q3 = z_rad * s / angle;
+}
+
+static void
+ao_quat_from_vector(struct ao_quat *r, double x, double y, double z)
+{
+ ao_vec_normalize(&x, &y, &z);
+ double x_rad = atan2(z, y);
+ double y_rad = atan2(x, z);
+ double z_rad = atan2(y, x);
+
+ ao_quat_from_angle(r, x_rad, y_rad, z_rad);
+}
+
+static double
+ao_quat_norm(struct ao_quat *a)
+{
+ return (a->q0 * a->q0 +
+ a->q1 * a->q1 +
+ a->q2 * a->q2 +
+ a->q3 * a->q3);
+}
+
+static void
+ao_quat_normalize(struct ao_quat *a)
+{
+ double norm = ao_quat_norm(a);
+
+ if (norm) {
+ double m = 1/sqrt(norm);
+
+ a->q0 *= m;
+ a->q1 *= m;
+ a->q2 *= m;
+ a->q3 *= m;
+ }
+}
+
+static struct ao_quat ao_up, ao_current;
+static struct ao_quat ao_orient;
+static int ao_orient_tick;
+
+void
+set_orientation(double x, double y, double z, int tick)
+{
+ struct ao_quat t;
+
+ printf ("set_orientation %g %g %g\n", x, y, z);
+ ao_quat_from_vector(&ao_orient, x, y, z);
+ ao_up.q1 = ao_up.q2 = 0;
+ ao_up.q0 = ao_up.q3 = sqrt(2)/2;
+ ao_orient_tick = tick;
+
+ ao_orient.q0 = 1;
+ ao_orient.q1 = 0;
+ ao_orient.q2 = 0;
+ ao_orient.q3 = 0;
+
+ printf ("orient (%g) %g %g %g up (%g) %g %g %g\n",
+ ao_orient.q0,
+ ao_orient.q1,
+ ao_orient.q2,
+ ao_orient.q3,
+ ao_up.q0,
+ ao_up.q1,
+ ao_up.q2,
+ ao_up.q3);
+
+ ao_quat_rot(&t, &ao_up, &ao_orient);
+ printf ("pad orient (%g) %g %g %g\n",
+ t.q0,
+ t.q1,
+ t.q2,
+ t.q3);
+
+}
+
+void
+update_orientation (double rate_x, double rate_y, double rate_z, int tick)
+{
+ struct ao_quat q_dot;
+ double lambda;
+ double dt = (tick - ao_orient_tick) / 100.0;
+
+ ao_orient_tick = tick;
+
+// lambda = 1 - ao_quat_norm(&ao_orient);
+ lambda = 0;
+
+ q_dot.q0 = -0.5 * (ao_orient.q1 * rate_x + ao_orient.q2 * rate_y + ao_orient.q3 * rate_z) + lambda * ao_orient.q0;
+ q_dot.q1 = 0.5 * (ao_orient.q0 * rate_x + ao_orient.q2 * rate_z - ao_orient.q3 * rate_y) + lambda * ao_orient.q1;
+ q_dot.q2 = 0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2;
+ q_dot.q3 = 0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3;
+
+#if 0
+ printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt);
+ printf ("q_dot (%g) %g %g %g\n",
+ q_dot.q0,
+ q_dot.q1,
+ q_dot.q2,
+ q_dot.q3);
+#endif
+
+ ao_orient.q0 += q_dot.q0 * dt;
+ ao_orient.q1 += q_dot.q1 * dt;
+ ao_orient.q2 += q_dot.q2 * dt;
+ ao_orient.q3 += q_dot.q3 * dt;
+
+ ao_quat_normalize(&ao_orient);
+
+ ao_quat_rot(&ao_current, &ao_up, &ao_orient);
+
+ ao_sample_qangle = 180 / M_PI * acos(ao_current.q3 * sqrt(2));
+#if 0
+ printf ("orient (%g) %g %g %g current (%g) %g %g %g\n",
+ ao_orient.q0,
+ ao_orient.q1,
+ ao_orient.q2,
+ ao_orient.q3,
+ ao_current.q0,
+ ao_current.q1,
+ ao_current.q2,
+ ao_current.q3);
+#endif
+}
+#endif
+
void
ao_sleep(void *wchan)
{
@@ -497,7 +774,11 @@ ao_sleep(void *wchan)
for (;;) {
if (ao_records_read > 2 && ao_flight_state == ao_flight_startup)
{
+#if TELEMEGA
+ ao_data_static.mpu6000 = ao_ground_mpu6000;
+#else
ao_data_static.adc.accel = ao_flight_ground_accel;
+#endif
ao_insert();
return;
}
@@ -519,13 +800,102 @@ ao_sleep(void *wchan)
if (words[nword] == NULL)
break;
}
- if (nword == 4) {
+#if TELEMEGA
+ if (log_format == AO_LOG_FORMAT_TELEMEGA && nword == 30 && strlen(words[0]) == 1) {
+ int i;
+ struct ao_ms5607_value value;
+
+ type = words[0][0];
+ tick = strtoul(words[1], NULL, 16);
+// printf ("%c %04x", type, tick);
+ for (i = 2; i < nword; i++) {
+ bytes[i - 2] = strtoul(words[i], NULL, 16);
+// printf(" %02x", bytes[i-2]);
+ }
+// printf ("\n");
+ switch (type) {
+ case 'F':
+ ao_flight_ground_accel = int16(bytes, 2);
+ ao_flight_started = 1;
+ ao_ground_pres = int32(bytes, 4);
+ ao_ground_height = ao_pa_to_altitude(ao_ground_pres);
+ break;
+ case 'A':
+ ao_data_static.tick = tick;
+ ao_data_static.ms5607_raw.pres = int32(bytes, 0);
+ ao_data_static.ms5607_raw.temp = int32(bytes, 4);
+ ao_ms5607_convert(&ao_data_static.ms5607_raw, &value);
+ ao_data_static.mpu6000.accel_x = int16(bytes, 8);
+ ao_data_static.mpu6000.accel_y = -int16(bytes, 10);
+ ao_data_static.mpu6000.accel_z = int16(bytes, 12);
+ ao_data_static.mpu6000.gyro_x = int16(bytes, 14);
+ ao_data_static.mpu6000.gyro_y = -int16(bytes, 16);
+ ao_data_static.mpu6000.gyro_z = int16(bytes, 18);
+#if HAS_MMA655X
+ ao_data_static.mma655x = int16(bytes, 26);
+#endif
+ if (ao_records_read == 0)
+ ao_ground_mpu6000 = ao_data_static.mpu6000;
+ else if (ao_records_read < 10) {
+#define f(f) ao_ground_mpu6000.f = ao_ground_mpu6000.f + ((ao_data_static.mpu6000.f - ao_ground_mpu6000.f) >> 2)
+ f(accel_x);
+ f(accel_y);
+ f(accel_z);
+ f(gyro_x);
+ f(gyro_y);
+ f(gyro_z);
+
+ double accel_x = ao_mpu6000_accel(ao_ground_mpu6000.accel_x);
+ double accel_y = ao_mpu6000_accel(ao_ground_mpu6000.accel_y);
+ double accel_z = ao_mpu6000_accel(ao_ground_mpu6000.accel_z);
+
+ /* X and Y are in the ground plane, arbitraryily picked as MPU X and Z axes
+ * Z is normal to the ground, the MPU y axis
+ */
+ set_orientation(accel_x, accel_z, accel_y, tick);
+ } else {
+ double rate_x = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x);
+ double rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y);
+ double rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z);
+
+ update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick);
+ }
+ ao_records_read++;
+ ao_insert();
+ return;
+ }
+ continue;
+ } else if (nword == 3 && strcmp(words[0], "ms5607") == 0) {
+ if (strcmp(words[1], "reserved:") == 0)
+ ms5607_prom.reserved = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "sens:") == 0)
+ ms5607_prom.sens = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "off:") == 0)
+ ms5607_prom.off = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "tcs:") == 0)
+ ms5607_prom.tcs = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "tco:") == 0)
+ ms5607_prom.tco = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "tref:") == 0)
+ ms5607_prom.tref = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "tempsens:") == 0)
+ ms5607_prom.tempsens = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "crc:") == 0)
+ ms5607_prom.crc = strtoul(words[2], NULL, 10);
+ continue;
+ }
+#else
+ if (nword == 4 && log_format != AO_LOG_FORMAT_TELEMEGA) {
type = words[0][0];
tick = strtoul(words[1], NULL, 16);
a = strtoul(words[2], NULL, 16);
b = strtoul(words[3], NULL, 16);
if (type == 'P')
type = 'A';
+ }
+#endif
+ else if (nword == 2 && strcmp(words[0], "log-format") == 0) {
+ log_format = strtoul(words[1], NULL, 10);
} else if (nword >= 6 && strcmp(words[0], "Accel") == 0) {
ao_config.accel_plus_g = atoi(words[3]);
ao_config.accel_minus_g = atoi(words[5]);
@@ -608,22 +978,22 @@ ao_sleep(void *wchan)
}
} else if (len == 99) {
ao_flight_started = 1;
- tick = uint16(bytes, 21);
- ao_flight_ground_accel = int16(bytes, 7);
- ao_config.accel_plus_g = int16(bytes, 17);
- ao_config.accel_minus_g = int16(bytes, 19);
+ tick = uint16(bytes+1, 21);
+ ao_flight_ground_accel = int16(bytes+1, 7);
+ ao_config.accel_plus_g = int16(bytes+1, 17);
+ ao_config.accel_minus_g = int16(bytes+1, 19);
type = 'A';
- a = int16(bytes, 23);
- b = int16(bytes, 25);
+ a = int16(bytes+1, 23);
+ b = int16(bytes+1, 25);
} else if (len == 98) {
ao_flight_started = 1;
- tick = uint16(bytes, 20);
- ao_flight_ground_accel = int16(bytes, 6);
- ao_config.accel_plus_g = int16(bytes, 16);
- ao_config.accel_minus_g = int16(bytes, 18);
+ tick = uint16(bytes+1, 20);
+ ao_flight_ground_accel = int16(bytes+1, 6);
+ ao_config.accel_plus_g = int16(bytes+1, 16);
+ ao_config.accel_minus_g = int16(bytes+1, 18);
type = 'A';
- a = int16(bytes, 22);
- b = int16(bytes, 24);
+ a = int16(bytes+1, 22);
+ b = int16(bytes+1, 24);
} else {
printf("unknown len %d\n", len);
continue;
@@ -632,6 +1002,10 @@ ao_sleep(void *wchan)
if (type != 'F' && !ao_flight_started)
continue;
+#if TELEMEGA
+ (void) a;
+ (void) b;
+#else
switch (type) {
case 'F':
ao_flight_ground_accel = a;
@@ -667,6 +1041,7 @@ ao_sleep(void *wchan)
case 'H':
break;
}
+#endif
}
}
diff --git a/src/test/ao_gps_test.c b/src/test/ao_gps_test.c
index d75a12ec..3844a326 100644
--- a/src/test/ao_gps_test.c
+++ b/src/test/ao_gps_test.c
@@ -88,6 +88,7 @@ ao_mutex_put(uint8_t *mutex)
static int
ao_gps_fd;
+#if 0
static void
ao_dbg_char(char c)
{
@@ -103,6 +104,7 @@ ao_dbg_char(char c)
}
write(1, line, strlen(line));
}
+#endif
#define QUEUE_LEN 4096
@@ -391,6 +393,7 @@ ao_serial1_putchar(char c)
#define AO_SERIAL_SPEED_4800 0
#define AO_SERIAL_SPEED_57600 1
+#define AO_SERIAL_SPEED_115200 2
static void
ao_serial1_set_speed(uint8_t speed)
@@ -407,6 +410,9 @@ ao_serial1_set_speed(uint8_t speed)
case AO_SERIAL_SPEED_57600:
cfsetspeed(&termios, B57600);
break;
+ case AO_SERIAL_SPEED_115200:
+ cfsetspeed(&termios, B115200);
+ break;
}
tcsetattr(fd, TCSAFLUSH, &termios);
tcflush(fd, TCIFLUSH);
@@ -420,7 +426,6 @@ ao_serial1_set_speed(uint8_t speed)
void
ao_dump_state(void *wchan)
{
- double lat, lon;
int i;
if (wchan == &ao_gps_data)
ao_gps_print(&ao_gps_data);
@@ -510,4 +515,5 @@ main (int argc, char **argv)
}
ao_gps_setup();
ao_gps();
+ return 0;
}
diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c
index 846daa94..81008b39 100644
--- a/src/test/ao_gps_test_skytraq.c
+++ b/src/test/ao_gps_test_skytraq.c
@@ -397,6 +397,7 @@ ao_serial1_putchar(char c)
#define AO_SERIAL_SPEED_4800 0
#define AO_SERIAL_SPEED_9600 1
#define AO_SERIAL_SPEED_57600 2
+#define AO_SERIAL_SPEED_115200 3
static void
ao_serial1_set_speed(uint8_t speed)
@@ -411,11 +412,14 @@ ao_serial1_set_speed(uint8_t speed)
cfsetspeed(&termios, B4800);
break;
case AO_SERIAL_SPEED_9600:
- cfsetspeed(&termios, B38400);
+ cfsetspeed(&termios, B9600);
break;
case AO_SERIAL_SPEED_57600:
cfsetspeed(&termios, B57600);
break;
+ case AO_SERIAL_SPEED_115200:
+ cfsetspeed(&termios, B115200);
+ break;
}
tcsetattr(fd, TCSAFLUSH, &termios);
tcflush(fd, TCIFLUSH);
@@ -423,6 +427,10 @@ ao_serial1_set_speed(uint8_t speed)
#define ao_time() 0
+uint8_t ao_task_minimize_latency;
+
+#define ao_usb_getchar() 0
+
#include "ao_gps_print.c"
#include "ao_gps_skytraq.c"
diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c
new file mode 100644
index 00000000..80671735
--- /dev/null
+++ b/src/test/ao_gps_test_ublox.c
@@ -0,0 +1,409 @@
+/*
+ * 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 AO_GPS_TEST
+#include "ao_host.h"
+#include <termios.h>
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#define AO_GPS_NUM_SAT_MASK (0xf << 0)
+#define AO_GPS_NUM_SAT_SHIFT (0)
+
+#define AO_GPS_VALID (1 << 4)
+#define AO_GPS_RUNNING (1 << 5)
+#define AO_GPS_DATE_VALID (1 << 6)
+#define AO_GPS_COURSE_VALID (1 << 7)
+
+struct ao_telemetry_location {
+ uint8_t year;
+ uint8_t month;
+ uint8_t day;
+ uint8_t hour;
+ uint8_t minute;
+ uint8_t second;
+ uint8_t flags;
+ int32_t latitude; /* degrees * 10⁷ */
+ int32_t longitude; /* degrees * 10⁷ */
+ int16_t altitude; /* m */
+ uint16_t ground_speed; /* cm/s */
+ uint8_t course; /* degrees / 2 */
+ uint8_t pdop; /* * 5 */
+ uint8_t hdop; /* * 5 */
+ uint8_t vdop; /* * 5 */
+ int16_t climb_rate; /* cm/s */
+ uint16_t h_error; /* m */
+ uint16_t v_error; /* m */
+};
+
+#define UBLOX_SAT_STATE_ACQUIRED (1 << 0)
+#define UBLOX_SAT_STATE_CARRIER_PHASE_VALID (1 << 1)
+#define UBLOX_SAT_BIT_SYNC_COMPLETE (1 << 2)
+#define UBLOX_SAT_SUBFRAME_SYNC_COMPLETE (1 << 3)
+#define UBLOX_SAT_CARRIER_PULLIN_COMPLETE (1 << 4)
+#define UBLOX_SAT_CODE_LOCKED (1 << 5)
+#define UBLOX_SAT_ACQUISITION_FAILED (1 << 6)
+#define UBLOX_SAT_EPHEMERIS_AVAILABLE (1 << 7)
+
+struct ao_telemetry_satellite_info {
+ uint8_t svid;
+ uint8_t c_n_1;
+};
+
+#define AO_TELEMETRY_SATELLITE_MAX_SAT 12
+
+struct ao_telemetry_satellite {
+ uint8_t channels;
+ struct ao_telemetry_satellite_info sats[AO_TELEMETRY_SATELLITE_MAX_SAT];
+};
+
+#define ao_gps_orig ao_telemetry_location
+#define ao_gps_tracking_orig ao_telemetry_satellite
+#define ao_gps_sat_orig ao_telemetry_satellite_info
+
+void
+ao_mutex_get(uint8_t *mutex)
+{
+}
+
+void
+ao_mutex_put(uint8_t *mutex)
+{
+}
+
+static int ao_gps_fd;
+static FILE *ao_gps_file;
+
+#if 0
+static void
+ao_dbg_char(char c)
+{
+ char line[128];
+ line[0] = '\0';
+ if (c < ' ') {
+ if (c == '\n')
+ sprintf (line, "\n");
+ else
+ sprintf (line, "\\%02x", ((int) c) & 0xff);
+ } else {
+ sprintf (line, "%c", c);
+ }
+ write(1, line, strlen(line));
+}
+#endif
+
+#include <sys/time.h>
+
+int
+get_millis(void)
+{
+ struct timeval tv;
+ gettimeofday(&tv, NULL);
+ return tv.tv_sec * 1000 + tv.tv_usec / 1000;
+}
+
+static uint8_t in_message[4096];
+static int in_len;
+static uint16_t recv_len;
+
+static void check_ublox_message(char *which, uint8_t *msg);
+
+char
+ao_serial1_getchar(void)
+{
+ char c;
+ uint8_t uc;
+ int i;
+
+ i = getc(ao_gps_file);
+ if (i == EOF) {
+ perror("getchar");
+ exit(1);
+ }
+ c = i;
+ uc = (uint8_t) c;
+ if (in_len || uc == 0xb5) {
+ in_message[in_len++] = c;
+ if (in_len == 6) {
+ recv_len = in_message[4] | (in_message[5] << 8);
+ } else if (in_len > 6 && in_len == recv_len + 8) {
+ check_ublox_message("recv", in_message + 2);
+ in_len = 0;
+ }
+
+ }
+ return c;
+}
+
+#define MESSAGE_LEN 4096
+
+static uint8_t message[MESSAGE_LEN];
+static int message_len;
+static uint16_t send_len;
+
+void
+ao_serial1_putchar(char c)
+{
+ int i;
+ uint8_t uc = (uint8_t) c;
+
+ if (message_len || uc == 0xb5) {
+ if (message_len < MESSAGE_LEN)
+ message[message_len++] = uc;
+ if (message_len == 6) {
+ send_len = message[4] | (message[5] << 8);
+ } else if (message_len > 6 && message_len == send_len + 8) {
+ check_ublox_message("send", message + 2);
+ message_len = 0;
+ }
+ }
+
+ for (;;) {
+ i = write(ao_gps_fd, &c, 1);
+ if (i == 1)
+ break;
+ if (i < 0 && (errno == EINTR || errno == EAGAIN))
+ continue;
+ perror("putchar");
+ exit(1);
+ }
+}
+
+#define AO_SERIAL_SPEED_4800 0
+#define AO_SERIAL_SPEED_9600 1
+#define AO_SERIAL_SPEED_57600 2
+#define AO_SERIAL_SPEED_115200 3
+
+static void
+ao_serial1_set_speed(uint8_t speed)
+{
+ int fd = ao_gps_fd;
+ struct termios termios;
+
+ printf ("\t\tset speed %d\n", speed);
+ tcdrain(fd);
+ tcgetattr(fd, &termios);
+ switch (speed) {
+ case AO_SERIAL_SPEED_4800:
+ cfsetspeed(&termios, B4800);
+ break;
+ case AO_SERIAL_SPEED_9600:
+ cfsetspeed(&termios, B9600);
+ break;
+ case AO_SERIAL_SPEED_57600:
+ cfsetspeed(&termios, B57600);
+ break;
+ case AO_SERIAL_SPEED_115200:
+ cfsetspeed(&termios, B115200);
+ break;
+ }
+ tcsetattr(fd, TCSAFLUSH, &termios);
+ tcflush(fd, TCIFLUSH);
+}
+
+#define ao_time() 0
+
+uint8_t ao_task_minimize_latency;
+
+#define ao_usb_getchar() 0
+
+#include "ao_gps_print.c"
+#include "ao_gps_ublox.c"
+
+static void
+check_ublox_message(char *which, uint8_t *msg)
+{
+ uint8_t class = msg[0];
+ uint8_t id = msg[1];
+ uint16_t len = msg[2] | (msg[3] << 8);
+ uint16_t i;
+ struct ao_ublox_cksum cksum_msg = { .a = msg[4 + len],
+ .b = msg[4 + len + 1] };
+ struct ao_ublox_cksum cksum= { 0, 0 };
+
+ for (i = 0; i < 4 + len; i++) {
+ add_cksum(&cksum, msg[i]);
+ }
+ if (cksum.a != cksum_msg.a || cksum.b != cksum_msg.b) {
+ printf ("\t%s: cksum mismatch %02x,%02x != %02x,%02x\n",
+ which,
+ cksum_msg.a & 0xff,
+ cksum_msg.b & 0xff,
+ cksum.a & 0xff,
+ cksum.b & 0xff);
+ return;
+ }
+ switch (class) {
+ case UBLOX_NAV:
+ switch (id) {
+ case UBLOX_NAV_DOP: ;
+ struct ublox_nav_dop *nav_dop = (void *) msg;
+ printf ("\tnav-dop iTOW %9u gDOP %5u dDOP %5u tDOP %5u vDOP %5u hDOP %5u nDOP %5u eDOP %5u\n",
+ nav_dop->itow,
+ nav_dop->gdop,
+ nav_dop->ddop,
+ nav_dop->tdop,
+ nav_dop->vdop,
+ nav_dop->hdop,
+ nav_dop->ndop,
+ nav_dop->edop);
+ return;
+ case UBLOX_NAV_POSLLH: ;
+ struct ublox_nav_posllh *nav_posllh = (void *) msg;
+ printf ("\tnav-posllh iTOW %9u lon %12.7f lat %12.7f height %10.3f hMSL %10.3f hAcc %10.3f vAcc %10.3f\n",
+ nav_posllh->itow,
+ nav_posllh->lon / 1e7,
+ nav_posllh->lat / 1e7,
+ nav_posllh->height / 1e3,
+ nav_posllh->hmsl / 1e3,
+ nav_posllh->hacc / 1e3,
+ nav_posllh->vacc / 1e3);
+ return;
+ case UBLOX_NAV_SOL: ;
+ struct ublox_nav_sol *nav_sol = (struct ublox_nav_sol *) msg;
+ printf ("\tnav-sol iTOW %9u fTOW %9d week %5d gpsFix %2d flags %02x\n",
+ nav_sol->itow, nav_sol->ftow, nav_sol->week,
+ nav_sol->gpsfix, nav_sol->flags);
+ return;
+ case UBLOX_NAV_SVINFO: ;
+ struct ublox_nav_svinfo *nav_svinfo = (struct ublox_nav_svinfo *) msg;
+ printf ("\tnav-svinfo iTOW %9u numCH %3d globalFlags %02x\n",
+ nav_svinfo->itow, nav_svinfo->numch, nav_svinfo->globalflags);
+ int i;
+ for (i = 0; i < nav_svinfo->numch; i++) {
+ struct ublox_nav_svinfo_block *nav_svinfo_block = (void *) (msg + 12 + 12 * i);
+ printf ("\t\tchn %3u svid %3u flags %02x quality %3u cno %3u elev %3d azim %6d prRes %9d\n",
+ nav_svinfo_block->chn,
+ nav_svinfo_block->svid,
+ nav_svinfo_block->flags,
+ nav_svinfo_block->quality,
+ nav_svinfo_block->cno,
+ nav_svinfo_block->elev,
+ nav_svinfo_block->azim,
+ nav_svinfo_block->prres);
+ }
+ return;
+ case UBLOX_NAV_VELNED: ;
+ struct ublox_nav_velned *nav_velned = (void *) msg;
+ printf ("\tnav-velned iTOW %9u velN %10.2f velE %10.2f velD %10.2f speed %10.2f gSpeed %10.2f heading %10.5f sAcc %10.2f cAcc %10.5f\n",
+ nav_velned->itow,
+ nav_velned->veln / 1e2,
+ nav_velned->vele / 1e2,
+ nav_velned->veld / 1e2,
+ nav_velned->speed / 1e2,
+ nav_velned->gspeed / 1e2,
+ nav_velned->heading / 1e5,
+ nav_velned->sacc / 1e5,
+ nav_velned->cacc / 1e6);
+ return;
+ case UBLOX_NAV_TIMEUTC:;
+ struct ublox_nav_timeutc *nav_timeutc = (void *) msg;
+ printf ("\tnav-timeutc iTOW %9u tAcc %5u nano %5d %4u-%2d-%2d %2d:%02d:%02d flags %02x\n",
+ nav_timeutc->itow,
+ nav_timeutc->tacc,
+ nav_timeutc->nano,
+ nav_timeutc->year,
+ nav_timeutc->month,
+ nav_timeutc->day,
+ nav_timeutc->hour,
+ nav_timeutc->min,
+ nav_timeutc->sec,
+ nav_timeutc->valid);
+ return;
+ }
+ break;
+ }
+#if 1
+ printf ("\t%s: class %02x id %02x len %d:", which, class & 0xff, id & 0xff, len & 0xffff);
+ for (i = 0; i < len; i++)
+ printf (" %02x", msg[4 + i]);
+ printf (" cksum %02x %02x", cksum_msg.a & 0xff, cksum_msg.b & 0xff);
+#endif
+ printf ("\n");
+}
+
+void
+ao_dump_state(void *wchan)
+{
+ if (wchan == &ao_gps_data)
+ ao_gps_print(&ao_gps_data);
+ else
+ ao_gps_tracking_print(&ao_gps_tracking_data);
+ putchar('\n');
+ return;
+}
+
+int
+ao_gps_open(const char *tty)
+{
+ struct termios termios;
+ int fd;
+
+ fd = open (tty, O_RDWR);
+ if (fd < 0)
+ return -1;
+
+ tcgetattr(fd, &termios);
+ cfmakeraw(&termios);
+ cfsetspeed(&termios, B4800);
+ tcsetattr(fd, TCSAFLUSH, &termios);
+
+ tcdrain(fd);
+ tcflush(fd, TCIFLUSH);
+ return fd;
+}
+
+#include <getopt.h>
+
+static const struct option options[] = {
+ { .name = "tty", .has_arg = 1, .val = 'T' },
+ { 0, 0, 0, 0},
+};
+
+static void usage(char *program)
+{
+ fprintf(stderr, "usage: %s [--tty <tty-name>]\n", program);
+ exit(1);
+}
+
+int
+main (int argc, char **argv)
+{
+ char *tty = "/dev/ttyUSB0";
+ int c;
+
+ while ((c = getopt_long(argc, argv, "T:", options, NULL)) != -1) {
+ switch (c) {
+ case 'T':
+ tty = optarg;
+ break;
+ default:
+ usage(argv[0]);
+ break;
+ }
+ }
+ ao_gps_fd = ao_gps_open(tty);
+ if (ao_gps_fd < 0) {
+ perror (tty);
+ exit (1);
+ }
+ ao_gps_file = fdopen(ao_gps_fd, "r");
+ ao_gps();
+ return 0;
+}
diff --git a/src/test/ao_micropeak_test.c b/src/test/ao_micropeak_test.c
new file mode 100644
index 00000000..5961bd93
--- /dev/null
+++ b/src/test/ao_micropeak_test.c
@@ -0,0 +1,220 @@
+/*
+ * 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.3f %10d %10d %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,
+ ao_pa_speed, ao_pa_accel);
+ }
+}
+
+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 double last_pressure;
+ static int been_here;
+ static int start_samples;
+ static int is_mp;
+ static int use_saved;
+
+ if (been_here && start_samples < 100) {
+ start_samples++;
+ return;
+ }
+ ao_micro_report();
+ if (use_saved) {
+ pa = last_pressure;
+ now = last_time;
+ use_saved = 0;
+// printf ("use saved %d %d\n", now, pa);
+ return;
+ }
+ 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;
+ } else if (!strcmp(toks[0], "Time")) {
+ time_id = 0;
+ pa_id = 1;
+ is_mp = 1;
+ continue;
+ }
+ time = strtod(toks[time_id],NULL);
+ pressure = strtod(toks[pa_id],NULL);
+ time *= 100;
+ if (been_here && time - last_time < 0.096 * 100)
+ continue;
+ if (is_mp && been_here) {
+ double avg_pressure = (pressure + last_pressure) / 2.0;
+ double avg_time = (time + last_time) / 2.0;
+
+ now = avg_time;
+ pa = avg_pressure;
+// printf ("new %d %d\n", now, pa);
+ use_saved = 1;
+ } else {
+ now = floor (time + 0.5);
+ pa = pressure;
+ }
+ last_pressure = pressure;
+ last_time = time;
+ been_here = 1;
+ 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..bb8f4d1d
--- /dev/null
+++ b/src/test/plotmicro
@@ -0,0 +1,16 @@
+#!/bin/sh
+for i in "$@"; do
+gnuplot -p << EOF &
+set title "$i"
+set ylabel "height (m)"
+set y2label "accel (m/s²)"
+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",\
+ "$i" using 1:6 with lines lt 3 axes x1y2 title "pa accel"
+EOF
+done
diff --git a/src/test/plotmm b/src/test/plotmm
new file mode 100755
index 00000000..5f5bd2ca
--- /dev/null
+++ b/src/test/plotmm
@@ -0,0 +1,11 @@
+gnuplot -persist << EOF
+set ylabel "altitude (m)"
+set y2label "angle (d)"
+set xlabel "time (s)"
+set xtics border out nomirror
+set ytics border out nomirror
+set y2tics border out nomirror
+plot "$1" using 1:3 with lines axes x1y1 title "raw height",\
+"$1" using 1:9 with lines axes x1y2 title "angle",\
+"$1" using 1:11 with lines axes x1y2 title "qangle"
+EOF
diff --git a/src/test/run-mm b/src/test/run-mm
new file mode 100755
index 00000000..6f3d97a2
--- /dev/null
+++ b/src/test/run-mm
@@ -0,0 +1,41 @@
+#!/bin/sh
+
+DIR=~/misc/rockets/flights
+
+for i in "$@"; do
+case "$i" in
+ */*)
+ file="$i"
+ ;;
+ *)
+ file="$DIR/$i"
+ ;;
+esac
+./ao_flight_test_mm "$file" > run-out.mm
+
+#./ao_flight_test_accel "$file" > run-out.accel
+#"run-out.accel" using 1:9 with lines lt 4 axes x1y1 title "accel height",\
+#"run-out.accel" using 1:11 with lines lt 4 axes x1y2 title "accel speed",\
+#"run-out.accel" using 1:13 with lines lt 4 axes x1y2 title "accel accel",\
+#"run-out.accel" using 1:15 with lines lt 4 axes x1y1 title "accel drogue",\
+#"run-out.accel" using 1:17 with lines lt 4 axes x1y1 title "accel main",\
+#
+
+gnuplot << EOF
+set ylabel "altitude (m)"
+set y2label "velocity (m/s), acceleration(m/s²)"
+set xlabel "time (s)"
+set xtics border out nomirror
+set ytics border out nomirror
+set y2tics border out nomirror
+set title "$i"
+plot "run-out.mm" using 1:3 with lines lw 2 lt 1 axes x1y1 title "raw height",\
+"run-out.mm" using 1:5 with lines lw 2 lt 1 axes x1y2 title "raw accel",\
+"run-out.mm" using 1:21 with lines lt 2 axes x1y1 title "mm height",\
+"run-out.mm" using 1:23 with lines lt 2 axes x1y2 title "mm speed",\
+"run-out.mm" using 1:25 with lines lt 2 axes x1y2 title "mm accel",\
+"run-out.mm" using 1:29 with lines lt 2 axes x1y1 title "mm drogue",\
+"run-out.mm" using 1:31 with lines lt 2 axes x1y1 title "mm main"
+pause mouse close
+EOF
+done \ No newline at end of file
generated by cgit v1.2.3 (git 2.39.1) at 2025-11-01 10:14:23 +0000