From: Keith Packard Date: Sun, 15 Sep 2013 21:11:50 +0000 (-0700) Subject: altos: Move micropeak sources around X-Git-Tag: 1.2.9.4~71 X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=commitdiff_plain;h=690094e2d7d9cfe5eb4edb478fd79e5d133c6b4b;ds=sidebyside altos: Move micropeak sources around This sticks the micropeak sources in appropriate directories, rather than in the micropeak product directory so that they can be shared with future micropeak-style products. Signed-off-by: Keith Packard --- diff --git a/src/attiny/ao_async.c b/src/attiny/ao_async.c new file mode 100644 index 00000000..3556f54c --- /dev/null +++ b/src/attiny/ao_async.c @@ -0,0 +1,71 @@ +/* + * 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) + +#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 data stop */ + w = (0x000 << 0) | (byte << 1) | (0x001 << 9); + + ao_arch_block_interrupts(); + for (b = 0; b < 10; b++) { + uint8_t v = LED_PORT & ~(1 << AO_LED_SERIAL); + v |= (w & 1) << AO_LED_SERIAL; + LED_PORT = v; + w >>= 1; + + /* 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/attiny/ao_async.h b/src/attiny/ao_async.h new file mode 100644 index 00000000..1b239712 --- /dev/null +++ b/src/attiny/ao_async.h @@ -0,0 +1,30 @@ +/* + * 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_start(void); + +void +ao_async_stop(void); + +void +ao_async_byte(uint8_t byte); + +#endif /* _AO_ASYNC_H_ */ diff --git a/src/core/ao_log_micro.c b/src/core/ao_log_micro.c new file mode 100644 index 00000000..d665efb5 --- /dev/null +++ b/src/core/ao_log_micro.c @@ -0,0 +1,121 @@ +/* + * 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 +#include + +static uint16_t ao_log_offset = STARTING_LOG_OFFSET; + +void +ao_log_micro_save(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_log_micro_restore(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_data(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); + } +} + +#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) +{ + uint16_t n_samples; + 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_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/core/ao_log_micro.h b/src/core/ao_log_micro.h new file mode 100644 index 00000000..976852ee --- /dev/null +++ b/src/core/ao_log_micro.h @@ -0,0 +1,39 @@ +/* + * 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 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_save(void); + +void +ao_log_micro_restore(void); + +void +ao_log_micro_data(void); + +void +ao_log_micro_dump(void); + +#endif /* _AO_LOG_MICRO_H_ */ diff --git a/src/core/ao_microflight.c b/src/core/ao_microflight.c new file mode 100644 index 00000000..714bb90a --- /dev/null +++ b/src/core/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/core/ao_microkalman.c b/src/core/ao_microkalman.c new file mode 100644 index 00000000..0684ea2b --- /dev/null +++ b/src/core/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/core/ao_report_micro.c b/src/core/ao_report_micro.c new file mode 100644 index 00000000..0e8e287f --- /dev/null +++ b/src/core/ao_report_micro.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_REPORT, time) +#define pause(time) ao_delay(time) + +static void +ao_report_digit(uint8_t digit) __reentrant +{ + if (!digit) { + mid(AO_MS_TO_TICKS(1000)); + pause(AO_MS_TO_TICKS(300)); + } else { + while (digit--) { + mid(AO_MS_TO_TICKS(300)); + pause(AO_MS_TO_TICKS(300)); + } + } + pause(AO_MS_TO_TICKS(1000)); +} + +void +ao_report_altitude(void) +{ + __pdata alt_t agl = ao_max_height; + static __xdata uint8_t digits[11]; + __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); +} diff --git a/src/micropeak/Makefile b/src/micropeak/Makefile index 44e0b873..e51b2847 100644 --- a/src/micropeak/Makefile +++ b/src/micropeak/Makefile @@ -2,7 +2,7 @@ # Tiny AltOS build # # -vpath % ../attiny:../drivers:../core:.. +vpath % ../attiny:../drivers:../core:../product:.. vpath ao-make-product.5c ../util vpath make-altitude-pa ../util @@ -30,7 +30,7 @@ ALTOS_SRC = \ ao_ms5607.c \ ao_exti.c \ ao_convert_pa.c \ - ao_report_tiny.c \ + ao_report_micro.c \ ao_notask.c \ ao_eeprom_tiny.c \ ao_panic.c \ @@ -53,7 +53,7 @@ INC=\ IDPRODUCT=0 PRODUCT=MicroPeak-v0.1 PRODUCT_DEF=-DMICROPEAK -CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../core -I.. -I../drivers +CFLAGS = $(PRODUCT_DEF) -I. -I../attiny -I../core -I.. -I../drivers -I../product 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 deleted file mode 100644 index 3556f54c..00000000 --- a/src/micropeak/ao_async.c +++ /dev/null @@ -1,71 +0,0 @@ -/* - * 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) - -#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 data stop */ - w = (0x000 << 0) | (byte << 1) | (0x001 << 9); - - ao_arch_block_interrupts(); - for (b = 0; b < 10; b++) { - uint8_t v = LED_PORT & ~(1 << AO_LED_SERIAL); - v |= (w & 1) << AO_LED_SERIAL; - LED_PORT = v; - w >>= 1; - - /* 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 deleted file mode 100644 index 1b239712..00000000 --- a/src/micropeak/ao_async.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * 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_start(void); - -void -ao_async_stop(void); - -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 deleted file mode 100644 index d665efb5..00000000 --- a/src/micropeak/ao_log_micro.c +++ /dev/null @@ -1,121 +0,0 @@ -/* - * 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 -#include - -static uint16_t ao_log_offset = STARTING_LOG_OFFSET; - -void -ao_log_micro_save(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_log_micro_restore(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_data(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); - } -} - -#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) -{ - uint16_t n_samples; - 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_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_log_micro.h b/src/micropeak/ao_log_micro.h deleted file mode 100644 index 976852ee..00000000 --- a/src/micropeak/ao_log_micro.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - * 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 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_save(void); - -void -ao_log_micro_restore(void); - -void -ao_log_micro_data(void); - -void -ao_log_micro_dump(void); - -#endif /* _AO_LOG_MICRO_H_ */ diff --git a/src/micropeak/ao_microflight.c b/src/micropeak/ao_microflight.c deleted file mode 100644 index 714bb90a..00000000 --- a/src/micropeak/ao_microflight.c +++ /dev/null @@ -1,143 +0,0 @@ -/* - * 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 deleted file mode 100644 index 0684ea2b..00000000 --- a/src/micropeak/ao_microkalman.c +++ /dev/null @@ -1,74 +0,0 @@ -/* - * 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 deleted file mode 100644 index 10f0d192..00000000 --- a/src/micropeak/ao_micropeak.c +++ /dev/null @@ -1,89 +0,0 @@ -/* - * 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 -#include -#include - -static struct ao_ms5607_sample sample; -static struct ao_ms5607_value value; - -alt_t ground_alt, max_alt; -alt_t ao_max_height; - -void -ao_pa_get(void) -{ - ao_ms5607_sample(&sample); - ao_ms5607_convert(&sample, &value); - pa = value.pres; -} - -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; -} - -static void -ao_pips(void) -{ - uint8_t i; - for (i = 0; i < 10; i++) { - ao_led_toggle(AO_LED_REPORT); - ao_delay(AO_MS_TO_TICKS(80)); - } - ao_delay(AO_MS_TO_TICKS(200)); -} - -int -main(void) -{ - ao_led_init(LEDS_AVAILABLE); - ao_timer_init(); - - /* Init external hardware */ - ao_spi_init(); - ao_ms5607_init(); - ao_ms5607_setup(); - - /* 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_pips(); - ao_log_micro_dump(); - - ao_delay(BOOST_DELAY); - - ao_microflight(); - - ao_log_micro_save(); - ao_compute_height(); - ao_report_altitude(); - for (;;) { - cli(); - set_sleep_mode(SLEEP_MODE_PWR_DOWN); - sleep_mode(); - } -} diff --git a/src/micropeak/ao_micropeak.h b/src/micropeak/ao_micropeak.h deleted file mode 100644 index 382b98d9..00000000 --- a/src/micropeak/ao_micropeak.h +++ /dev/null @@ -1,78 +0,0 @@ -/* - * 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 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 24 /* 2m at sea level, 2.4m at 2000m */ - -/* Current sensor pressure value */ -extern uint32_t pa; - -/* 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; - -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/micropeak/ao_report_tiny.c b/src/micropeak/ao_report_tiny.c deleted file mode 100644 index 0e8e287f..00000000 --- a/src/micropeak/ao_report_tiny.c +++ /dev/null @@ -1,57 +0,0 @@ -/* - * 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_REPORT, time) -#define pause(time) ao_delay(time) - -static void -ao_report_digit(uint8_t digit) __reentrant -{ - if (!digit) { - mid(AO_MS_TO_TICKS(1000)); - pause(AO_MS_TO_TICKS(300)); - } else { - while (digit--) { - mid(AO_MS_TO_TICKS(300)); - pause(AO_MS_TO_TICKS(300)); - } - } - pause(AO_MS_TO_TICKS(1000)); -} - -void -ao_report_altitude(void) -{ - __pdata alt_t agl = ao_max_height; - static __xdata uint8_t digits[11]; - __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); -} diff --git a/src/product/ao_micropeak.c b/src/product/ao_micropeak.c new file mode 100644 index 00000000..10f0d192 --- /dev/null +++ b/src/product/ao_micropeak.c @@ -0,0 +1,89 @@ +/* + * 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 +#include +#include + +static struct ao_ms5607_sample sample; +static struct ao_ms5607_value value; + +alt_t ground_alt, max_alt; +alt_t ao_max_height; + +void +ao_pa_get(void) +{ + ao_ms5607_sample(&sample); + ao_ms5607_convert(&sample, &value); + pa = value.pres; +} + +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; +} + +static void +ao_pips(void) +{ + uint8_t i; + for (i = 0; i < 10; i++) { + ao_led_toggle(AO_LED_REPORT); + ao_delay(AO_MS_TO_TICKS(80)); + } + ao_delay(AO_MS_TO_TICKS(200)); +} + +int +main(void) +{ + ao_led_init(LEDS_AVAILABLE); + ao_timer_init(); + + /* Init external hardware */ + ao_spi_init(); + ao_ms5607_init(); + ao_ms5607_setup(); + + /* 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_pips(); + ao_log_micro_dump(); + + ao_delay(BOOST_DELAY); + + ao_microflight(); + + ao_log_micro_save(); + ao_compute_height(); + ao_report_altitude(); + for (;;) { + cli(); + set_sleep_mode(SLEEP_MODE_PWR_DOWN); + sleep_mode(); + } +} diff --git a/src/product/ao_micropeak.h b/src/product/ao_micropeak.h new file mode 100644 index 00000000..3e3dec15 --- /dev/null +++ b/src/product/ao_micropeak.h @@ -0,0 +1,78 @@ +/* + * 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 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(60) + +/* Pressure change (in Pa) to detect landing */ +#define LAND_DETECT 24 /* 2m at sea level, 2.4m at 2000m */ + +/* Current sensor pressure value */ +extern uint32_t pa; + +/* 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; + +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 +