From b9bf8e01e243508297f28b102cb2477dc1bc74df Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 12 Oct 2012 14:22:41 -0700 Subject: altos: Add initial micropeak implementation Blinks out max height in decimeters, stores previous flight data to internal eeprom. Signed-off-by: Keith Packard --- src/micropeak/Makefile | 105 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 105 insertions(+) create mode 100644 src/micropeak/Makefile (limited to 'src/micropeak/Makefile') diff --git a/src/micropeak/Makefile b/src/micropeak/Makefile new file mode 100644 index 00000000..e0b2b80d --- /dev/null +++ b/src/micropeak/Makefile @@ -0,0 +1,105 @@ +# +# Tiny AltOS build +# +# +vpath % ../attiny:../drivers:../core:.. +vpath ao-make-product.5c ../util +vpath make-altitude-pa ../util + +MCU=attiny85 +DUDECPUTYPE=t85 +#PROGRAMMER=stk500v2 -P usb +PROGRAMMER=usbtiny +LOADCMD=avrdude +LOADARG=-p $(DUDECPUTYPE) -c $(PROGRAMMER) -e -U flash:w: +CC=avr-gcc +OBJCOPY=avr-objcopy + +ifndef VERSION +include ../Version +endif + +ALTOS_SRC = \ + ao_micropeak.c \ + ao_spi_attiny.c \ + ao_led.c \ + ao_clock.c \ + ao_ms5607.c \ + ao_exti.c \ + ao_convert_pa.c \ + ao_i2c_attiny.c \ + ao_at24c.c \ + ao_report_tiny.c \ + ao_async.c \ + ao_notask.c \ + ao_eeprom_tiny.c \ + ao_panic.c + +INC=\ + ao.h \ + ao_pins.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_exti.h \ + ao_ms5607.h \ + altitude-pa.h + +IDPRODUCT=0 +PRODUCT=MicroPeak-v0.1 +PRODUCT_DEF=-DMICROPEAK +CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../core -I.. -I../drivers +CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O3 -mcall-prologues -DATTINY + +NICKLE=nickle + +PROG=micropeak-v0.1 + +SRC=$(ALTOS_SRC) +OBJ=$(SRC:.c=.o) + +V=0 +# The user has explicitly enabled quiet compilation. +ifeq ($(V),0) +quiet = @printf " $1 $2 $@\n"; $($1) +endif +# Otherwise, print the full command line. +quiet ?= $($1) + +all: $(PROG) $(PROG).hex + +CHECK=sh ../util/check-avr-mem + +$(PROG): Makefile $(OBJ) + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) + $(call quiet,CHECK) $(PROG) || ($(RM) -f $(PROG); exit 1) + +$(PROG).hex: $(PROG) + avr-size $(PROG) + $(OBJCOPY) -R .eeprom -O ihex $(PROG) $@ + + +load: $(PROG).hex + $(LOADCMD) $(LOADARG)$(PROG).hex + +ao_product.h: ao-make-product.5c ../Version + $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ + +ao_product.o: ao_product.c ao_product.h + +%.o : %.c $(INC) + $(call quiet,CC) -c $(CFLAGS) $< + +distclean: clean + +clean: + rm -f *.o $(PROG) $(PROG).hex + rm -f ao_product.h + +../altitude-pa.h: make-altitude-pa + nickle $< > $@ + +install: + +uninstall: + +$(OBJ): ao_product.h $(INC) -- cgit v1.2.3 From e9ea0ad4024532fd6f87bb6708bf76b0c7aa1c5b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 29 Oct 2012 11:47:17 -0700 Subject: altos/micropeak: Switch to MS5607 sensor. Require 4m for boost. Elide dead code Signed-off-by: Keith Packard --- src/micropeak/Makefile | 13 +++++++++---- src/micropeak/ao_micropeak.c | 18 ++++++++++++------ src/micropeak/ao_pins.h | 2 +- 3 files changed, 22 insertions(+), 11 deletions(-) (limited to 'src/micropeak/Makefile') diff --git a/src/micropeak/Makefile b/src/micropeak/Makefile index e0b2b80d..8cf608bb 100644 --- a/src/micropeak/Makefile +++ b/src/micropeak/Makefile @@ -19,6 +19,13 @@ ifndef VERSION include ../Version endif +# Support for a logging EEPROM +# +#EEPROM_SRC=ao_async.c \ +# ao_i2c_attiny.c \ +# ao_at24c.c +# + ALTOS_SRC = \ ao_micropeak.c \ ao_spi_attiny.c \ @@ -27,13 +34,11 @@ ALTOS_SRC = \ ao_ms5607.c \ ao_exti.c \ ao_convert_pa.c \ - ao_i2c_attiny.c \ - ao_at24c.c \ ao_report_tiny.c \ - ao_async.c \ ao_notask.c \ ao_eeprom_tiny.c \ - ao_panic.c + ao_panic.c \ + $(EEPROM_SRC) INC=\ ao.h \ diff --git a/src/micropeak/ao_micropeak.c b/src/micropeak/ao_micropeak.c index 6ceec3b5..1d11300e 100644 --- a/src/micropeak/ao_micropeak.c +++ b/src/micropeak/ao_micropeak.c @@ -47,6 +47,12 @@ ao_pa_get(void) #define GROUND_AVG_SHIFT 4 #define GROUND_AVG (1 << GROUND_AVG_SHIFT) +/* Pressure change (in Pa) to detect boost */ +#define BOOST_DETECT 48 /* 4m at sea level, 4.8m at 2000m */ + +/* Pressure change (in Pa) to detect landing */ +#define LAND_DETECT 12 /* 1m at sea level, 1.2m at 2000m */ + static void ao_compute_height(void) { @@ -122,9 +128,9 @@ main(void) sample_count = 0; for (;;) { time += SAMPLE_SLEEP; - ao_delay_until(time); if (sample_count == 0) ao_led_on(AO_LED_BLUE); + ao_delay_until(time); ao_pa_get(); if (sample_count == 0) ao_led_off(AO_LED_BLUE); @@ -133,12 +139,11 @@ main(void) if (pa_diff < 0) pa_diff = -pa_diff; - /* about 2 meters at sea level, more if you're higher */ - if (pa_diff > (24 << FILTER_SHIFT)) + /* Check for a significant pressure change */ + if (pa_diff > (BOOST_DETECT << FILTER_SHIFT)) break; if (sample_count < GROUND_AVG * 2) { - ao_led_off(AO_LED_BLUE); if (sample_count < GROUND_AVG) pa_sum += pa; ++sample_count; @@ -178,8 +183,9 @@ main(void) if (sample_count == (GROUND_AVG - 1)) { pa_diff = pa_interval_max - pa_interval_min; - /* About 1m at sea level */ - if (pa_diff < (12 << FILTER_SHIFT)) + + /* 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; diff --git a/src/micropeak/ao_pins.h b/src/micropeak/ao_pins.h index de9fc7f2..67de1a8e 100644 --- a/src/micropeak/ao_pins.h +++ b/src/micropeak/ao_pins.h @@ -29,7 +29,7 @@ #define HAS_SERIAL_1 0 #define HAS_TASK 0 #define HAS_MS5607 1 -#define HAS_MS5611 1 +#define HAS_MS5611 0 #define HAS_EEPROM 0 #define HAS_BEEP 0 #define AVR_CLOCK 8000000UL -- cgit v1.2.3 From 07a45c50429389ae7b51e12bc847d34fb1577bc6 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 12 Dec 2012 10:57:03 -0800 Subject: altos: Add load-slow target for MicroPeak This sets the programming clock to 1/4 of the 250kHz clock used by the MicroPeak firmware, allowing the device to be reprogrammed. Signed-off-by: Keith Packard --- src/micropeak/Makefile | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/micropeak/Makefile') diff --git a/src/micropeak/Makefile b/src/micropeak/Makefile index 8cf608bb..0c48ed66 100644 --- a/src/micropeak/Makefile +++ b/src/micropeak/Makefile @@ -11,6 +11,7 @@ DUDECPUTYPE=t85 #PROGRAMMER=stk500v2 -P usb PROGRAMMER=usbtiny LOADCMD=avrdude +LOADSLOW=-i 32 -B 32 LOADARG=-p $(DUDECPUTYPE) -c $(PROGRAMMER) -e -U flash:w: CC=avr-gcc OBJCOPY=avr-objcopy @@ -86,6 +87,9 @@ $(PROG).hex: $(PROG) load: $(PROG).hex $(LOADCMD) $(LOADARG)$(PROG).hex +load-slow: $(PROG).hex + $(LOADCMD) $(LOADSLOW) $(LOADARG)$(PROG).hex + ao_product.h: ao-make-product.5c ../Version $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ -- cgit v1.2.3 From fc2e5beb9173663e1e37a9b5a7b6eea1046222f7 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 14 Dec 2012 11:11:39 -0800 Subject: altos: Log baro readings for MicroPeak This logs barometric data every 192ms (more or less) to the 504 remaining bytes of internal EEPROM storage in the ATtiny85. This provides 48.192 seconds of logging. Signed-off-by: Keith Packard --- src/micropeak/Makefile | 12 ++-- src/micropeak/ao_log_micro.c | 65 ++++++++++------------ src/micropeak/ao_log_micro.h | 19 ++++--- src/micropeak/ao_micropeak.c | 129 +++++++------------------------------------ src/micropeak/ao_micropeak.h | 56 +++++++++++++++++++ 5 files changed, 121 insertions(+), 160 deletions(-) create mode 100644 src/micropeak/ao_micropeak.h (limited to 'src/micropeak/Makefile') diff --git a/src/micropeak/Makefile b/src/micropeak/Makefile index 0c48ed66..82944cb1 100644 --- a/src/micropeak/Makefile +++ b/src/micropeak/Makefile @@ -20,13 +20,6 @@ ifndef VERSION include ../Version endif -# Support for a logging EEPROM -# -#EEPROM_SRC=ao_async.c \ -# ao_i2c_attiny.c \ -# ao_at24c.c -# - ALTOS_SRC = \ ao_micropeak.c \ ao_spi_attiny.c \ @@ -39,7 +32,8 @@ ALTOS_SRC = \ ao_notask.c \ ao_eeprom_tiny.c \ ao_panic.c \ - $(EEPROM_SRC) + ao_log_micro.c \ + ao_async.c INC=\ ao.h \ @@ -48,6 +42,8 @@ INC=\ ao_arch_funcs.h \ ao_exti.h \ ao_ms5607.h \ + ao_log_micro.h \ + ao_micropeak.h \ altitude-pa.h IDPRODUCT=0 diff --git a/src/micropeak/ao_log_micro.c b/src/micropeak/ao_log_micro.c index eda0d1d2..40a7a35d 100644 --- a/src/micropeak/ao_log_micro.c +++ b/src/micropeak/ao_log_micro.c @@ -16,58 +16,53 @@ */ #include +#include #include #include -#if HAS_EEPROM - -ao_pos_t ao_log_micro_pos; +static uint16_t ao_log_offset = STARTING_LOG_OFFSET; void -ao_log_micro_data(uint32_t data) +ao_log_micro_save(void) { - ao_storage_write(ao_log_micro_pos, &data, sizeof (data)); - ao_log_micro_pos += sizeof (data); + uint16_t n_samples = (ao_log_offset - STARTING_LOG_OFFSET) / sizeof (uint16_t); + ao_eeprom_write(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground)); + ao_eeprom_write(PA_MIN_OFFSET, &pa_min, sizeof (pa_min)); + ao_eeprom_write(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); } -uint32_t ao_log_last_ground; -uint32_t ao_log_last_done; - -uint8_t -ao_log_micro_scan(void) +void +ao_log_micro_restore(void) { - uint32_t data; - ao_pos_t pos; + ao_eeprom_read(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground)); + ao_eeprom_read(PA_MIN_OFFSET, &pa_min, sizeof (pa_min)); +} - ao_storage_read(0, &data, sizeof (data)); - if ((data & AO_LOG_MICRO_MASK) != AO_LOG_MICRO_GROUND) - return 0; +void +ao_log_micro_data(void) +{ + uint16_t low_bits = pa; - ao_log_last_ground = data & ~(AO_LOG_MICRO_MASK); - for (pos = 4; pos < ao_storage_total; pos += 4) { - ao_storage_read(pos, &data, sizeof (data)); - if ((data & AO_LOG_MICRO_MASK) == AO_LOG_MICRO_GROUND) { - ao_log_last_done = data & ~(AO_LOG_MICRO_MASK); - return 1; - } + if (ao_log_offset < MAX_LOG_OFFSET) { + ao_eeprom_write(ao_log_offset, &low_bits, sizeof (low_bits)); + ao_log_offset += sizeof (low_bits); } - return 0; } void ao_log_micro_dump(void) { - ao_pos_t pos; - uint8_t data[4]; - uint8_t i; + uint16_t n_samples; + uint16_t nbytes; + uint8_t byte; + uint16_t b; - for (pos = 0; pos < ao_storage_total; pos += 4) { - ao_storage_read(pos, data, 4); - for (i = 0; i < 4; i++) - ao_async_byte(data[i]); - if (data[3] == (uint8_t) (AO_LOG_MICRO_GROUND >> 24)) - break; + ao_eeprom_read(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); + nbytes = STARTING_LOG_OFFSET + sizeof (uint16_t) * n_samples; + ao_async_byte('M'); + ao_async_byte('P'); + for (b = 0; b < nbytes; b++) { + ao_eeprom_read(b, &byte, 1); + ao_async_byte(byte); } } - -#endif diff --git a/src/micropeak/ao_log_micro.h b/src/micropeak/ao_log_micro.h index 15b2d178..976852ee 100644 --- a/src/micropeak/ao_log_micro.h +++ b/src/micropeak/ao_log_micro.h @@ -18,19 +18,20 @@ #ifndef _AO_LOG_MICRO_H_ #define _AO_LOG_MICRO_H_ -#define AO_LOG_MICRO_GROUND (0l << 24) -#define AO_LOG_MICRO_DATA (1l << 24) -#define AO_LOG_MICRO_DONE (0xaal << 24) -#define AO_LOG_MICRO_MASK (0xffl << 24) +#define PA_GROUND_OFFSET 0 +#define PA_MIN_OFFSET 4 +#define N_SAMPLES_OFFSET 8 +#define STARTING_LOG_OFFSET 10 +#define MAX_LOG_OFFSET 512 void -ao_log_micro_data(uint32_t data); +ao_log_micro_save(void); -extern uint32_t ao_log_last_ground; -extern uint32_t ao_log_last_done; +void +ao_log_micro_restore(void); -uint8_t -ao_log_micro_scan(void); +void +ao_log_micro_data(void); void ao_log_micro_dump(void); diff --git a/src/micropeak/ao_micropeak.c b/src/micropeak/ao_micropeak.c index 525cfa42..3bbc7eea 100644 --- a/src/micropeak/ao_micropeak.c +++ b/src/micropeak/ao_micropeak.c @@ -16,22 +16,22 @@ */ #include +#include #include #include static struct ao_ms5607_sample sample; static struct ao_ms5607_value value; -static uint32_t pa; -static uint32_t pa_sum; -static uint32_t pa_avg; -static int32_t pa_diff; -static uint32_t pa_ground; -static uint32_t pa_min; -static uint32_t pa_interval_min, pa_interval_max; -static alt_t ground_alt, max_alt; +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 ao_pa_get(void) { @@ -40,22 +40,6 @@ ao_pa_get(void) pa = value.pres; } -#define FILTER_SHIFT 3 -#define SAMPLE_SLEEP AO_MS_TO_TICKS(96) - -/* 16 sample, or about two seconds worth */ -#define GROUND_AVG_SHIFT 4 -#define GROUND_AVG (1 << GROUND_AVG_SHIFT) - -/* Pressure change (in Pa) to detect boost */ -#define BOOST_DETECT 120 /* 10m at sea level, 12m at 2000m */ - -/* Wait after power on before doing anything to give the user time to assemble the rocket */ -#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 */ - static void ao_compute_height(void) { @@ -64,90 +48,30 @@ ao_compute_height(void) ao_max_height = max_alt - ground_alt; } -#if !HAS_EEPROM - -#define PA_GROUND_OFFSET 0 -#define PA_MIN_OFFSET 4 -#define N_SAMPLES_OFFSET 8 -#define STARTING_LOG_OFFSET 10 -#define MAX_LOG_OFFSET 512 - -static uint16_t ao_log_offset = STARTING_LOG_OFFSET; - -void -ao_save_flight(void) -{ - uint16_t n_samples = (ao_log_offset - STARTING_LOG_OFFSET) / sizeof (uint16_t); - ao_eeprom_write(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground)); - ao_eeprom_write(PA_MIN_OFFSET, &pa_min, sizeof (pa_min)); - ao_eeprom_write(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); -} - -void -ao_restore_flight(void) -{ - ao_eeprom_read(PA_GROUND_OFFSET, &pa_ground, sizeof (pa_ground)); - ao_eeprom_read(PA_MIN_OFFSET, &pa_min, sizeof (pa_min)); -} - -void -ao_log_micro(void) -{ - uint16_t low_bits = pa; - - if (ao_log_offset < MAX_LOG_OFFSET) { - ao_eeprom_write(ao_log_offset, &low_bits, sizeof (low_bits)); - ao_log_offset += sizeof (low_bits); - } -} -#endif - int main(void) { int16_t sample_count; uint16_t time; -#if HAS_EEPROM - uint8_t dump_eeprom = 0; -#endif + uint32_t pa_interval_min, pa_interval_max; + int32_t pa_diff; + ao_led_init(LEDS_AVAILABLE); ao_timer_init(); -#if HAS_EEPROM - - /* Set MOSI and CLK as inputs with pull-ups */ - DDRB &= ~(1 << 0) | (1 << 2); - PORTB |= (1 << 0) | (1 << 2); - - /* Check to see if either MOSI or CLK are pulled low by the - * user shorting them to ground. If so, dump the eeprom out - * via the LED. Wait for the shorting wire to go away before - * continuing. - */ - while ((PINB & ((1 << 0) | (1 << 2))) != ((1 << 0) | (1 << 2))) - dump_eeprom = 1; - PORTB &= ~(1 << 0) | (1 << 2); - - ao_i2c_init(); -#endif - ao_restore_flight(); - ao_compute_height(); - /* Give the person a second to get their finger out of the way */ - ao_delay(AO_MS_TO_TICKS(1000)); - ao_report_altitude(); - + /* Init external hardware */ ao_spi_init(); ao_ms5607_init(); ao_ms5607_setup(); -#if HAS_EEPROM - ao_storage_init(); - - /* Check to see if there's a flight recorded in memory */ - if (dump_eeprom && ao_log_micro_scan()) - ao_log_micro_dump(); -#endif + /* Give the person a second to get their finger out of the way */ + ao_delay(AO_MS_TO_TICKS(1000)); + ao_log_micro_restore(); + ao_compute_height(); + ao_report_altitude(); + ao_log_micro_dump(); + ao_delay(BOOST_DELAY); /* Wait for motion, averaging values to get ground pressure */ time = ao_time(); @@ -182,10 +106,6 @@ main(void) pa_ground >>= FILTER_SHIFT; -#if HAS_EEPROM - ao_log_micro_data(AO_LOG_MICRO_GROUND | pa_ground); -#endif - /* Now sit around until the pressure is stable again and record the max */ sample_count = 0; @@ -200,12 +120,8 @@ main(void) ao_pa_get(); if ((sample_count & 3) == 0) ao_led_off(AO_LED_REPORT); -#if HAS_EEPROM - ao_log_micro_data(AO_LOG_MICRO_DATA | pa); -#else if (sample_count & 1) - ao_log_micro(); -#endif + ao_log_micro_data(); pa_avg = pa_avg - (pa_avg >> FILTER_SHIFT) + pa; if (pa_avg < pa_min) pa_min = pa_avg; @@ -228,10 +144,7 @@ main(void) } } pa_min >>= FILTER_SHIFT; -#if HAS_EEPROM - ao_log_micro_data(AO_LOG_MICRO_DONE | pa_min); -#endif - ao_save_flight(); + ao_log_micro_save(); ao_compute_height(); ao_report_altitude(); for (;;) { diff --git a/src/micropeak/ao_micropeak.h b/src/micropeak/ao_micropeak.h new file mode 100644 index 00000000..e408d7c5 --- /dev/null +++ b/src/micropeak/ao_micropeak.h @@ -0,0 +1,56 @@ +/* + * Copyright © 2012 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_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 GROUND_AVG_SHIFT 4 +#define GROUND_AVG (1 << GROUND_AVG_SHIFT) + +/* Pressure change (in Pa) to detect boost */ +#define BOOST_DETECT 120 /* 10m at sea level, 12m at 2000m */ + +/* Wait after power on before doing anything to give the user time to assemble the rocket */ +#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 */ + +/* 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; + +/* Minimum recorded filtered pressure value */ +extern uint32_t pa_min; + +/* Pressure values converted to altitudes */ +extern alt_t ground_alt, max_alt; + +/* max_alt - ground_alt */ +extern alt_t ao_max_height; + +#endif + -- cgit v1.2.3 From 23dc9a63ae8bc982d9352cfb7a3f508d8a08c374 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 17 Dec 2012 22:58:49 -0800 Subject: altos: Make micropeak 'serial' interface work I prototyped the mpserial interface on a breadboard and tuned the circuit to register the LED correctly. Then adjusted the serial code to send bits at the right speed and format. The logging contents are now in hexdecimal with a CCITT CRC-16 computed to verify correct reception. Signed-off-by: Keith Packard --- src/micropeak/Makefile | 2 +- src/micropeak/ao_async.c | 43 +++++++++++++++++++++++++++++----- src/micropeak/ao_async.h | 6 +++++ src/micropeak/ao_log_micro.c | 55 +++++++++++++++++++++++++++++++++++++++++++- src/micropeak/ao_micropeak.c | 1 + 5 files changed, 99 insertions(+), 8 deletions(-) (limited to 'src/micropeak/Makefile') diff --git a/src/micropeak/Makefile b/src/micropeak/Makefile index 82944cb1..ff0a4499 100644 --- a/src/micropeak/Makefile +++ b/src/micropeak/Makefile @@ -50,7 +50,7 @@ IDPRODUCT=0 PRODUCT=MicroPeak-v0.1 PRODUCT_DEF=-DMICROPEAK CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../core -I.. -I../drivers -CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O3 -mcall-prologues -DATTINY +CFLAGS += -g -mmcu=$(MCU) -Wall -Wstrict-prototypes -O2 -mcall-prologues -DATTINY NICKLE=nickle diff --git a/src/micropeak/ao_async.c b/src/micropeak/ao_async.c index 04bba9e8..3556f54c 100644 --- a/src/micropeak/ao_async.c +++ b/src/micropeak/ao_async.c @@ -21,20 +21,51 @@ #define AO_ASYNC_BAUD 38400l #define AO_ASYNC_DELAY (uint8_t) (1000000l / AO_ASYNC_BAUD) +#define LED_PORT PORTB + +void +ao_async_start(void) +{ + LED_PORT |= (1 << AO_LED_SERIAL); +} + +void +ao_async_stop(void) +{ + LED_PORT &= ~(1 << AO_LED_SERIAL); +} + void ao_async_byte(uint8_t byte) { uint8_t b; uint16_t w; - /* start bit */ - - /* start data stop */ - w = 0x001 | (byte << 1) | 0x000; + /* start data stop */ + w = (0x000 << 0) | (byte << 1) | (0x001 << 9); + ao_arch_block_interrupts(); for (b = 0; b < 10; b++) { - ao_led_set((w & 1) << AO_LED_SERIAL); + uint8_t v = LED_PORT & ~(1 << AO_LED_SERIAL); + v |= (w & 1) << AO_LED_SERIAL; + LED_PORT = v; w >>= 1; - ao_delay_us(26); + + /* Carefully timed to hit around 9600 baud */ + asm volatile ("nop"); + asm volatile ("nop"); + + asm volatile ("nop"); + asm volatile ("nop"); + asm volatile ("nop"); + asm volatile ("nop"); + asm volatile ("nop"); + + asm volatile ("nop"); + asm volatile ("nop"); + asm volatile ("nop"); + asm volatile ("nop"); + asm volatile ("nop"); } + ao_arch_release_interrupts(); } diff --git a/src/micropeak/ao_async.h b/src/micropeak/ao_async.h index a06d2e1a..1b239712 100644 --- a/src/micropeak/ao_async.h +++ b/src/micropeak/ao_async.h @@ -18,6 +18,12 @@ #ifndef _AO_ASYNC_H_ #define _AO_ASYNC_H_ +void +ao_async_start(void); + +void +ao_async_stop(void); + void ao_async_byte(uint8_t byte); diff --git a/src/micropeak/ao_log_micro.c b/src/micropeak/ao_log_micro.c index 40a7a35d..d665efb5 100644 --- a/src/micropeak/ao_log_micro.c +++ b/src/micropeak/ao_log_micro.c @@ -49,6 +49,46 @@ ao_log_micro_data(void) } } +#define POLY 0x8408 + +static uint16_t +ao_log_micro_crc(uint16_t crc, uint8_t byte) +{ + uint8_t i; + + for (i = 0; i < 8; i++) { + if ((crc & 0x0001) ^ (byte & 0x0001)) + crc = (crc >> 1) ^ POLY; + else + crc = crc >> 1; + byte >>= 1; + } + return crc; +} + +static void +ao_log_hex_nibble(uint8_t b) +{ + if (b < 10) + ao_async_byte('0' + b); + else + ao_async_byte('a' - 10 + b); +} + +static void +ao_log_hex(uint8_t b) +{ + ao_log_hex_nibble(b>>4); + ao_log_hex_nibble(b&0xf); +} + +static void +ao_log_newline(void) +{ + ao_async_byte('\r'); + ao_async_byte('\n'); +} + void ao_log_micro_dump(void) { @@ -56,13 +96,26 @@ ao_log_micro_dump(void) uint16_t nbytes; uint8_t byte; uint16_t b; + uint16_t crc = 0xffff; ao_eeprom_read(N_SAMPLES_OFFSET, &n_samples, sizeof (n_samples)); + if (n_samples == 0xffff) + n_samples = 0; nbytes = STARTING_LOG_OFFSET + sizeof (uint16_t) * n_samples; + ao_async_start(); ao_async_byte('M'); ao_async_byte('P'); for (b = 0; b < nbytes; b++) { + if ((b & 0xf) == 0) + ao_log_newline(); ao_eeprom_read(b, &byte, 1); - ao_async_byte(byte); + ao_log_hex(byte); + crc = ao_log_micro_crc(crc, byte); } + ao_log_newline(); + crc = ~crc; + ao_log_hex(crc >> 8); + ao_log_hex(crc); + ao_log_newline(); + ao_async_stop(); } diff --git a/src/micropeak/ao_micropeak.c b/src/micropeak/ao_micropeak.c index 3bbc7eea..f579a09a 100644 --- a/src/micropeak/ao_micropeak.c +++ b/src/micropeak/ao_micropeak.c @@ -19,6 +19,7 @@ #include #include #include +#include static struct ao_ms5607_sample sample; static struct ao_ms5607_value value; -- cgit v1.2.3 From 540309240a8515116120dbd4403902282ed8c27b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 16 Jan 2013 15:15:49 -0800 Subject: 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 (limited to 'src/micropeak/Makefile') 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 -- cgit v1.2.3