From b9bf8e01e243508297f28b102cb2477dc1bc74df Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 12 Oct 2012 14:22:41 -0700 Subject: [PATCH] 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 +++++++++++++++++ src/micropeak/ao_async.c | 40 +++++++ src/micropeak/ao_async.h | 24 ++++ src/micropeak/ao_log_micro.c | 73 ++++++++++++ src/micropeak/ao_log_micro.h | 38 ++++++ src/micropeak/ao_micropeak.c | 207 +++++++++++++++++++++++++++++++++ src/micropeak/ao_morse.c | 95 +++++++++++++++ src/micropeak/ao_notask.c | 45 +++++++ src/micropeak/ao_pins.h | 60 ++++++++++ src/micropeak/ao_report_tiny.c | 57 +++++++++ 10 files changed, 744 insertions(+) create mode 100644 src/micropeak/Makefile create mode 100644 src/micropeak/ao_async.c create mode 100644 src/micropeak/ao_async.h create mode 100644 src/micropeak/ao_log_micro.c create mode 100644 src/micropeak/ao_log_micro.h create mode 100644 src/micropeak/ao_micropeak.c create mode 100644 src/micropeak/ao_morse.c create mode 100644 src/micropeak/ao_notask.c create mode 100644 src/micropeak/ao_pins.h create mode 100644 src/micropeak/ao_report_tiny.c 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) diff --git a/src/micropeak/ao_async.c b/src/micropeak/ao_async.c new file mode 100644 index 00000000..04bba9e8 --- /dev/null +++ b/src/micropeak/ao_async.c @@ -0,0 +1,40 @@ +/* + * 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. + */ + +#include +#include + +#define AO_ASYNC_BAUD 38400l +#define AO_ASYNC_DELAY (uint8_t) (1000000l / AO_ASYNC_BAUD) + +void +ao_async_byte(uint8_t byte) +{ + uint8_t b; + uint16_t w; + + /* start bit */ + + /* start data stop */ + w = 0x001 | (byte << 1) | 0x000; + + for (b = 0; b < 10; b++) { + ao_led_set((w & 1) << AO_LED_SERIAL); + w >>= 1; + ao_delay_us(26); + } +} diff --git a/src/micropeak/ao_async.h b/src/micropeak/ao_async.h new file mode 100644 index 00000000..a06d2e1a --- /dev/null +++ b/src/micropeak/ao_async.h @@ -0,0 +1,24 @@ +/* + * 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_ASYNC_H_ +#define _AO_ASYNC_H_ + +void +ao_async_byte(uint8_t byte); + +#endif /* _AO_ASYNC_H_ */ diff --git a/src/micropeak/ao_log_micro.c b/src/micropeak/ao_log_micro.c new file mode 100644 index 00000000..eda0d1d2 --- /dev/null +++ b/src/micropeak/ao_log_micro.c @@ -0,0 +1,73 @@ +/* + * 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. + */ + +#include +#include +#include + +#if HAS_EEPROM + +ao_pos_t ao_log_micro_pos; + +void +ao_log_micro_data(uint32_t data) +{ + ao_storage_write(ao_log_micro_pos, &data, sizeof (data)); + ao_log_micro_pos += sizeof (data); +} + +uint32_t ao_log_last_ground; +uint32_t ao_log_last_done; + +uint8_t +ao_log_micro_scan(void) +{ + uint32_t data; + ao_pos_t pos; + + ao_storage_read(0, &data, sizeof (data)); + if ((data & AO_LOG_MICRO_MASK) != AO_LOG_MICRO_GROUND) + return 0; + + 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; + } + } + return 0; +} + +void +ao_log_micro_dump(void) +{ + ao_pos_t pos; + uint8_t data[4]; + uint8_t i; + + 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; + } +} + +#endif diff --git a/src/micropeak/ao_log_micro.h b/src/micropeak/ao_log_micro.h new file mode 100644 index 00000000..15b2d178 --- /dev/null +++ b/src/micropeak/ao_log_micro.h @@ -0,0 +1,38 @@ +/* + * 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_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) + +void +ao_log_micro_data(uint32_t data); + +extern uint32_t ao_log_last_ground; +extern uint32_t ao_log_last_done; + +uint8_t +ao_log_micro_scan(void); + +void +ao_log_micro_dump(void); + +#endif /* _AO_LOG_MICRO_H_ */ diff --git a/src/micropeak/ao_micropeak.c b/src/micropeak/ao_micropeak.c new file mode 100644 index 00000000..6ceec3b5 --- /dev/null +++ b/src/micropeak/ao_micropeak.c @@ -0,0 +1,207 @@ +/* + * 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. + */ + +#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; +alt_t ao_max_height; + +static void +ao_pa_get(void) +{ + ao_ms5607_sample(&sample); + ao_ms5607_convert(&sample, &value); + 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) + +static void +ao_compute_height(void) +{ + ground_alt = ao_pa_to_altitude(pa_ground); + max_alt = ao_pa_to_altitude(pa_min); + ao_max_height = max_alt - ground_alt; +} + +#if !HAS_EEPROM +void +ao_save_flight(void) +{ + ao_eeprom_write(0, &pa_ground, sizeof (pa_ground)); + ao_eeprom_write(sizeof (pa_ground), &pa_min, sizeof (pa_min)); +} + +void +ao_restore_flight(void) +{ + ao_eeprom_read(0, &pa_ground, sizeof (pa_ground)); + ao_eeprom_read(sizeof (pa_ground), &pa_min, sizeof (pa_min)); +} +#endif + +int +main(void) +{ + int16_t sample_count; + uint16_t time; +#if HAS_EEPROM + uint8_t dump_eeprom = 0; +#endif + 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(); + ao_report_altitude(); + + 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 + + /* 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; + for (;;) { + time += SAMPLE_SLEEP; + ao_delay_until(time); + if (sample_count == 0) + ao_led_on(AO_LED_BLUE); + ao_pa_get(); + if (sample_count == 0) + ao_led_off(AO_LED_BLUE); + pa_avg = pa_avg - (pa_avg >> FILTER_SHIFT) + pa; + pa_diff = pa_ground - pa_avg; + 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)) + break; + + if (sample_count < GROUND_AVG * 2) { + ao_led_off(AO_LED_BLUE); + 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; + +#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; + 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_BLUE); + ao_pa_get(); + if ((sample_count & 3) == 0) + ao_led_off(AO_LED_BLUE); +#if HAS_EEPROM + ao_log_micro_data(AO_LOG_MICRO_DATA | pa); +#endif + 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; + /* About 1m at sea level */ + if (pa_diff < (12 << 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; +#if HAS_EEPROM + ao_log_micro_data(AO_LOG_MICRO_DONE | pa_min); +#endif + ao_save_flight(); + ao_compute_height(); + ao_report_altitude(); + for (;;) { + cli(); + set_sleep_mode(SLEEP_MODE_PWR_DOWN); + sleep_mode(); + } +} diff --git a/src/micropeak/ao_morse.c b/src/micropeak/ao_morse.c new file mode 100644 index 00000000..9a7f88e5 --- /dev/null +++ b/src/micropeak/ao_morse.c @@ -0,0 +1,95 @@ +static void +pause(uint8_t j) +{ + int64_t i; + + while (j--) { + for (i = 0; i < 2000; i++) + ao_arch_nop(); + } +} + +#define BIT(i,x) ((x) ? (1 << (i)) : 0) +#define MORSE1(a) (1 | BIT(3,a)) +#define MORSE2(a,b) (2 | BIT(3,a) | BIT(4,b)) +#define MORSE3(a,b,c) (3 | BIT(3,a) | BIT(4,b) | BIT(5,c)) +#define MORSE4(a,b,c,d) (4 | BIT(3,a) | BIT(4,b) | BIT(5,c) | BIT(6,d)) +#define MORSE5(a,b,c,d,e) (5 | BIT(3,a) | BIT(4,b) | BIT(5,c) | BIT(6,d) | BIT(7,e)) + +#define ___ 1 +#define _ 0 + +static const uint8_t morse[26] = { + MORSE2(0,1), /* A */ + MORSE4(1,0,0,0), /* B */ + MORSE4(1,0,1,0), /* C */ + MORSE3(1,0,0), /* D */ + MORSE1(0), /* E */ + MORSE4(0,0,1,0), /* F */ + MORSE3(1,1,0), /* G */ + MORSE4(0,0,0,0), /* H */ + MORSE2(0,0), /* I */ + MORSE4(0,1,1,1), /* J */ + MORSE3(1,0,1), /* K */ + MORSE4(0,1,0,0), /* L */ + MORSE2(1,1), /* M */ + MORSE2(1,1), /* N */ + MORSE3(1,1,1), /* O */ + MORSE4(0,1,1,0), /* P */ + MORSE4(1,1,0,1), /* Q */ + MORSE3(0,1,0), /* R */ + MORSE3(0,0,0), /* S */ + MORSE1(1), /* T */ + MORSE3(0,0,1), /* U */ + MORSE4(0,0,0,1), /* V */ + MORSE3(0,1,1), /* W */ + MORSE4(1,0,0,1), /* X */ + MORSE4(1,0,1,1), /* Y */ + MORSE4(1,1,0,0), /* Z */ +}; + +static void +on(void) +{ + PORTB |= (1 << 4); +} + +static void +off(void) +{ + PORTB &= ~(1 << 4); +} + +static void +morse_char (char c) +{ + uint8_t r = morse[c - 'a']; + uint8_t l = r & 7; + + if (!r) + return; + while (l--) { + on(); + if (r & 8) + pause(3); + else + pause(1); + off(); + pause(1); + r >>= 1; + } + pause(2); +} + +static void +morse_string(char *s) { + char c; + + while ((c = *s++)) { + if (c == ' ') + pause(5); + else + morse_char(c); + } +} + diff --git a/src/micropeak/ao_notask.c b/src/micropeak/ao_notask.c new file mode 100644 index 00000000..0aef9cf3 --- /dev/null +++ b/src/micropeak/ao_notask.c @@ -0,0 +1,45 @@ +/* + * 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. + */ + +#include + +static volatile void *ao_wchan; + +uint8_t +ao_sleep(__xdata void *wchan) +{ +#if 1 + ao_wchan = wchan; + ao_arch_cpu_idle(); +#else + uint8_t sreg; + + ao_wchan = wchan; + asm("in %0,__SREG__" : "=&r" (sreg)); + sei(); + while (ao_wchan) + ao_arch_cpu_idle(); + asm("out __SREG__,%0" : : "r" (sreg)); +#endif + return 0; +} + +void +ao_wakeup(__xdata void *wchan) +{ + ao_wchan = 0; +} diff --git a/src/micropeak/ao_pins.h b/src/micropeak/ao_pins.h new file mode 100644 index 00000000..de9fc7f2 --- /dev/null +++ b/src/micropeak/ao_pins.h @@ -0,0 +1,60 @@ +/* + * Copyright © 2011 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_PINS_H_ +#define _AO_PINS_H_ +#include + +#define AO_LED_BLUE (1<<4) +#define AO_LED_SERIAL 4 +#define AO_LED_PANIC AO_LED_BLUE +#define LEDS_AVAILABLE (AO_LED_BLUE) +#define USE_SERIAL_1_STDIN 0 +#define HAS_USB 0 +#define PACKET_HAS_SLAVE 0 +#define HAS_SERIAL_1 0 +#define HAS_TASK 0 +#define HAS_MS5607 1 +#define HAS_MS5611 1 +#define HAS_EEPROM 0 +#define HAS_BEEP 0 +#define AVR_CLOCK 8000000UL + +/* SPI */ +#define SPI_PORT PORTB +#define SPI_PIN PINB +#define SPI_DIR DDRB +#define AO_MS5607_CS_PORT PORTB +#define AO_MS5607_CS_PIN 3 + +#define AO_MS5607_SPI_INDEX 0 +#define AO_MS5607_MISO_PORT PORTB +#define AO_MS5607_MISO_PIN 0 + +/* I2C */ +#define I2C_PORT PORTB +#define I2C_PIN PINB +#define I2C_DIR DDRB +#define I2C_PIN_SCL PINB2 +#define I2C_PIN_SDA PINB0 + +#define AO_CONST_ATTRIB PROGMEM +#define FETCH_ALT(o) ((alt_t) pgm_read_dword(&altitude_table[o])) + +#define AO_ALT_VALUE(x) ((x) * 10) + +#endif /* _AO_PINS_H_ */ diff --git a/src/micropeak/ao_report_tiny.c b/src/micropeak/ao_report_tiny.c new file mode 100644 index 00000000..5937508b --- /dev/null +++ b/src/micropeak/ao_report_tiny.c @@ -0,0 +1,57 @@ +/* + * 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. + */ + +#include + +#define mid(time) ao_led_for(AO_LED_BLUE, time) +#define pause(time) ao_delay(time) + +static void +ao_report_digit(uint8_t digit) __reentrant +{ + if (!digit) { + mid(AO_MS_TO_TICKS(600)); + pause(AO_MS_TO_TICKS(200)); + } else { + while (digit--) { + mid(AO_MS_TO_TICKS(200)); + pause(AO_MS_TO_TICKS(200)); + } + } + pause(AO_MS_TO_TICKS(300)); +} + +void +ao_report_altitude(void) +{ + __pdata int16_t agl = ao_max_height; + __xdata uint8_t digits[10]; + __pdata uint8_t ndigits, i; + + if (agl < 0) + agl = 0; + ndigits = 0; + do { + digits[ndigits++] = agl % 10; + agl /= 10; + } while (agl); + + i = ndigits; + do + ao_report_digit(digits[--i]); + while (i != 0); +} -- 2.30.2