diff options
Diffstat (limited to 'src')
253 files changed, 14463 insertions, 1653 deletions
diff --git a/src/.gitignore b/src/.gitignore index cae36ae6..a8628fae 100644 --- a/src/.gitignore +++ b/src/.gitignore @@ -1,3 +1,4 @@ +Makedefs altitude.h altitude-pa.h ao_whiten.h diff --git a/src/Makedefs.in b/src/Makedefs.in new file mode 100644 index 00000000..a52f96fa --- /dev/null +++ b/src/Makedefs.in @@ -0,0 +1,15 @@ +ARM_CC=@ARM_CC@ +HAVE_ARM_M3_CC=@HAVE_ARM_M3_CC@ +HAVE_ARM_M0_CC=@HAVE_ARM_M0_CC@ +PDCLIB_INCLUDES=@PDCLIB_INCLUDES@ +PDCLIB_LIBS_M0=@PDCLIB_LIBS_M0@ +PDCLIB_LIBS_M3=@PDCLIB_LIBS_M3@ +PDCLIB_ROOT=@PDCLIB_ROOT@ +HAVE_PDCLIB=@HAVE_PDCLIB@ + +SDCC=@SDCC@ +HAVE_SDCC=@HAVE_SDCC@ + +AVR_CC=@AVR_CC@ +AVR_OBJCOPY=@AVR_OBJCOPY@ +HAVE_AVR_CC=@HAVE_AVR_CC@ diff --git a/src/Makefile b/src/Makefile index 11f149dd..392262d4 100644 --- a/src/Makefile +++ b/src/Makefile @@ -13,43 +13,59 @@ vpath load_csv.5c kalman vpath matrix.5c kalman include Version +TOPDIR=. +include Makedefs SDCCDIRS=\ telemetrum-v1.2 telemetrum-v1.1 telemetrum-v1.0 \ - teledongle-v0.2 teledongle-v0.1 \ - telemini-v1.0 telenano-v0.1 \ + teledongle-v0.2 \ + telemini-v1.0 \ telebt-v1.0 \ - telemetrum-v0.1-sky telemetrum-v0.1-sirf \ - telelaunch-v0.1 tidongle test \ teleterra-v0.2 teleshield-v0.1 \ - telefire-v0.1 \ - spiradio-v0.1 + telefire-v0.1 telefire-v0.2 \ + telemini-v2.0 -AVRDIRS=\ - telescience-v0.1 telescience-pwm telepyro-v0.1 micropeak - -ARMDIRS=\ +ARMM3DIRS=\ telemega-v0.1 telemega-v0.1/flash-loader \ - telemega-v0.3 telemega-v0.3/flash-loader \ + telemega-v1.0 telemega-v1.0/flash-loader \ + telemetrum-v2.0 telemetrum-v2.0/flash-loader \ megadongle-v0.1 megadongle-v0.1/flash-loader \ - telegps-v0.1 telegps-v0.1/flash-loader \ - stm-bringup stm-demo \ + telegps-v0.3 telegps-v0.3/flash-loader \ telelco-v0.2 telelco-v0.2/flash-loader \ - telescience-v0.2 + telescience-v0.2 telescience-v0.2/flash-loader + +ARMM0DIRS=\ + easymini-v1.0 easymini-v1.0/flash-loader + +AVRDIRS=\ + telescience-v0.1 telescience-pwm micropeak nanopeak-v0.1 + +SUBDIRS= -ifneq ($(shell which sdcc),) - SUBDIRS += $(SDCCDIRS) +ifeq ($(strip $(HAVE_PDCLIB)),yes) +PDCLIB=pdclib +CLEAN_PDCLIB=clean-pdclib endif -ifneq ($(shell which avr-gcc),) - SUBDIRS += $(AVRDIRS) +ifeq ($(strip $(HAVE_SDCC)),yes) +SUBDIRS+=$(SDCCDIRS) endif -ifneq ($(shell which arm-none-eabi-gcc),) - SUBDIRS += $(ARMDIRS) +ifeq ($(strip $(HAVE_ARM_M3_CC)),yes) +SUBDIRS+=$(ARMM3DIRS) +foo=bar endif -ALLDIRS=$(SDCCDIRS) $(AVRDIRS) $(ARMDIRS) +ifeq ($(strip $(HAVE_ARM_M0_CC)),yes) +SUBDIRS+=$(ARMM0DIRS) +baz=bletch +endif + +ifeq ($(strip $(HAVE_AVR_CC)),yes) +SUBDIRS += $(AVRDIRS) +endif + +ALLDIRS=$(SDCCDIRS) $(ARMM3DIRS) $(ARMM0DIRS) $(AVRDIRS) all: all-local all-recursive @@ -81,7 +97,7 @@ uninstall: all-recursive: all-local -all-local: altitude.h altitude-pa.h ao_kalman.h ao_whiten.h +all-local: altitude.h altitude-pa.h ao_kalman.h ao_whiten.h $(PDCLIB) altitude.h: make-altitude nickle $< > $@ @@ -95,5 +111,13 @@ ao_kalman.h: make-kalman kalman.5c kalman_filter.5c load_csv.5c matrix.5c ao_whiten.h: make-whiten nickle $< > $@ -clean-local: +clean-local: $(CLEAN_PDCLIB) rm -f altitude.h ao_kalman.h + +pdclib: + mkdir -p $(PDCLIB_ROOT)/include $(PDCLIB_ROOT)/lib + cd ../pdclib && make && make prefix=`pwd`/../pdclib-root install + +clean-pdclib: + rm -rf $(PDCLIB_ROOT) + cd ../pdclib && make clean diff --git a/src/aes/ao_aes.c b/src/aes/ao_aes.c index 71ffadb1..52463f5d 100644 --- a/src/aes/ao_aes.c +++ b/src/aes/ao_aes.c @@ -310,6 +310,10 @@ void xrijndaelEncrypt(word32 block[], roundkey *rkk) xKeyAddition(block, block2, rp, BC); } +#if NOTUSED +/* We don't actually need this in AltOS, so don't bother including it */ + +/* Decryption of one block. */ static void xrijndaelDecrypt(word32 block[], roundkey *rkk) { @@ -352,6 +356,7 @@ void xrijndaelDecrypt(word32 block[], roundkey *rkk) xKeyAddition(block, block, rp, BC); } +#endif uint8_t ao_aes_mutex; static uint8_t key[16]; diff --git a/src/attiny/ao_arch_funcs.h b/src/attiny/ao_arch_funcs.h index 8c9d1ae6..d4584a9f 100644 --- a/src/attiny/ao_arch_funcs.h +++ b/src/attiny/ao_arch_funcs.h @@ -20,21 +20,16 @@ */ #define ao_spi_get_mask(reg,mask,bus,speed) do { \ - (reg) &= ~(mask); \ + (reg) &= ~(mask); \ } while (0) #define ao_spi_put_mask(reg,mask,bus) do { \ (reg) |= (mask); \ } while (0) -#define ao_spi_get_bit(reg,bit,pin,bus,speed) do { \ - (pin) = 0; \ - } while (0) - -#define ao_spi_put_bit(reg,bit,pin,bus) do { \ - (pin) = 1; \ - } while (0) +#define ao_spi_get_bit(reg,bit,pin,bus,speed) ao_spi_get_mask(reg,(1<<(bit)),bus,speed) +#define ao_spi_put_bit(reg,bit,pin,bus) ao_spi_put_mask(reg,(1<<(bit)),bus) #define ao_gpio_token_paster(x,y) x ## y #define ao_gpio_token_evaluator(x,y) ao_gpio_token_paster(x,y) @@ -46,6 +41,8 @@ PORTB &= ~(1 << bit); \ } while (0) +#define ao_gpio_get(port, bit, pin) ((PORTB >> (bit)) & 1) + /* * The SPI mutex must be held to call either of these * functions -- this mutex covers the entire SPI operation, diff --git a/src/micropeak/ao_async.c b/src/attiny/ao_async.c index 3556f54c..3556f54c 100644 --- a/src/micropeak/ao_async.c +++ b/src/attiny/ao_async.c diff --git a/src/micropeak/ao_async.h b/src/attiny/ao_async.h index 1b239712..1b239712 100644 --- a/src/micropeak/ao_async.h +++ b/src/attiny/ao_async.h diff --git a/src/attiny/ao_exti.h b/src/attiny/ao_exti.h index 2ea4f47d..85bb2fba 100644 --- a/src/attiny/ao_exti.h +++ b/src/attiny/ao_exti.h @@ -30,5 +30,6 @@ ao_exti_setup_port(uint8_t pin, uint8_t mode, void (*callback)(void)); #define ao_exti_init() #define AO_EXTI_MODE_RISING 1 +#define AO_EXTI_PIN_NOCONFIGURE 0 #endif /* _AO_EXTI_H_ */ diff --git a/src/avr-demo/Makefile b/src/avr-demo/Makefile index 93295166..6d9bfea2 100644 --- a/src/avr-demo/Makefile +++ b/src/avr-demo/Makefile @@ -11,18 +11,12 @@ vpath load_csv.5c ../kalman vpath matrix.5c ../kalman vpath ao-make-product.5c ../util +include ../avr/Makefile.defs + MCU=atmega32u4 DUDECPUTYPE=m32u4 #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 INC = \ ao.h \ diff --git a/src/avr/Makefile.defs b/src/avr/Makefile.defs new file mode 100644 index 00000000..eeb9a881 --- /dev/null +++ b/src/avr/Makefile.defs @@ -0,0 +1,16 @@ +ifndef TOPDIR +TOPDIR=.. +endif + +ifndef VERSION +include $(TOPDIR)/Version +endif + +include $(TOPDIR)/Makedefs + +CC=$(AVR_CC) +OBJCOPY=$(AVR_OBJCOPY) +LDSCRIPTS=/usr/lib/avr/lib/ldscripts + +PROGRAMMER=usbtiny +LOADCMD=avrdude diff --git a/src/cc1111/Makefile.cc1111 b/src/cc1111/Makefile.cc1111 index 0e19603b..78b653b3 100644 --- a/src/cc1111/Makefile.cc1111 +++ b/src/cc1111/Makefile.cc1111 @@ -1,4 +1,5 @@ -CC=sdcc +include ../Makedefs +CC=$(SDCC) CFLAGS=--model-small --debug --opt-code-speed -DCODESIZE=$(CODESIZE) diff --git a/src/cc1111/ao_adc.c b/src/cc1111/ao_adc.c index ed76179b..1689ebef 100644 --- a/src/cc1111/ao_adc.c +++ b/src/cc1111/ao_adc.c @@ -19,23 +19,26 @@ volatile __xdata struct ao_data ao_data_ring[AO_DATA_RING]; volatile __data uint8_t ao_data_head; +#if (AO_DATA_ALL & ~(AO_DATA_ADC)) +volatile __data uint8_t ao_data_present; +#endif + +#ifdef TELENANO_V_0_1 +# define AO_ADC_FIRST_PIN 1 +#endif + +#if HAS_ACCEL_REF +# define AO_ADC_FIRST_PIN 2 +#endif #ifndef AO_ADC_FIRST_PIN -#define AO_ADC_FIRST_PIN 0 +# define AO_ADC_FIRST_PIN 0 #endif void ao_adc_poll(void) { -#if HAS_ACCEL_REF - ADCCON3 = ADCCON3_EREF_VDD | ADCCON3_EDIV_512 | 2; -#else -# ifdef TELENANO_V_0_1 - ADCCON3 = ADCCON3_EREF_VDD | ADCCON3_EDIV_512 | 1; -# else ADCCON3 = ADCCON3_EREF_VDD | ADCCON3_EDIV_512 | AO_ADC_FIRST_PIN; -# endif -#endif } void @@ -82,6 +85,7 @@ ao_adc_isr(void) __interrupt 1 else #endif ADCCON3 = ADCCON3_EREF_VDD | ADCCON3_EDIV_512 | sequence; + return; } #endif @@ -141,15 +145,18 @@ ao_adc_isr(void) __interrupt 1 if (sequence) { /* Start next conversion */ ADCCON3 = sequence; + return; } #endif /* telemini || telenano */ -#ifdef TELEFIRE_V_0_1 +#if defined(TELEFIRE_V_0_1) || defined(TELEFIRE_V_0_2) a = (uint8_t __xdata *) (&ao_data_ring[ao_data_head].adc.sense[0] + sequence - AO_ADC_FIRST_PIN); a[0] = ADCL; a[1] = ADCH; - if (sequence < 5) + if (sequence < 5) { ADCCON3 = ADCCON3_EREF_VDD | ADCCON3_EDIV_512 | (sequence + 1); + return; + } #define GOT_ADC #endif /* TELEFIRE_V_0_1 */ @@ -157,21 +164,22 @@ ao_adc_isr(void) __interrupt 1 a = (uint8_t __xdata *) (&ao_data_ring[ao_data_head].adc.batt); a[0] = ADCL; a[1] = ADCH; - if (0) - ; #define GOT_ADC #endif +#ifdef FETCH_ADC + FETCH_ADC(); +#define GOT_ADC +#endif + #ifndef GOT_ADC #error No known ADC configuration set #endif - else { - /* record this conversion series */ - ao_data_ring[ao_data_head].tick = ao_time(); - ao_data_head = ao_data_ring_next(ao_data_head); - ao_wakeup(DATA_TO_XDATA(&ao_data_head)); - } + /* record this conversion series */ + ao_data_ring[ao_data_head].tick = ao_time(); + ao_data_head = ao_data_ring_next(ao_data_head); + ao_wakeup(DATA_TO_XDATA(&ao_data_head)); } static void diff --git a/src/cc1111/ao_arch.h b/src/cc1111/ao_arch.h index 9097557f..34235b08 100644 --- a/src/cc1111/ao_arch.h +++ b/src/cc1111/ao_arch.h @@ -321,4 +321,9 @@ void ao_serial1_tx_isr(void) ao_arch_interrupt(14); #endif +#if HAS_EXTI_0 +void +ao_p0_isr(void) __interrupt(13); +#endif + #endif /* _AO_ARCH_H_ */ diff --git a/src/cc1111/ao_arch_funcs.h b/src/cc1111/ao_arch_funcs.h index 8f1cc094..ea340dfd 100644 --- a/src/cc1111/ao_arch_funcs.h +++ b/src/cc1111/ao_arch_funcs.h @@ -19,46 +19,74 @@ * ao_spi.c */ -extern __xdata uint8_t ao_spi_mutex; +#if !HAS_SPI_0 && !HAS_SPI_1 +#define HAS_SPI_0 1 +#define SPI_0_ALT_2 1 +#endif + +#if HAS_SPI_0 && HAS_SPI_1 +#define MULTI_SPI 1 +#define N_SPI 2 +#else +#define MULTI_SPI 0 +#define N_SPI 1 +#endif + +extern __xdata uint8_t ao_spi_mutex[N_SPI]; + +#if MULTI_SPI +#define ao_spi_get(bus) ao_mutex_get(&ao_spi_mutex[bus]) +#define ao_spi_put(bus) ao_mutex_put(&ao_spi_mutex[bus]) +#else +#define ao_spi_get(bus) ao_mutex_get(&ao_spi_mutex[0]) +#define ao_spi_put(bus) ao_mutex_put(&ao_spi_mutex[0]) +#endif #define AO_SPI_SPEED_FAST 17 #define AO_SPI_SPEED_200kHz 13 -#define ao_spi_set_speed(speed) (U0GCR = (UxGCR_CPOL_NEGATIVE | \ - UxGCR_CPHA_FIRST_EDGE | \ - UxGCR_ORDER_MSB | \ - ((speed) << UxGCR_BAUD_E_SHIFT))) +#if MULTI_SPI +#define ao_spi_set_speed(bus,speed) (*(bus ? &U1GCR : &U0GCR) =(UxGCR_CPOL_NEGATIVE | \ + UxGCR_CPHA_FIRST_EDGE | \ + UxGCR_ORDER_MSB | \ + ((speed) << UxGCR_BAUD_E_SHIFT))) +#else +#define ao_spi_set_speed(bus,speed) (U0GCR = (UxGCR_CPOL_NEGATIVE | \ + UxGCR_CPHA_FIRST_EDGE | \ + UxGCR_ORDER_MSB | \ + ((speed) << UxGCR_BAUD_E_SHIFT))) +#endif #define ao_spi_get_slave(bus) do { \ - ao_mutex_get(&ao_spi_mutex); \ - ao_spi_set_speed(AO_SPI_SPEED_FAST); \ + ao_spi_get(bus); \ + ao_spi_set_speed(bus,AO_SPI_SPEED_FAST); \ } while (0) #define ao_spi_put_slave(bus) do { \ - ao_mutex_put(&ao_spi_mutex); \ + ao_spi_put(bus); \ } while (0) #define ao_spi_get_mask(reg,mask,bus,speed) do { \ - ao_mutex_get(&ao_spi_mutex); \ - ao_spi_set_speed(speed); \ + ao_spi_get(bus); \ + ao_spi_set_speed(bus,speed); \ (reg) &= ~(mask); \ } while (0) #define ao_spi_put_mask(reg,mask,bus) do { \ (reg) |= (mask); \ - ao_mutex_put(&ao_spi_mutex); \ + ao_spi_put(bus); \ } while (0) #define ao_spi_get_bit(reg,bit,pin,bus,speed) do { \ - ao_mutex_get(&ao_spi_mutex); \ - ao_spi_set_speed(speed); \ - pin = 0; \ + ao_spi_get(bus); \ + ao_spi_set_speed(bus,speed); \ + pin = 0; \ } while (0) #define ao_spi_put_bit(reg,bit,pin,bus) do { \ pin = 1; \ - ao_mutex_put(&ao_spi_mutex); \ + ao_spi_put(bus); \ } while (0) @@ -68,6 +96,13 @@ extern __xdata uint8_t ao_spi_mutex; * from chip select low to chip select high */ +#if MULTI_SPI +void +ao_spi_send(void __xdata *block, uint16_t len, uint8_t bus) __reentrant; + +void +ao_spi_recv(void __xdata *block, uint16_t len, uint8_t bus) __reentrant; +#else void ao_spi_send_bus(void __xdata *block, uint16_t len) __reentrant; @@ -76,6 +111,7 @@ ao_spi_recv_bus(void __xdata *block, uint16_t len) __reentrant; #define ao_spi_send(block, len, bus) ao_spi_send_bus(block, len) #define ao_spi_recv(block, len, bus) ao_spi_recv_bus(block, len) +#endif #if AO_SPI_SLAVE void @@ -88,10 +124,15 @@ ao_spi_recv_wait(void); void ao_spi_init(void); -#define ao_spi_init_cs(port, mask) do { \ - SPI_CS_PORT |= mask; \ - SPI_CS_DIR |= mask; \ - SPI_CS_SEL &= ~mask; \ +#define token_paster(x,y) x ## y +#define token_paster3(x,y,z) x ## y ## z +#define token_evaluator(x,y) token_paster(x,y) +#define token_evaluator3(x,y,z) token_paster3(x,y,z) + +#define ao_spi_init_cs(port, mask) do { \ + port |= mask; \ + token_evaluator(port,DIR) |= mask; \ + token_evaluator(port,SEL) &= ~mask; \ } while (0) #define cc1111_enable_output(port,dir,sel,pin,bit,v) do { \ @@ -102,7 +143,7 @@ ao_spi_init(void); #define disable_unreachable _Pragma("disable_warning 126") -#define token_paster(x,y) x ## y -#define token_evaluator(x,y) token_paster(x,y) #define ao_enable_output(port,bit,pin,v) cc1111_enable_output(port,token_evaluator(port,DIR), token_evaluator(port,SEL), pin, bit, v) #define ao_gpio_set(port, bit, pin, v) ((pin) = (v)) +#define ao_gpio_get(port, bit, pin) (pin) + diff --git a/src/cc1111/ao_exti.c b/src/cc1111/ao_exti.c new file mode 100644 index 00000000..537f6252 --- /dev/null +++ b/src/cc1111/ao_exti.c @@ -0,0 +1,33 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_exti.h> + +#if HAS_EXTI_0 +__xdata void (*ao_int_callback)(void); + +void +ao_p0_isr(void) __interrupt(13) +{ + if (P0IF && (P0IFG & (AO_MS5607_MISO_MASK))) { + (*ao_int_callback)(); + } + P0IFG = 0; + P0IF = 0; +} +#endif diff --git a/src/cc1111/ao_exti.h b/src/cc1111/ao_exti.h new file mode 100644 index 00000000..49fca5d2 --- /dev/null +++ b/src/cc1111/ao_exti.h @@ -0,0 +1,56 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_EXTI_H_ +#define _AO_EXTI_H_ + +#define AO_EXTI_MODE_RISING 1 +#define AO_EXTI_MODE_FALLING 2 +#define AO_EXTI_MODE_PULL_UP 4 +#define AO_EXTI_MODE_PULL_DOWN 8 +#define AO_EXTI_PRIORITY_LOW 16 +#define AO_EXTI_PRIORITY_MED 0 +#define AO_EXTI_PRIORITY_HIGH 32 + +extern void (*ao_int_callback)(void); + +#define ao_exti_setup(gpio, pin, mode, callback) do { \ + ao_int_callback = callback; \ + } while (0) + +#define ao_exti_set_mode(gpio, pin, mode) do { \ + } while (0) + +#define ao_exti_set_callback(port, pin, callback) do { \ + ao_int_callback = callback; \ + } while (0) + +#define ao_exti_init() + +#define ao_exti_enable(port, pin) do { \ + P0IFG &= ~(1 << pin); \ + P0IF = 0; \ + PICTL |= PICTL_P0IENL; \ + IEN1 |= IEN1_P0IE; \ + } while (0) + +#define ao_exti_disable(port, pin) do { \ + IEN1 &= ~IEN1_P0IE; \ + PICTL &= ~PICTL_P0IENL; \ + } while (0) + +#endif /* _AO_EXTI_H_ */ diff --git a/src/cc1111/ao_radio.c b/src/cc1111/ao_radio.c index 88b8f686..190647ce 100644 --- a/src/cc1111/ao_radio.c +++ b/src/cc1111/ao_radio.c @@ -374,7 +374,7 @@ ao_radio_recv(__xdata void *packet, uint8_t size, uint8_t timeout) __reentrant } #if NEED_RADIO_RSSI else - ao_radio_rssi = AO_RSSI_FROM_RADIO(((uint8_t *)packet)[size - 1]); + ao_radio_rssi = AO_RSSI_FROM_RADIO(((uint8_t *)packet)[size - 2]); #endif ao_radio_put(); return ao_radio_dma_done; diff --git a/src/cc1111/ao_spi.c b/src/cc1111/ao_spi.c index cdef6bda..fb08f3f5 100644 --- a/src/cc1111/ao_spi.c +++ b/src/cc1111/ao_spi.c @@ -18,10 +18,6 @@ #include "ao.h" /* Default pin usage for existing Altus Metrum devices */ -#if !HAS_SPI_0 && !HAS_SPI_1 -#define HAS_SPI_0 1 -#define SPI_0_ALT_2 1 -#endif #ifndef SPI_CONST #define SPI_CONST 0xff @@ -61,62 +57,107 @@ */ #if HAS_SPI_0 -#define SPI_CSR U0CSR -#define SPI_BUF U0DBUFXADDR -#define SPI_BAUD U0BAUD -#define SPI_GCR U0GCR -#define SPI_CFG_MASK PERCFG_U0CFG_ALT_MASK -#define SPI_DMA_TX DMA_CFG0_TRIGGER_UTX0 -#define SPI_DMA_RX DMA_CFG0_TRIGGER_URX0 +#define SPI_BUF_0 &U0DBUFXADDR +#define SPI_CSR_0 U0CSR +#define SPI_BAUD_0 U0BAUD +#define SPI_GCR_0 U0GCR +#define SPI_CFG_MASK_0 PERCFG_U0CFG_ALT_MASK +#define SPI_DMA_TX_0 DMA_CFG0_TRIGGER_UTX0 +#define SPI_DMA_RX_0 DMA_CFG0_TRIGGER_URX0 #if SPI_0_ALT_1 -#define SPI_CFG PERCFG_U0CFG_ALT_1 -#define SPI_SEL P0SEL -#define SPI_BITS (1 << 3) | (1 << 2) | (1 << 5) -#define SPI_CSS_BIT (1 << 4) +#define SPI_CFG_0 PERCFG_U0CFG_ALT_1 +#define SPI_SEL_0 P0SEL +#define SPI_BITS_0 (1 << 3) | (1 << 2) | (1 << 5) +#define SPI_CSS_BIT_0 (1 << 4) #endif #if SPI_0_ALT_2 -#define SPI_CFG PERCFG_U0CFG_ALT_2 -#define SPI_SEL P1SEL -#define SPI_PRI P2SEL_PRI3P1_USART0 -#define SPI_BITS (1 << 5) | (1 << 4) | (1 << 3) -#define SPI_CSS_BIT (1 << 2) +#define SPI_CFG_0 PERCFG_U0CFG_ALT_2 +#define SPI_SEL_0 P1SEL +#define SPI_PRI_0 P2SEL_PRI3P1_USART0 +#define SPI_BITS_0 (1 << 5) | (1 << 4) | (1 << 3) +#define SPI_CSS_BIT_0 (1 << 2) #endif #endif #if HAS_SPI_1 -#define SPI_CSR U1CSR -#define SPI_BUF U1DBUFXADDR -#define SPI_BAUD U1BAUD -#define SPI_GCR U1GCR -#define SPI_CFG_MASK PERCFG_U1CFG_ALT_MASK -#define SPI_DMA_TX DMA_CFG0_TRIGGER_UTX1 -#define SPI_DMA_RX DMA_CFG0_TRIGGER_URX1 +#define SPI_BUF_1 &U1DBUFXADDR +#define SPI_CSR_1 U1CSR +#define SPI_BAUD_1 U1BAUD +#define SPI_GCR_1 U1GCR +#define SPI_CFG_MASK_1 PERCFG_U1CFG_ALT_MASK +#define SPI_DMA_TX_1 DMA_CFG0_TRIGGER_UTX1 +#define SPI_DMA_RX_1 DMA_CFG0_TRIGGER_URX1 #if SPI_1_ALT_1 -#define SPI_CFG PERCFG_U1CFG_ALT_1 -#define SPI_SEL P0SEL -#define SPI_BITS (1 << 4) | (1 << 5) | (1 << 3) -#define SPI_CSS_BIT (1 << 2) +#define SPI_CFG_1 PERCFG_U1CFG_ALT_1 +#define SPI_SEL_1 P0SEL +#define SPI_BITS_1 (1 << 4) | (1 << 5) | (1 << 3) +#define SPI_CSS_BIT_1 (1 << 2) #endif #if SPI_1_ALT_2 -#define SPI_CFG PERCFG_U1CFG_ALT_2 -#define SPI_SEL P1SEL -#define SPI_PRI P2SEL_PRI3P1_USART1 -#define SPI_BITS (1 << 6) | (1 << 7) | (1 << 5) -#define SPI_CSS_BIT (1 << 4) +#define SPI_CFG_1 PERCFG_U1CFG_ALT_2 +#define SPI_SEL_1 P1SEL +#define SPI_PRI_1 P2SEL_PRI3P1_USART1 +#define SPI_BITS_1 (1 << 6) | (1 << 7) | (1 << 5) +#define SPI_CSS_BIT_1 (1 << 4) #endif #endif +#if MULTI_SPI + +#define SPI_BUF(bus) ((bus) ? SPI_BUF_1 : SPI_BUF_0) +#define SPI_CSR(bus) ((bus) ? SPI_CSR_1 : SPI_CSR_0) +#define SPI_BAUD(bus) ((bus) ? SPI_BAUD_1 : SPI_BAUD_0) +#define SPI_GCR(bus) ((bus) ? SPI_GCR_1 : SPI_GCR_0) +#define SPI_CFG_MASK(bus) ((bus) ? SPI_CFG_MASK_1 : SPI_CFG_MASK_0) +#define SPI_DMA_TX(bus) ((bus) ? SPI_DMA_TX_1 : SPI_DMA_TX_0) +#define SPI_DMA_RX(bus) ((bus) ? SPI_DMA_RX_1 : SPI_DMA_RX_0) +#define SPI_CFG(bus) ((bus) ? SPI_CFG_1 : SPI_CFG_0) +#define SPI_SEL(bus) ((bus) ? SPI_SEL_1 : SPI_SEL_0) +#define SPI_BITS(bus) ((bus) ? SPI_BITS_1 : SPI_BITS_0) +#define SPI_CSS_BIT(bus) ((bus) ? SPI_CSS_BIT_1 : SPI_CSS_BIT_0) + +#else + +#if HAS_SPI_0 +#define SPI_BUF(bus) SPI_BUF_0 +#define SPI_CSR(bus) SPI_CSR_0 +#define SPI_BAUD(bus) SPI_BAUD_0 +#define SPI_GCR(bus) SPI_GCR_0 +#define SPI_CFG_MASK(bus) SPI_CFG_MASK_0 +#define SPI_DMA_TX(bus) SPI_DMA_TX_0 +#define SPI_DMA_RX(bus) SPI_DMA_RX_0 +#define SPI_CFG(bus) SPI_CFG_0 +#define SPI_SEL(bus) SPI_SEL_0 +#define SPI_BITS(bus) SPI_BITS_0 +#define SPI_CSS_BIT(bus) SPI_CSS_BIT_0 +#endif +#if HAS_SPI_1 +#define SPI_BUF(bus) SPI_BUF_1 +#define SPI_CSR(bus) SPI_CSR_1 +#define SPI_BAUD(bus) SPI_BAUD_1 +#define SPI_GCR(bus) SPI_GCR_1 +#define SPI_CFG_MASK(bus) SPI_CFG_MASK_1 +#define SPI_DMA_TX(bus) SPI_DMA_TX_1 +#define SPI_DMA_RX(bus) SPI_DMA_RX_1 +#define SPI_CFG(bus) SPI_CFG_1 +#define SPI_SEL(bus) SPI_SEL_1 +#define SPI_BITS(bus) SPI_BITS_1 +#define SPI_CSS_BIT(bus) SPI_CSS_BIT_1 +#endif + +#endif /* MULTI_SPI */ + #if AO_SPI_SLAVE -#define CSS SPI_CSS_BIT +#define CSS(bus) SPI_CSS_BIT(bus) #define UxCSR_DIRECTION UxCSR_SLAVE #else -#define CSS 0 +#define CSS(bus) 0 #define UxCSR_DIRECTION UxCSR_MASTER #endif @@ -124,15 +165,16 @@ * operation, from CS low to CS high. This means that any SPI * user must protect the SPI bus with this mutex */ -__xdata uint8_t ao_spi_mutex; -__xdata uint8_t ao_spi_dma_in_done; -__xdata uint8_t ao_spi_dma_out_done; +__xdata uint8_t ao_spi_mutex[N_SPI]; +__xdata uint8_t ao_spi_dma_in_done[N_SPI]; +__xdata uint8_t ao_spi_dma_out_done[N_SPI]; -uint8_t ao_spi_dma_out_id; -uint8_t ao_spi_dma_in_id; +uint8_t ao_spi_dma_out_id[N_SPI]; +uint8_t ao_spi_dma_in_id[N_SPI]; static __xdata uint8_t ao_spi_const; + /* Send bytes over SPI. * * This sets up two DMA engines, one writing the data and another reading @@ -140,45 +182,52 @@ static __xdata uint8_t ao_spi_const; * is complete, as the transmit register is double buffered and hence signals * completion one byte before the transfer is actually complete */ +#if MULTI_SPI +void +ao_spi_send(void __xdata *block, uint16_t len, uint8_t bus) __reentrant +#else void ao_spi_send_bus(void __xdata *block, uint16_t len) __reentrant +#define bus 0 +#endif { - ao_dma_set_transfer(ao_spi_dma_in_id, - &SPI_BUF, + ao_dma_set_transfer(ao_spi_dma_in_id[bus], + SPI_BUF(bus), &ao_spi_const, len, DMA_CFG0_WORDSIZE_8 | DMA_CFG0_TMODE_SINGLE | - SPI_DMA_RX, + SPI_DMA_RX(bus), DMA_CFG1_SRCINC_0 | DMA_CFG1_DESTINC_0 | DMA_CFG1_PRIORITY_NORMAL); - ao_dma_set_transfer(ao_spi_dma_out_id, + ao_dma_set_transfer(ao_spi_dma_out_id[bus], block, - &SPI_BUF, + SPI_BUF(bus), len, DMA_CFG0_WORDSIZE_8 | DMA_CFG0_TMODE_SINGLE | - SPI_DMA_TX, + SPI_DMA_TX(bus), DMA_CFG1_SRCINC_1 | DMA_CFG1_DESTINC_0 | DMA_CFG1_PRIORITY_NORMAL); - ao_dma_start(ao_spi_dma_in_id); - ao_dma_start(ao_spi_dma_out_id); - ao_dma_trigger(ao_spi_dma_out_id); + ao_dma_start(ao_spi_dma_in_id[bus]); + ao_dma_start(ao_spi_dma_out_id[bus]); + ao_dma_trigger(ao_spi_dma_out_id[bus]); #if !AO_SPI_SLAVE - __critical while (!ao_spi_dma_in_done) - ao_sleep(&ao_spi_dma_in_done); + __critical while (!ao_spi_dma_in_done[bus]) + ao_sleep(&ao_spi_dma_in_done[bus]); #endif +#undef bus } #if AO_SPI_SLAVE void ao_spi_send_wait(void) { - __critical while (!ao_spi_dma_in_done) - ao_sleep(&ao_spi_dma_in_done); + __critical while (!ao_spi_dma_in_done[0]) + ao_sleep(&ao_spi_dma_in_done[0]); } #endif @@ -188,16 +237,22 @@ ao_spi_send_wait(void) * writing constant values to the SPI transmitter as that is what * clocks the data coming in. */ +#if MULTI_SPI +void +ao_spi_recv(void __xdata *block, uint16_t len, uint8_t bus) __reentrant +#else void ao_spi_recv_bus(void __xdata *block, uint16_t len) __reentrant +#define bus 0 +#endif { - ao_dma_set_transfer(ao_spi_dma_in_id, - &SPI_BUF, + ao_dma_set_transfer(ao_spi_dma_in_id[bus], + SPI_BUF(bus), block, len, DMA_CFG0_WORDSIZE_8 | DMA_CFG0_TMODE_SINGLE | - SPI_DMA_RX, + SPI_DMA_RX(bus), DMA_CFG1_SRCINC_0 | DMA_CFG1_DESTINC_1 | DMA_CFG1_PRIORITY_NORMAL); @@ -205,24 +260,24 @@ ao_spi_recv_bus(void __xdata *block, uint16_t len) __reentrant ao_spi_const = SPI_CONST; #if !AO_SPI_SLAVE - ao_dma_set_transfer(ao_spi_dma_out_id, + ao_dma_set_transfer(ao_spi_dma_out_id[bus], &ao_spi_const, - &SPI_BUF, + SPI_BUF(bus), len, DMA_CFG0_WORDSIZE_8 | DMA_CFG0_TMODE_SINGLE | - SPI_DMA_TX, + SPI_DMA_TX(bus), DMA_CFG1_SRCINC_0 | DMA_CFG1_DESTINC_0 | DMA_CFG1_PRIORITY_NORMAL); #endif - ao_dma_start(ao_spi_dma_in_id); + ao_dma_start(ao_spi_dma_in_id[bus]); #if !AO_SPI_SLAVE - ao_dma_start(ao_spi_dma_out_id); - ao_dma_trigger(ao_spi_dma_out_id); - __critical while (!ao_spi_dma_in_done) - ao_sleep(&ao_spi_dma_in_done); + ao_dma_start(ao_spi_dma_out_id[bus]); + ao_dma_trigger(ao_spi_dma_out_id[bus]); + __critical while (!ao_spi_dma_in_done[bus]) + ao_sleep(&ao_spi_dma_in_done[bus]); #endif } @@ -230,17 +285,43 @@ ao_spi_recv_bus(void __xdata *block, uint16_t len) __reentrant void ao_spi_recv_wait(void) { - __critical while (!ao_spi_dma_in_done) - ao_sleep(&ao_spi_dma_in_done); + __critical while (!ao_spi_dma_in_done[0]) + ao_sleep(&ao_spi_dma_in_done[0]); } #endif +/* Set up the USART. + * + * SPI master/slave mode + */ +/* Set the baud rate and signal parameters + * + * The cc1111 is limited to a 24/8 MHz SPI clock. + * Every peripheral I've ever seen goes faster than that, + * so set the clock to 3MHz (BAUD_E 17, BAUD_M 0) + */ +#define SPI_INIT(bus,o) do { \ + /* Set up the USART pin assignment */ \ + PERCFG = (PERCFG & ~SPI_CFG_MASK(bus)) | SPI_CFG(bus); \ + \ + /* Make the SPI pins be controlled by the USART peripheral */ \ + SPI_SEL(bus) |= SPI_BITS(bus) | CSS(bus); \ + SPI_CSR(bus) = (UxCSR_MODE_SPI | UxCSR_RE | UxCSR_DIRECTION); \ + SPI_BAUD(bus) = 0; \ + SPI_GCR(bus) = (UxGCR_CPOL_NEGATIVE | \ + UxGCR_CPHA_FIRST_EDGE | \ + UxGCR_ORDER_MSB | \ + (17 << UxGCR_BAUD_E_SHIFT)); \ + /* Set up OUT DMA */ \ + ao_spi_dma_out_id[o] = ao_dma_alloc(&ao_spi_dma_out_done[o]); \ + \ + /* Set up IN DMA */ \ + ao_spi_dma_in_id[o] = ao_dma_alloc(&ao_spi_dma_in_done[o]); \ + } while (0) + void ao_spi_init(void) { - /* Set up the USART pin assignment */ - PERCFG = (PERCFG & ~SPI_CFG_MASK) | SPI_CFG; - /* Ensure that SPI USART takes precidence over the other USART * for pins that they share */ @@ -248,30 +329,10 @@ ao_spi_init(void) P2SEL = (P2SEL & ~P2SEL_PRI3P1_MASK) | SPI_PRI; #endif - /* Make the SPI pins be controlled by the USART peripheral */ - SPI_SEL |= SPI_BITS | CSS; - - /* Set up OUT DMA */ - ao_spi_dma_out_id = ao_dma_alloc(&ao_spi_dma_out_done); - - /* Set up IN DMA */ - ao_spi_dma_in_id = ao_dma_alloc(&ao_spi_dma_in_done); - - /* Set up the USART. - * - * SPI master/slave mode - */ - SPI_CSR = (UxCSR_MODE_SPI | UxCSR_RE | UxCSR_DIRECTION); - - /* Set the baud rate and signal parameters - * - * The cc1111 is limited to a 24/8 MHz SPI clock. - * Every peripheral I've ever seen goes faster than that, - * so set the clock to 3MHz (BAUD_E 17, BAUD_M 0) - */ - SPI_BAUD = 0; - SPI_GCR = (UxGCR_CPOL_NEGATIVE | - UxGCR_CPHA_FIRST_EDGE | - UxGCR_ORDER_MSB | - (17 << UxGCR_BAUD_E_SHIFT)); +#if HAS_SPI_0 + SPI_INIT(0, 0); +#endif +#if HAS_SPI_1 + SPI_INIT(1, MULTI_SPI); +#endif } diff --git a/src/cc1111/ao_timer.c b/src/cc1111/ao_timer.c index a64b5aba..75cc4ce8 100644 --- a/src/cc1111/ao_timer.c +++ b/src/cc1111/ao_timer.c @@ -39,6 +39,9 @@ void ao_timer_isr(void) __interrupt 9 if (++ao_adc_count == ao_adc_interval) { ao_adc_count = 0; ao_adc_poll(); +#if (AO_DATA_ALL & ~(AO_DATA_ADC)) + ao_wakeup(DATA_TO_XDATA(&ao_adc_count)); +#endif } #endif } @@ -92,6 +95,13 @@ ao_clock_init(void) while (!(SLEEP & SLEEP_XOSC_STB)) ; + /* Power down the unused HFRC oscillator */ + SLEEP |= SLEEP_OSC_PD; + + /* Wait for HFRC to power down */ + while ((SLEEP & SLEEP_HFRC_STB) != 0) + ; + /* Crank up the timer tick and system clock speed */ CLKCON = ((CLKCON & ~(CLKCON_TICKSPD_MASK | CLKCON_CLKSPD_MASK)) | (CLKCON_TICKSPD_1 | CLKCON_CLKSPD_1)); diff --git a/src/cc1111/ao_usb.c b/src/cc1111/ao_usb.c index a655d1be..b0ab409d 100644 --- a/src/cc1111/ao_usb.c +++ b/src/cc1111/ao_usb.c @@ -201,6 +201,11 @@ ao_usb_ep0_setup(void) ao_usb_ep0_queue_byte(0); break; case AO_USB_REQ_SET_ADDRESS: +#if USB_FORCE_FLIGHT_IDLE + /* Go to idle mode if USB is connected + */ + ao_flight_force_idle = 1; +#endif ao_usb_set_address(ao_usb_setup.value); break; case AO_USB_REQ_GET_DESCRIPTOR: diff --git a/src/core/ao.h b/src/core/ao.h index 0ad3e4aa..0b634a79 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -43,6 +43,12 @@ #define HAS_TASK 1 #endif +#ifndef AO_PORT_TYPE +#define AO_PORT_TYPE uint8_t +#endif + +typedef AO_PORT_TYPE ao_port_t; + #if HAS_TASK #include <ao_task.h> #else @@ -68,6 +74,8 @@ #define AO_PANIC_SPI 13 /* SPI communication failure */ #define AO_PANIC_CRASH 14 /* Processor crashed */ #define AO_PANIC_BUFIO 15 /* Mis-using bufio API */ +#define AO_PANIC_EXTI 16 /* Mis-using exti API */ +#define AO_PANIC_FAST_TIMER 17 /* Mis-using fast timer API */ #define AO_PANIC_SELF_TEST_CC1120 0x40 | 1 /* Self test failure */ #define AO_PANIC_SELF_TEST_HMC5883 0x40 | 2 /* Self test failure */ #define AO_PANIC_SELF_TEST_MPU6000 0x40 | 3 /* Self test failure */ @@ -174,7 +182,7 @@ void ao_cmd_hex(void); void -ao_cmd_decimal(void); +ao_cmd_decimal(void) __reentrant; /* Read a single hex nibble off stdin. */ uint8_t @@ -334,6 +342,10 @@ ao_spi_slave(void); #define AO_GPS_DATE_VALID (1 << 6) #define AO_GPS_COURSE_VALID (1 << 7) +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + +extern __xdata uint8_t ao_gps_new; extern __pdata uint16_t ao_gps_tick; extern __xdata uint8_t ao_gps_mutex; extern __xdata struct ao_telemetry_location ao_gps_data; @@ -380,6 +392,9 @@ void ao_gps_tracking_print(__xdata struct ao_gps_tracking_orig *gps_tracking_data); void +ao_gps_show(void) __reentrant; + +void ao_gps_init(void); /* @@ -693,6 +708,8 @@ struct ao_ignition { uint8_t firing; }; +extern __code char * __code ao_igniter_status_names[]; + extern __xdata struct ao_ignition ao_ignition[2]; enum ao_igniter_status @@ -722,7 +739,7 @@ extern __xdata uint8_t ao_force_freq; #endif #define AO_CONFIG_MAJOR 1 -#define AO_CONFIG_MINOR 14 +#define AO_CONFIG_MINOR 15 #define AO_AES_LEN 16 @@ -756,6 +773,11 @@ struct ao_config { #if HAS_RADIO_AMP uint8_t radio_amp; /* minor version 14 */ #endif +#if HAS_GYRO + int16_t accel_zero_along; /* minor version 15 */ + int16_t accel_zero_across; /* minor version 15 */ + int16_t accel_zero_through; /* minor version 15 */ +#endif }; #define AO_IGNITE_MODE_DUAL 0 diff --git a/src/core/ao_adc.h b/src/core/ao_adc.h index 0dd87080..373db1c4 100644 --- a/src/core/ao_adc.h +++ b/src/core/ao_adc.h @@ -28,10 +28,6 @@ ao_adc_poll(void); void ao_adc_sleep(void); -/* Get a copy of the last complete sample set */ -void -ao_data_get(__xdata struct ao_data *packet); - /* Initialize the A/D converter */ void ao_adc_init(void); diff --git a/src/core/ao_cmd.c b/src/core/ao_cmd.c index 188b8bb4..4ebaa607 100644 --- a/src/core/ao_cmd.c +++ b/src/core/ao_cmd.c @@ -206,9 +206,9 @@ ao_cmd_hex(void) } void -ao_cmd_decimal(void) +ao_cmd_decimal(void) __reentrant { - __pdata uint8_t r = ao_cmd_lex_error; + uint8_t r = ao_cmd_lex_error; ao_cmd_lex_u32 = 0; ao_cmd_white(); @@ -290,9 +290,6 @@ version(void) , ao_log_format #endif ); -#if HAS_MS5607 - ao_ms5607_info(); -#endif printf("software-version %s\n", ao_version); } #endif diff --git a/src/core/ao_config.c b/src/core/ao_config.c index 73608a55..a30ec64a 100644 --- a/src/core/ao_config.c +++ b/src/core/ao_config.c @@ -17,7 +17,7 @@ #include "ao.h" #include "ao_log.h" -#include <ao_storage.h> +#include <ao_config.h> #if HAS_FLIGHT #include <ao_sample.h> #include <ao_data.h> @@ -28,6 +28,9 @@ __pdata uint8_t ao_config_loaded; __pdata uint8_t ao_config_dirty; __xdata uint8_t ao_config_mutex; +#ifndef AO_CONFIG_DEFAULT_APRS_INTERVAL +#define AO_CONFIG_DEFAULT_APRS_INTERVAL 0 +#endif #define AO_CONFIG_DEFAULT_MAIN_DEPLOY 250 #define AO_CONFIG_DEFAULT_RADIO_CHANNEL 0 #define AO_CONFIG_DEFAULT_CALLSIGN "N0CALL" @@ -47,20 +50,22 @@ __xdata uint8_t ao_config_mutex; #define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ((uint32_t) 192 * (uint32_t) 1024) #endif #endif +#ifndef AO_CONFIG_DEFAULT_RADIO_POWER #define AO_CONFIG_DEFAULT_RADIO_POWER 0x60 +#endif #define AO_CONFIG_DEFAULT_RADIO_AMP 0 #if HAS_EEPROM static void _ao_config_put(void) { - ao_storage_setup(); - ao_storage_erase(ao_storage_config); - ao_storage_write(ao_storage_config, &ao_config, sizeof (ao_config)); + ao_config_setup(); + ao_config_erase(); + ao_config_write(0, &ao_config, sizeof (ao_config)); #if HAS_FLIGHT ao_log_write_erase(0); #endif - ao_storage_flush(); + ao_config_flush(); } void @@ -92,8 +97,8 @@ _ao_config_get(void) * but ao_storage_setup *also* sets ao_storage_config, which we * need before calling ao_storage_read here */ - ao_storage_setup(); - ao_storage_read(ao_storage_config, &ao_config, sizeof (ao_config)); + ao_config_setup(); + ao_config_read(0, &ao_config, sizeof (ao_config)); #endif if (ao_config.major != AO_CONFIG_MAJOR) { ao_config.major = AO_CONFIG_MAJOR; @@ -122,8 +127,10 @@ _ao_config_get(void) ao_config.radio_cal = ao_radio_cal; #endif /* Fixups for minor version 4 */ +#if HAS_FLIGHT if (minor < 4) ao_config.flight_log_max = AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX; +#endif /* Fixupes for minor version 5 */ if (minor < 5) ao_config.ignite_mode = AO_CONFIG_DEFAULT_IGNITE_MODE; @@ -142,7 +149,7 @@ _ao_config_get(void) memset(&ao_config.pyro, '\0', sizeof (ao_config.pyro)); #endif if (minor < 13) - ao_config.aprs_interval = 0; + ao_config.aprs_interval = AO_CONFIG_DEFAULT_APRS_INTERVAL; #if HAS_RADIO_POWER if (minor < 14) ao_config.radio_power = AO_CONFIG_DEFAULT_RADIO_POWER; @@ -151,6 +158,19 @@ _ao_config_get(void) if (minor < 14) ao_config.radio_amp = AO_CONFIG_DEFAULT_RADIO_AMP; #endif +#if HAS_GYRO + if (minor < 15) { + ao_config.accel_zero_along = 0; + ao_config.accel_zero_across = 0; + ao_config.accel_zero_through = 0; + + /* Reset the main accel offsets to force + * re-calibration + */ + ao_config.accel_plus_g = 0; + ao_config.accel_minus_g = 0; + } +#endif ao_config.minor = AO_CONFIG_MINOR; ao_config_dirty = 1; } @@ -270,17 +290,34 @@ ao_config_accel_calibrate_show(void) __reentrant { printf("Accel cal +1g: %d -1g: %d\n", ao_config.accel_plus_g, ao_config.accel_minus_g); +#if HAS_GYRO + printf ("IMU cal along %d across %d through %d\n", + ao_config.accel_zero_along, + ao_config.accel_zero_across, + ao_config.accel_zero_through); +#endif } #define ACCEL_CALIBRATE_SAMPLES 1024 #define ACCEL_CALIBRATE_SHIFT 10 +#if HAS_GYRO +static int16_t accel_cal_along; +static int16_t accel_cal_across; +static int16_t accel_cal_through; +#endif + static int16_t ao_config_accel_calibrate_auto(char *orientation) __reentrant { uint16_t i; int32_t accel_total; uint8_t cal_data_ring; +#if HAS_GYRO + int32_t accel_along_total = 0; + int32_t accel_across_total = 0; + int32_t accel_through_total = 0; +#endif printf("Orient antenna %s and press a key...", orientation); flush(); @@ -294,10 +331,20 @@ ao_config_accel_calibrate_auto(char *orientation) __reentrant ao_sleep(DATA_TO_XDATA(&ao_sample_data)); while (i && cal_data_ring != ao_sample_data) { accel_total += (int32_t) ao_data_accel(&ao_data_ring[cal_data_ring]); +#if HAS_GYRO + accel_along_total += (int32_t) ao_data_along(&ao_data_ring[cal_data_ring]); + accel_across_total += (int32_t) ao_data_across(&ao_data_ring[cal_data_ring]); + accel_through_total += (int32_t) ao_data_through(&ao_data_ring[cal_data_ring]); +#endif cal_data_ring = ao_data_ring_next(cal_data_ring); i--; } } +#if HAS_GYRO + accel_cal_along = accel_along_total >> ACCEL_CALIBRATE_SHIFT; + accel_cal_across = accel_across_total >> ACCEL_CALIBRATE_SHIFT; + accel_cal_through = accel_through_total >> ACCEL_CALIBRATE_SHIFT; +#endif return accel_total >> ACCEL_CALIBRATE_SHIFT; } @@ -305,12 +352,28 @@ void ao_config_accel_calibrate_set(void) __reentrant { int16_t up, down; +#if HAS_GYRO + int16_t accel_along_up, accel_along_down; + int16_t accel_across_up, accel_across_down; + int16_t accel_through_up, accel_through_down; +#endif + ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) return; if (ao_cmd_lex_i == 0) { up = ao_config_accel_calibrate_auto("up"); +#if HAS_GYRO + accel_along_up = accel_cal_along; + accel_across_up = accel_cal_across; + accel_through_up = accel_cal_through; +#endif down = ao_config_accel_calibrate_auto("down"); +#if HAS_GYRO + accel_along_down = accel_cal_along; + accel_across_down = accel_cal_across; + accel_through_down = accel_cal_through; +#endif } else { up = ao_cmd_lex_i; ao_cmd_decimal(); @@ -326,6 +389,11 @@ ao_config_accel_calibrate_set(void) __reentrant _ao_config_edit_start(); ao_config.accel_plus_g = up; ao_config.accel_minus_g = down; +#if HAS_GYRO + ao_config.accel_zero_along = (accel_along_up + accel_along_down) / 2; + ao_config.accel_zero_across = (accel_across_up + accel_across_down) / 2; + ao_config.accel_zero_through = (accel_through_up + accel_through_down) / 2; +#endif _ao_config_edit_finish(); } #endif /* HAS_ACCEL */ @@ -399,7 +467,7 @@ void ao_config_log_set(void) __reentrant { uint16_t block = (uint16_t) (ao_storage_block >> 10); - uint16_t config = (uint16_t) (ao_storage_config >> 10); + uint16_t log_max = (uint16_t) (ao_storage_log_max >> 10); ao_cmd_decimal(); if (ao_cmd_status != ao_cmd_success) @@ -408,8 +476,8 @@ ao_config_log_set(void) __reentrant printf("Storage must be empty before changing log size\n"); else if (block > 1024 && (ao_cmd_lex_i & (block - 1))) printf("Flight log size must be multiple of %d kB\n", block); - else if (ao_cmd_lex_i > config) - printf("Flight log max %d kB\n", config); + else if (ao_cmd_lex_i > log_max) + printf("Flight log max %d kB\n", log_max); else { _ao_config_edit_start(); ao_config.flight_log_max = (uint32_t) ao_cmd_lex_i << 10; @@ -589,7 +657,7 @@ static void ao_config_show(void) __reentrant; static void -ao_config_write(void) __reentrant; +ao_config_save(void) __reentrant; __code struct ao_config_var ao_config_vars[] = { #if HAS_FLIGHT @@ -648,7 +716,7 @@ __code struct ao_config_var ao_config_vars[] = { ao_config_show, 0 }, #if HAS_EEPROM { "w\0Write to eeprom", - ao_config_write, 0 }, + ao_config_save, 0 }, #endif { "?\0Help", ao_config_help, 0 }, @@ -693,11 +761,14 @@ ao_config_show(void) __reentrant for (cmd = 0; ao_config_vars[cmd].str != NULL; cmd++) if (ao_config_vars[cmd].show) (*ao_config_vars[cmd].show)(); +#if HAS_MS5607 + ao_ms5607_info(); +#endif } #if HAS_EEPROM static void -ao_config_write(void) __reentrant +ao_config_save(void) __reentrant { uint8_t saved = 0; ao_mutex_get(&ao_config_mutex); diff --git a/src/core/ao_config.h b/src/core/ao_config.h new file mode 100644 index 00000000..e101af8e --- /dev/null +++ b/src/core/ao_config.h @@ -0,0 +1,53 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_CONFIG_H_ +#define _AO_CONFIG_H_ + +#ifndef USE_STORAGE_CONFIG +#define USE_STORAGE_CONFIG 1 +#endif + +#ifndef USE_EEPROM_CONFIG +#define USE_EEPROM_CONFIG 0 +#endif + +#if USE_STORAGE_CONFIG + +#include <ao_storage.h> + +#define ao_config_setup() ao_storage_setup() +#define ao_config_erase() ao_storage_erase(ao_storage_config) +#define ao_config_write(pos,bytes, len) ao_storage_write(ao_storage_config+(pos), bytes, len) +#define ao_config_read(pos,bytes, len) ao_storage_read(ao_storage_config+(pos), bytes, len) +#define ao_config_flush() ao_storage_flush() + +#endif + +#if USE_EEPROM_CONFIG + +#include <ao_eeprom.h> + +#define ao_config_setup() +#define ao_config_erase() +#define ao_config_write(pos,bytes, len) ao_eeprom_write(pos, bytes, len) +#define ao_config_read(pos,bytes, len) ao_eeprom_read(pos, bytes, len) +#define ao_config_flush() + +#endif + +#endif /* _AO_CONFIG_H_ */ diff --git a/src/stm/ao_data.c b/src/core/ao_data.c index 38d2f7ff..6a3d02a1 100644 --- a/src/stm/ao_data.c +++ b/src/core/ao_data.c @@ -22,6 +22,7 @@ volatile __xdata struct ao_data ao_data_ring[AO_DATA_RING]; volatile __data uint8_t ao_data_head; volatile __data uint8_t ao_data_present; +#ifndef ao_data_count void ao_data_get(__xdata struct ao_data *packet) { @@ -32,3 +33,4 @@ ao_data_get(__xdata struct ao_data *packet) #endif memcpy(packet, (void *) &ao_data_ring[i], sizeof (struct ao_data)); } +#endif diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 7e2f85d8..e1d8a139 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -18,6 +18,8 @@ #ifndef _AO_DATA_H_ #define _AO_DATA_H_ +#define GRAVITY 9.80665 + #if HAS_ADC #define AO_DATA_ADC (1 << 0) #else @@ -82,6 +84,10 @@ struct ao_data { #define ao_data_ring_next(n) (((n) + 1) & (AO_DATA_RING - 1)) #define ao_data_ring_prev(n) (((n) - 1) & (AO_DATA_RING - 1)) +/* Get a copy of the last complete sample set */ +void +ao_data_get(__xdata struct ao_data *packet); + extern volatile __xdata struct ao_data ao_data_ring[AO_DATA_RING]; extern volatile __data uint8_t ao_data_head; extern volatile __data uint8_t ao_data_present; @@ -97,7 +103,7 @@ extern volatile __data uint8_t ao_data_count; * signaled by the timer tick */ #define AO_DATA_WAIT() do { \ - ao_sleep((void *) &ao_data_count); \ + ao_sleep(DATA_TO_XDATA ((void *) &ao_data_count)); \ } while (0) #endif /* AO_DATA_RING */ @@ -268,7 +274,11 @@ typedef int16_t accel_t; /* MMA655X is hooked up so that positive values represent negative acceleration */ #define ao_data_accel(packet) ((packet)->mma655x) +#if AO_MMA655X_INVERT +#define ao_data_accel_cook(packet) (4095 - (packet)->mma655x) +#else #define ao_data_accel_cook(packet) ((packet)->mma655x) +#endif #define ao_data_set_accel(packet, accel) ((packet)->mma655x = (accel)) #define ao_data_accel_invert(accel) (4095 - (accel)) @@ -292,8 +302,8 @@ typedef int16_t accel_t; #define HAS_GYRO 1 -typedef int16_t gyro_t; -typedef int32_t angle_t; +typedef int16_t gyro_t; /* in raw sample units */ +typedef int16_t angle_t; /* in degrees */ /* Y axis is aligned with the direction of motion (along) */ /* X axis is aligned in the other board axis (across) */ @@ -309,4 +319,16 @@ typedef int32_t angle_t; #endif +#if !HAS_MAG && HAS_HMC5883 + +#define HAS_MAG 1 + +typedef int16_t ao_mag_t; /* in raw sample units */ + +#define ao_data_mag_along(packet) ((packet)->hmc5883.x) +#define ao_data_mag_across(packet) ((packet)->hmc5883.y) +#define ao_data_mag_through(packet) ((packet)->hmc5883.z) + +#endif + #endif /* _AO_DATA_H_ */ diff --git a/src/core/ao_debounce.c b/src/core/ao_debounce.c new file mode 100644 index 00000000..b9d67729 --- /dev/null +++ b/src/core/ao_debounce.c @@ -0,0 +1,163 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_debounce.h> +#include <ao_fast_timer.h> + +static uint8_t ao_debounce_initialized; +static uint8_t ao_debounce_running; +static struct ao_debounce *ao_debounce; + +static uint8_t values[64]; +static uint8_t n; + +#define d_step(n) (((n) + 1) & 63) + +static void +_ao_debounce_set(struct ao_debounce *debounce, uint8_t value) +{ + if (value != debounce->value) { + values[n] = value; + n = (n + 1) & 63; + debounce->value = value; + debounce->_set(debounce, value); + } + _ao_debounce_stop(debounce); +} + +void +ao_debounce_dump(void) +{ + uint8_t s; + + for (s = 0; s < n; s++) { + printf ("%d: %d\n", + s, values[s]); + } + n = 0; +} + +/* + * Get the current value, set the result when we've + * reached the debounce count limit + */ +static void +_ao_debounce_check(struct ao_debounce *debounce) +{ + uint8_t next = debounce->_get(debounce); + + if (next == debounce->current) { + if (debounce->count < debounce->hold) { + if (++debounce->count == debounce->hold) + _ao_debounce_set(debounce, debounce->current); + } + } else { + debounce->count = 0; + debounce->current = next; + } +} + +static void +_ao_debounce_isr(void) +{ + struct ao_debounce *debounce, *next; + + for (debounce = ao_debounce; debounce; debounce = next) { + next = debounce->next; + _ao_debounce_check(debounce); + } +} + +static void +ao_debounce_on(void) +{ + ao_fast_timer_on(_ao_debounce_isr); +} + +static void +ao_debounce_off(void) +{ + ao_fast_timer_off(_ao_debounce_isr); +} + +/* + * Start monitoring one pin + */ +void +_ao_debounce_start(struct ao_debounce *debounce) +{ + uint32_t m; + + m = ao_arch_irqsave(); + if (!debounce->running) { + debounce->running = 1; + + /* Reset the counter */ + debounce->count = 0; + + /* Link into list */ + debounce->next = ao_debounce; + ao_debounce = debounce; + + /* Make sure the timer is running */ + if (!ao_debounce_running++) + ao_debounce_on(); + + /* And go check the current value */ + _ao_debounce_check(debounce); + } + ao_arch_irqrestore(m); +} + +/* + * Stop monitoring one pin + */ +void +_ao_debounce_stop(struct ao_debounce *debounce) +{ + struct ao_debounce **prev; + uint32_t m; + + m = ao_arch_irqsave(); + if (debounce->running) { + debounce->running = 0; + + /* Unlink */ + for (prev = &ao_debounce; (*prev); prev = &((*prev)->next)) { + if (*prev == debounce) { + *prev = debounce->next; + break; + } + } + debounce->next = NULL; + + /* Turn off the timer if possible */ + if (!--ao_debounce_running) + ao_debounce_off(); + } + ao_arch_irqrestore(m); +} + +void +ao_debounce_init(void) +{ + if (ao_debounce_initialized) + return; + ao_debounce_initialized = 1; + ao_fast_timer_init(); +} diff --git a/src/core/ao_debounce.h b/src/core/ao_debounce.h new file mode 100644 index 00000000..19c620f5 --- /dev/null +++ b/src/core/ao_debounce.h @@ -0,0 +1,74 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_DEBOUNCE_H_ +#define _AO_DEBOUNCE_H_ + +struct ao_debounce { + struct ao_debounce *next; + + /* time that pin value must be stable before accepting */ + uint8_t hold; + + /* last value reported to app; don't report it twice */ + uint8_t value; + + /* current value received from pins */ + uint8_t current; + + /* current count of intervals pin value has been stable */ + uint8_t count; + + /* This pin is running */ + uint8_t running; + + /* Get the current pin value */ + uint8_t (*_get)(struct ao_debounce *debounce); + + /* The stable value has changed */ + void (*_set)(struct ao_debounce *debounce, uint8_t value); +}; + +static inline void +ao_debounce_config(struct ao_debounce *debounce, + uint8_t (*_get)(struct ao_debounce *debounce), + void (*_set)(struct ao_debounce *debounce, uint8_t value), + uint8_t hold) +{ + debounce->next = 0; + debounce->hold = hold; + debounce->value = 0xff; + debounce->current = 0xff; + debounce->count = 0; + debounce->running = 0; + debounce->_get = _get; + debounce->_set = _set; +} + +void +_ao_debounce_start(struct ao_debounce *debounce); + +void +_ao_debounce_stop(struct ao_debounce *debounce); + +void +ao_debounce_init(void); + +void +ao_debounce_dump(void); + +#endif /* _AO_DEBOUNCE_H_ */ diff --git a/src/core/ao_eeprom.h b/src/core/ao_eeprom.h new file mode 100644 index 00000000..915522bf --- /dev/null +++ b/src/core/ao_eeprom.h @@ -0,0 +1,43 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_EEPROM_H_ +#define _AO_EEPROM_H_ + +extern const ao_pos_t ao_eeprom_total; + +/* + * Write to eeprom + */ + +uint8_t +ao_eeprom_write(ao_pos_t pos32, __xdata void *v, uint16_t len); + +/* + * Read from eeprom + */ +uint8_t +ao_eeprom_read(ao_pos_t pos, __xdata void *v, uint16_t len); + +/* + * Initialize eeprom + */ + +void +ao_eeprom_init(void); + +#endif /* _AO_EEPROM_H_ */ diff --git a/src/core/ao_flight.c b/src/core/ao_flight.c index 9d9d4c6e..463ff4a2 100644 --- a/src/core/ao_flight.c +++ b/src/core/ao_flight.c @@ -20,6 +20,10 @@ #include <ao_log.h> #endif +#if HAS_MPU6000 +#include <ao_quaternion.h> +#endif + #ifndef HAS_ACCEL #error Please define HAS_ACCEL #endif @@ -42,6 +46,11 @@ __pdata enum ao_flight_state ao_flight_state; /* current flight state */ __pdata uint16_t ao_boost_tick; /* time of launch detect */ __pdata uint16_t ao_motor_number; /* number of motors burned so far */ +#if HAS_IMU +/* Any sensor can set this to mark the flight computer as 'broken' */ +__xdata uint8_t ao_sensor_errors; +#endif + /* * track min/max data over a long interval to detect * resting @@ -95,6 +104,9 @@ ao_flight(void) ao_config.accel_minus_g == 0 || ao_ground_accel < ao_config.accel_plus_g - ACCEL_NOSE_UP || ao_ground_accel > ao_config.accel_minus_g + ACCEL_NOSE_UP || +#if HAS_IMU + ao_sensor_errors || +#endif ao_ground_height < -1000 || ao_ground_height > 7000) { @@ -117,14 +129,14 @@ ao_flight(void) { /* Set pad mode - we can fly! */ ao_flight_state = ao_flight_pad; -#if HAS_USB && HAS_RADIO && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE +#if HAS_USB && !HAS_FLIGHT_DEBUG && !HAS_SAMPLE_PROFILE /* Disable the USB controller in flight mode * to save power */ ao_usb_disable(); #endif -#if !HAS_ACCEL +#if !HAS_ACCEL && PACKET_HAS_SLAVE /* Disable packet mode in pad state on TeleMini */ ao_packet_slave_stop(); #endif @@ -134,8 +146,10 @@ ao_flight(void) ao_rdf_set(1); ao_telemetry_set_interval(AO_TELEMETRY_INTERVAL_PAD); #endif +#if HAS_LED /* signal successful initialization by turning off the LED */ ao_led_off(AO_LED_RED); +#endif } else { /* Set idle mode */ ao_flight_state = ao_flight_idle; @@ -145,8 +159,10 @@ ao_flight(void) ao_packet_slave_start(); #endif +#if HAS_LED /* signal successful initialization by turning off the LED */ ao_led_off(AO_LED_RED); +#endif } /* wakeup threads due to state change */ ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); @@ -190,8 +206,8 @@ ao_flight(void) #if HAS_GPS /* Record current GPS position by waking up GPS log tasks */ - ao_wakeup(&ao_gps_data); - ao_wakeup(&ao_gps_tracking_data); + ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING; + ao_wakeup(&ao_gps_new); #endif ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); @@ -356,6 +372,18 @@ ao_flight(void) ao_interval_end = ao_sample_tick + AO_INTERVAL_TICKS; } break; +#if HAS_FLIGHT_DEBUG + case ao_flight_test: +#if HAS_GYRO + printf ("angle %4d pitch %7d yaw %7d roll %7d\n", + ao_sample_orient, + ((ao_sample_pitch << 9) - ao_ground_pitch) >> 9, + ((ao_sample_yaw << 9) - ao_ground_yaw) >> 9, + ((ao_sample_roll << 9) - ao_ground_roll) >> 9); +#endif + flush(); + break; +#endif /* HAS_FLIGHT_DEBUG */ default: break; } @@ -406,8 +434,17 @@ ao_flight_dump(void) printf (" error_avg %d\n", ao_error_h_sq_avg); } +static void +ao_gyro_test(void) +{ + ao_flight_state = ao_flight_test; + ao_getchar(); + ao_flight_state = ao_flight_idle; +} + __code struct ao_cmds ao_flight_cmds[] = { { ao_flight_dump, "F\0Dump flight status" }, + { ao_gyro_test, "G\0Test gyro code" }, { 0, NULL }, }; #endif diff --git a/src/core/ao_flight.h b/src/core/ao_flight.h index b80202f0..c7c02ccf 100644 --- a/src/core/ao_flight.h +++ b/src/core/ao_flight.h @@ -33,13 +33,18 @@ enum ao_flight_state { ao_flight_drogue = 6, ao_flight_main = 7, ao_flight_landed = 8, - ao_flight_invalid = 9 + ao_flight_invalid = 9, + ao_flight_test = 10 }; extern __pdata enum ao_flight_state ao_flight_state; extern __pdata uint16_t ao_boost_tick; extern __pdata uint16_t ao_motor_number; +#if HAS_IMU +extern __xdata uint8_t ao_sensor_errors; +#endif + extern __pdata uint16_t ao_launch_time; extern __pdata uint8_t ao_flight_force_idle; diff --git a/src/core/ao_gps_report.c b/src/core/ao_gps_report.c index c52ef621..07201ac2 100644 --- a/src/core/ao_gps_report.c +++ b/src/core/ao_gps_report.c @@ -22,78 +22,68 @@ ao_gps_report(void) { static __xdata struct ao_log_record gps_log; static __xdata struct ao_telemetry_location gps_data; + static __xdata struct ao_telemetry_satellite gps_tracking_data; uint8_t date_reported = 0; + uint8_t new; for (;;) { - ao_sleep(&ao_gps_data); + while ((new = ao_gps_new) == 0) + ao_sleep(&ao_gps_new); ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + if (new & AO_GPS_NEW_DATA) + ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + if (new & AO_GPS_NEW_TRACKING) + ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + ao_gps_new = 0; ao_mutex_put(&ao_gps_mutex); - if (!(gps_data.flags & AO_GPS_VALID)) - continue; - - gps_log.tick = ao_gps_tick; - gps_log.type = AO_LOG_GPS_TIME; - gps_log.u.gps_time.hour = gps_data.hour; - gps_log.u.gps_time.minute = gps_data.minute; - gps_log.u.gps_time.second = gps_data.second; - gps_log.u.gps_time.flags = gps_data.flags; - ao_log_data(&gps_log); - gps_log.type = AO_LOG_GPS_LAT; - gps_log.u.gps_latitude = gps_data.latitude; - ao_log_data(&gps_log); - gps_log.type = AO_LOG_GPS_LON; - gps_log.u.gps_longitude = gps_data.longitude; - ao_log_data(&gps_log); - gps_log.type = AO_LOG_GPS_ALT; - gps_log.u.gps_altitude.altitude = gps_data.altitude; - gps_log.u.gps_altitude.unused = 0xffff; - ao_log_data(&gps_log); - if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) { - gps_log.type = AO_LOG_GPS_DATE; - gps_log.u.gps_date.year = gps_data.year; - gps_log.u.gps_date.month = gps_data.month; - gps_log.u.gps_date.day = gps_data.day; - gps_log.u.gps_date.extra = 0; - date_reported = ao_log_data(&gps_log); + if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { + gps_log.tick = ao_gps_tick; + gps_log.type = AO_LOG_GPS_TIME; + gps_log.u.gps_time.hour = gps_data.hour; + gps_log.u.gps_time.minute = gps_data.minute; + gps_log.u.gps_time.second = gps_data.second; + gps_log.u.gps_time.flags = gps_data.flags; + ao_log_data(&gps_log); + gps_log.type = AO_LOG_GPS_LAT; + gps_log.u.gps_latitude = gps_data.latitude; + ao_log_data(&gps_log); + gps_log.type = AO_LOG_GPS_LON; + gps_log.u.gps_longitude = gps_data.longitude; + ao_log_data(&gps_log); + gps_log.type = AO_LOG_GPS_ALT; + gps_log.u.gps_altitude.altitude = gps_data.altitude; + gps_log.u.gps_altitude.unused = 0xffff; + ao_log_data(&gps_log); + if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) { + gps_log.type = AO_LOG_GPS_DATE; + gps_log.u.gps_date.year = gps_data.year; + gps_log.u.gps_date.month = gps_data.month; + gps_log.u.gps_date.day = gps_data.day; + gps_log.u.gps_date.extra = 0; + date_reported = ao_log_data(&gps_log); + } } - } -} - -void -ao_gps_tracking_report(void) -{ - static __xdata struct ao_log_record gps_log; - static __xdata struct ao_telemetry_satellite gps_tracking_data; - uint8_t c, n; - - for (;;) { - ao_sleep(&ao_gps_tracking_data); - ao_mutex_get(&ao_gps_mutex); - gps_log.tick = ao_gps_tick; - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); - ao_mutex_put(&ao_gps_mutex); + if (new & AO_GPS_NEW_TRACKING) { + uint8_t c, n; - if (!(n = gps_tracking_data.channels)) - continue; - - gps_log.type = AO_LOG_GPS_SAT; - for (c = 0; c < n; c++) - if ((gps_log.u.gps_sat.svid = gps_tracking_data.sats[c].svid)) - { - gps_log.u.gps_sat.c_n = gps_tracking_data.sats[c].c_n_1; - ao_log_data(&gps_log); + if ((n = gps_tracking_data.channels) != 0) { + gps_log.type = AO_LOG_GPS_SAT; + for (c = 0; c < n; c++) + if ((gps_log.u.gps_sat.svid = gps_tracking_data.sats[c].svid)) + { + gps_log.u.gps_sat.c_n = gps_tracking_data.sats[c].c_n_1; + ao_log_data(&gps_log); + } } + } } } __xdata struct ao_task ao_gps_report_task; -__xdata struct ao_task ao_gps_tracking_report_task; void ao_gps_report_init(void) { ao_add_task(&ao_gps_report_task, ao_gps_report, "gps_report"); - ao_add_task(&ao_gps_tracking_report_task, ao_gps_tracking_report, "gps_tracking_report"); } diff --git a/src/core/ao_gps_report_mega.c b/src/core/ao_gps_report_mega.c index e3af4307..d13885dd 100644 --- a/src/core/ao_gps_report_mega.c +++ b/src/core/ao_gps_report_mega.c @@ -23,66 +23,62 @@ ao_gps_report_mega(void) { static __xdata struct ao_log_mega gps_log; static __xdata struct ao_telemetry_location gps_data; - uint8_t date_reported = 0; - - for (;;) { - ao_sleep(&ao_gps_data); - ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); - ao_mutex_put(&ao_gps_mutex); - - if (!(gps_data.flags & AO_GPS_VALID)) - continue; - - gps_log.tick = ao_gps_tick; - gps_log.type = AO_LOG_GPS_TIME; - gps_log.u.gps.latitude = gps_data.latitude; - gps_log.u.gps.longitude = gps_data.longitude; - gps_log.u.gps.altitude = gps_data.altitude; - - gps_log.u.gps.hour = gps_data.hour; - gps_log.u.gps.minute = gps_data.minute; - gps_log.u.gps.second = gps_data.second; - gps_log.u.gps.flags = gps_data.flags; - gps_log.u.gps.year = gps_data.year; - gps_log.u.gps.month = gps_data.month; - gps_log.u.gps.day = gps_data.day; - ao_log_mega(&gps_log); - } -} - -void -ao_gps_tracking_report_mega(void) -{ - static __xdata struct ao_log_mega gps_log; static __xdata struct ao_telemetry_satellite gps_tracking_data; + uint8_t date_reported = 0; + uint8_t new; uint8_t c, n, i; for (;;) { - ao_sleep(&ao_gps_tracking_data); + while (!(new = ao_gps_new)) + ao_sleep(&ao_gps_new); ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + if (new & AO_GPS_NEW_DATA) + ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + if (new & AO_GPS_NEW_TRACKING) + ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + ao_gps_new = 0; ao_mutex_put(&ao_gps_mutex); - if (!(n = gps_tracking_data.channels)) - continue; + if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { + gps_log.tick = ao_gps_tick; + gps_log.type = AO_LOG_GPS_TIME; + gps_log.u.gps.latitude = gps_data.latitude; + gps_log.u.gps.longitude = gps_data.longitude; + gps_log.u.gps.altitude = gps_data.altitude; - gps_log.type = AO_LOG_GPS_SAT; - gps_log.tick = ao_gps_tick; - i = 0; - for (c = 0; c < n; c++) - if ((gps_log.u.gps_sat.sats[i].svid = gps_tracking_data.sats[c].svid)) - { - gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1; - i++; - } - gps_log.u.gps_sat.channels = i; - ao_log_mega(&gps_log); + gps_log.u.gps.hour = gps_data.hour; + gps_log.u.gps.minute = gps_data.minute; + gps_log.u.gps.second = gps_data.second; + gps_log.u.gps.flags = gps_data.flags; + gps_log.u.gps.year = gps_data.year; + gps_log.u.gps.month = gps_data.month; + gps_log.u.gps.day = gps_data.day; + gps_log.u.gps.course = gps_data.course; + gps_log.u.gps.ground_speed = gps_data.ground_speed; + gps_log.u.gps.climb_rate = gps_data.climb_rate; + gps_log.u.gps.pdop = gps_data.pdop; + gps_log.u.gps.hdop = gps_data.hdop; + gps_log.u.gps.vdop = gps_data.vdop; + gps_log.u.gps.mode = gps_data.mode; + ao_log_mega(&gps_log); + } + if ((new & AO_GPS_NEW_TRACKING) && (n = gps_tracking_data.channels) != 0) { + gps_log.tick = ao_gps_tick; + gps_log.type = AO_LOG_GPS_SAT; + i = 0; + for (c = 0; c < n; c++) + if ((gps_log.u.gps_sat.sats[i].svid = gps_tracking_data.sats[c].svid)) + { + gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1; + i++; + } + gps_log.u.gps_sat.channels = i; + ao_log_mega(&gps_log); + } } } __xdata struct ao_task ao_gps_report_mega_task; -__xdata struct ao_task ao_gps_tracking_report_mega_task; void ao_gps_report_mega_init(void) @@ -90,7 +86,4 @@ ao_gps_report_mega_init(void) ao_add_task(&ao_gps_report_mega_task, ao_gps_report_mega, "gps_report"); - ao_add_task(&ao_gps_tracking_report_mega_task, - ao_gps_tracking_report_mega, - "gps_tracking_report"); } diff --git a/src/core/ao_gps_report_metrum.c b/src/core/ao_gps_report_metrum.c new file mode 100644 index 00000000..fa038976 --- /dev/null +++ b/src/core/ao_gps_report_metrum.c @@ -0,0 +1,97 @@ +/* + * Copyright © 2009 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include "ao_log.h" + +void +ao_gps_report_metrum(void) +{ + static __xdata struct ao_log_metrum gps_log; + static __xdata struct ao_telemetry_location gps_data; + static __xdata struct ao_telemetry_satellite gps_tracking_data; + uint8_t c, n, i, p, valid, packets; + uint8_t svid; + uint8_t date_reported = 0; + uint8_t new; + + for (;;) { + while (!(new = ao_gps_new)) + ao_sleep(&ao_gps_new); + ao_mutex_get(&ao_gps_mutex); + if (new & AO_GPS_NEW_DATA) + ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + if (new & AO_GPS_NEW_TRACKING) + ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + ao_gps_new = 0; + ao_mutex_put(&ao_gps_mutex); + + if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { + gps_log.tick = ao_gps_tick; + gps_log.type = AO_LOG_GPS_POS; + gps_log.u.gps.latitude = gps_data.latitude; + gps_log.u.gps.longitude = gps_data.longitude; + gps_log.u.gps.altitude = gps_data.altitude; + ao_log_metrum(&gps_log); + + gps_log.type = AO_LOG_GPS_TIME; + gps_log.u.gps_time.hour = gps_data.hour; + gps_log.u.gps_time.minute = gps_data.minute; + gps_log.u.gps_time.second = gps_data.second; + gps_log.u.gps_time.flags = gps_data.flags; + gps_log.u.gps_time.year = gps_data.year; + gps_log.u.gps_time.month = gps_data.month; + gps_log.u.gps_time.day = gps_data.day; + ao_log_metrum(&gps_log); + } + + if ((new & AO_GPS_NEW_TRACKING) && (n = gps_tracking_data.channels)) { + gps_log.type = AO_LOG_GPS_SAT; + gps_log.tick = ao_gps_tick; + i = 0; + for (c = 0; c < n; c++) { + svid = gps_tracking_data.sats[c].svid; + if (svid != 0) { + if (i == 4) { + gps_log.u.gps_sat.channels = i; + gps_log.u.gps_sat.more = 1; + ao_log_metrum(&gps_log); + i = 0; + } + gps_log.u.gps_sat.sats[i].svid = svid; + gps_log.u.gps_sat.sats[i].c_n = gps_tracking_data.sats[c].c_n_1; + i++; + } + } + if (i) { + gps_log.u.gps_sat.channels = i; + gps_log.u.gps_sat.more = 0; + ao_log_metrum(&gps_log); + } + } + } +} + +__xdata struct ao_task ao_gps_report_metrum_task; + +void +ao_gps_report_metrum_init(void) +{ + ao_add_task(&ao_gps_report_metrum_task, + ao_gps_report_metrum, + "gps_report"); +} diff --git a/src/core/ao_gps_show.c b/src/core/ao_gps_show.c new file mode 100644 index 00000000..3a05e35a --- /dev/null +++ b/src/core/ao_gps_show.c @@ -0,0 +1,39 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef AO_GPS_TEST +#include <ao.h> +#endif + +void +ao_gps_show(void) __reentrant +{ + uint8_t i; + ao_mutex_get(&ao_gps_mutex); + printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day); + printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second); + printf ("Lat/Lon: %ld %ld\n", (long) ao_gps_data.latitude, (long) ao_gps_data.longitude); + printf ("Alt: %d\n", ao_gps_data.altitude); + printf ("Flags: 0x%x\n", ao_gps_data.flags); + printf ("Sats: %d", ao_gps_tracking_data.channels); + for (i = 0; i < ao_gps_tracking_data.channels; i++) + printf (" %d %d", + ao_gps_tracking_data.sats[i].svid, + ao_gps_tracking_data.sats[i].c_n_1); + printf ("\ndone\n"); + ao_mutex_put(&ao_gps_mutex); +} diff --git a/src/core/ao_ignite.c b/src/core/ao_ignite.c index 74bd0c5a..9f2ec0a7 100644 --- a/src/core/ao_ignite.c +++ b/src/core/ao_ignite.c @@ -17,7 +17,11 @@ #include "ao.h" #include <ao_data.h> +#if AO_PYRO_NUM +#include <ao_pyro.h> +#endif +#if HAS_IGNITE __xdata struct ao_ignition ao_ignition[2]; void @@ -150,6 +154,8 @@ ao_igniter(void) } } +#endif + void ao_ignite_manual(void) { @@ -157,33 +163,50 @@ ao_ignite_manual(void) if (!ao_match_word("DoIt")) return; ao_cmd_white(); - if (ao_cmd_lex_c == 'm') { - if(ao_match_word("main")) - ao_igniter_fire(ao_igniter_main); - } else { - if(ao_match_word("drogue")) - ao_igniter_fire(ao_igniter_drogue); +#if HAS_IGNITE + if (ao_cmd_lex_c == 'm' && ao_match_word("main")) { + ao_igniter_fire(ao_igniter_main); + return; + } + if (ao_cmd_lex_c == 'd' && ao_match_word("drogue")) { + ao_igniter_fire(ao_igniter_drogue); + return; + } +#endif +#if AO_PYRO_NUM + if ('0' <= ao_cmd_lex_c && ao_cmd_lex_c <= '9') { + ao_pyro_manual(ao_cmd_lex_c - '0'); + return; } +#endif + ao_cmd_status = ao_cmd_syntax_error; } -static __code char * __code igniter_status_names[] = { +__code char * __code ao_igniter_status_names[] = { "unknown", "ready", "active", "open" }; +#if HAS_IGNITE void ao_ignite_print_status(enum ao_igniter igniter, __code char *name) __reentrant { enum ao_igniter_status status = ao_igniter_status(igniter); printf("Igniter: %6s Status: %s\n", name, - igniter_status_names[status]); + ao_igniter_status_names[status]); } +#endif void ao_ignite_test(void) { +#if HAS_IGNITE ao_ignite_print_status(ao_igniter_drogue, "drogue"); ao_ignite_print_status(ao_igniter_main, "main"); +#endif +#if AO_PYRO_NUM + ao_pyro_print_status(); +#endif } __code struct ao_cmds ao_ignite_cmds[] = { @@ -192,6 +215,7 @@ __code struct ao_cmds ao_ignite_cmds[] = { { 0, NULL }, }; +#if HAS_IGNITE __xdata struct ao_task ao_igniter_task; void @@ -200,11 +224,14 @@ ao_ignite_set_pins(void) ao_enable_output(AO_IGNITER_DROGUE_PORT, AO_IGNITER_DROGUE_PIN, AO_IGNITER_DROGUE, 0); ao_enable_output(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, AO_IGNITER_MAIN, 0); } +#endif void ao_igniter_init(void) { +#if HAS_IGNITE ao_ignite_set_pins(); - ao_cmd_register(&ao_ignite_cmds[0]); ao_add_task(&ao_igniter_task, ao_igniter, "igniter"); +#endif + ao_cmd_register(&ao_ignite_cmds[0]); } diff --git a/src/core/ao_int64.c b/src/core/ao_int64.c new file mode 100644 index 00000000..aa23dbe0 --- /dev/null +++ b/src/core/ao_int64.c @@ -0,0 +1,158 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao_int64.h> + +__pdata ao_int64_t *__data ao_64r, *__data ao_64a, *__data ao_64b; + +void ao_plus64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, __pdata ao_int64_t *b) __FATTR { + __LOCAL uint32_t t; + + r->high = a->high + b->high; + t = a->low + b->low; + if (t < a->low) + r->high++; + r->low = t; +} + +void ao_minus64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, __pdata ao_int64_t *b) __FATTR { + __LOCAL uint32_t t; + + r->high = a->high - b->high; + t = a->low - b->low; + if (t > a->low) + r->high--; + r->low = t; +} + +void ao_rshift64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, uint8_t d) __FATTR { + if (d < 32) { + r->low = a->low >> d; + if (d) + r->low |= a->high << (32 - d); + r->high = (int32_t) a->high >> d; + } else { + d &= 0x1f; + r->low = (int32_t) a->high >> d; + r->high = 0; + } +} + +void ao_lshift64(__pdata ao_int64_t *r, __pdata ao_int64_t *a, uint8_t d) __FATTR { + if (d < 32) { + r->high = a->high << d; + if (d) + r->high |= a->low >> (32 - d); + r->low = a->low << d; + } else { + d &= 0x1f; + r->high = a->low << d; + r->low = 0; + } +} + +static void ao_umul64_32_32(__ARG ao_int64_t *r, uint32_t a, uint32_t b) __reentrant { + __LOCAL uint32_t s; + __LOCAL ao_int64_t t; + r->low = (uint32_t) (uint16_t) a * (uint16_t) b; + r->high = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) (b >> 16); + + s = (uint32_t) (uint16_t) (a >> 16) * (uint16_t) b; + + t.high = s >> 16; + t.low = s << 16; + ao_plus64(r, r, &t); + + s = (uint32_t) (uint16_t) a * (uint16_t) (b >> 16); + + t.high = s >> 16; + t.low = s << 16; + ao_plus64(r, r, &t); +} + +void ao_neg64(__pdata ao_int64_t *r, __pdata ao_int64_t *a) __FATTR { + r->high = ~a->high; + if (!(r->low = ~a->low + 1)) + r->high++; +} + +void ao_mul64_32_32(__ARG ao_int64_t *r, int32_t a, int32_t b) __FATTR { + uint8_t negative = 0; + + if (a < 0) { + a = -a; + ++negative; + } + if (b < 0) { + b = -b; + --negative; + } + ao_umul64_32_32(r, a, b); + if (negative) + ao_neg64(r, r); +} + +static void ao_umul64(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG ao_int64_t *b) __reentrant { + __LOCAL ao_int64_t r2, r3; + + ao_umul64_32_32(&r2, a->high, b->low); + ao_umul64_32_32(&r3, a->low, b->high); + ao_umul64_32_32(r, a->low, b->low); + + r->high += r2.low + r3.low; +} + +static __ARG ao_int64_t ap, bp; + +void ao_mul64(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG ao_int64_t *b) __FATTR { + uint8_t negative = 0; + + if (ao_int64_negativep(a)) { + ao_neg64(&ap, a); + a = ≈ + ++negative; + } + if (ao_int64_negativep(b)) { + ao_neg64(&bp, b); + b = &bp; + --negative; + } + ao_umul64(r, a, b); + if (negative) + ao_neg64(r, r); +} + +static void ao_umul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, uint16_t b) __reentrant { + __LOCAL uint32_t h; + + h = a->high * b; + ao_umul64_32_32(r, a->low, b); + r->high += h; +} + +void ao_mul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG uint16_t b) __FATTR { + uint8_t negative = 0; + + if ((int32_t) a->high < 0) { + ao_neg64(&ap, a); + a = ≈ + negative++; + } else + ao_umul64_64_16(r, a, b); + if (negative) + ao_neg64(r, r); +} diff --git a/src/core/ao_int64.h b/src/core/ao_int64.h new file mode 100644 index 00000000..b16db58c --- /dev/null +++ b/src/core/ao_int64.h @@ -0,0 +1,48 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_INT64_H_ +#define _AO_INT64_H_ + +#include <stdint.h> + +typedef struct { + uint32_t high; + uint32_t low; +} ao_int64_t; + +#define __FATTR +#define __ARG __pdata +#define __LOCAL static __pdata + +void ao_plus64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, __pdata ao_int64_t *ao_64b) __FATTR; +void ao_minus64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, __pdata ao_int64_t *ao_64b) __FATTR; +void ao_neg64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a) __FATTR; +void ao_rshift64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, uint8_t d) __FATTR; +void ao_lshift64(__pdata ao_int64_t *ao_64r, __pdata ao_int64_t *ao_64a, uint8_t d) __FATTR; +void ao_mul64_32_32(__ARG ao_int64_t *r, __ARG int32_t a, __ARG int32_t b) __FATTR; +void ao_mul64_64_16(__ARG ao_int64_t *r, __ARG ao_int64_t *a, __ARG uint16_t b) __FATTR; +void ao_mul64(__ARG ao_int64_t * __ARG r, __ARG ao_int64_t * __ARG a, __ARG ao_int64_t *__ARG b) __FATTR; + +#define ao_int64_init32(r, a) (((r)->high = 0), (r)->low = (a)) +#define ao_int64_init64(r, a, b) (((r)->high = (a)), (r)->low = (b)) + +#define ao_cast64(a) (((int64_t) (a)->high << 32) | (a)->low) + +#define ao_int64_negativep(a) (((int32_t) (a)->high) < 0) + +#endif /* _AO_INT64_H_ */ diff --git a/src/core/ao_kalman.c b/src/core/ao_kalman.c index 59ffd8b2..7fd4f889 100644 --- a/src/core/ao_kalman.c +++ b/src/core/ao_kalman.c @@ -40,9 +40,9 @@ static __pdata int32_t ao_k_accel; __pdata int16_t ao_height; __pdata int16_t ao_speed; __pdata int16_t ao_accel; -__pdata int16_t ao_max_height; +__xdata int16_t ao_max_height; static __pdata int32_t ao_avg_height_scaled; -__pdata int16_t ao_avg_height; +__xdata int16_t ao_avg_height; __pdata int16_t ao_error_h; __pdata int16_t ao_error_h_sq_avg; @@ -292,7 +292,4 @@ ao_kalman(void) else #endif ao_avg_height = (ao_avg_height_scaled + 63) >> 7; -#ifdef AO_FLIGHT_TEST - ao_sample_prev_tick = ao_sample_tick; -#endif } diff --git a/src/core/ao_log.c b/src/core/ao_log.c index 7884ec3c..701c81ab 100644 --- a/src/core/ao_log.c +++ b/src/core/ao_log.c @@ -17,6 +17,7 @@ #include "ao.h" #include <ao_log.h> +#include <ao_config.h> __pdata uint32_t ao_log_current_pos; __pdata uint32_t ao_log_end_pos; @@ -48,7 +49,7 @@ static __xdata struct ao_log_erase erase; static uint32_t ao_log_erase_pos(uint8_t i) { - return i * sizeof (struct ao_log_erase) + AO_STORAGE_ERASE_LOG; + return i * sizeof (struct ao_log_erase) + AO_CONFIG_MAX_SIZE; } void @@ -56,14 +57,14 @@ ao_log_write_erase(uint8_t pos) { erase.unused = 0x00; erase.flight = ao_flight_number; - ao_storage_write(ao_log_erase_pos(pos), &erase, sizeof (erase)); - ao_storage_flush(); + ao_config_write(ao_log_erase_pos(pos), &erase, sizeof (erase)); + ao_config_flush(); } static void ao_log_read_erase(uint8_t pos) { - ao_storage_read(ao_log_erase_pos(pos), &erase, sizeof (erase)); + ao_config_read(ao_log_erase_pos(pos), &erase, sizeof (erase)); } @@ -87,7 +88,7 @@ ao_log_erase_mark(void) static uint8_t ao_log_slots() { - return (uint8_t) (ao_storage_config / ao_config.flight_log_max); + return (uint8_t) (ao_storage_log_max / ao_config.flight_log_max); } uint32_t @@ -278,6 +279,11 @@ ao_log_init(void) ao_cmd_register(&ao_log_cmds[0]); +#ifndef HAS_ADC +#error Define HAS_ADC for ao_log.c +#endif +#if HAS_ADC /* Create a task to log events to eeprom */ ao_add_task(&ao_log_task, ao_log, "log"); +#endif } diff --git a/src/core/ao_log.h b/src/core/ao_log.h index a68a40dd..09f31188 100644 --- a/src/core/ao_log.h +++ b/src/core/ao_log.h @@ -44,6 +44,9 @@ extern __pdata enum ao_flight_state ao_log_state; #define AO_LOG_FORMAT_TELEMETRY 3 /* 32 byte ao_telemetry records */ #define AO_LOG_FORMAT_TELESCIENCE 4 /* 32 byte typed telescience records */ #define AO_LOG_FORMAT_TELEMEGA 5 /* 32 byte typed telemega records */ +#define AO_LOG_FORMAT_EASYMINI 6 /* 16-byte MS5607 baro only, 3.0V supply */ +#define AO_LOG_FORMAT_TELEMETRUM 7 /* 16-byte typed telemetrum records */ +#define AO_LOG_FORMAT_TELEMINI 8 /* 16-byte MS5607 baro only, 3.3V supply */ #define AO_LOG_FORMAT_NONE 127 /* No log at all */ extern __code uint8_t ao_log_format; @@ -134,6 +137,7 @@ ao_log_full(void); #define AO_LOG_GPS_ALT 'H' #define AO_LOG_GPS_SAT 'V' #define AO_LOG_GPS_DATE 'Y' +#define AO_LOG_GPS_POS 'P' #define AO_LOG_POS_NONE (~0UL) @@ -235,7 +239,8 @@ struct ao_log_mega { int16_t v_pbatt; /* 6 */ int16_t n_sense; /* 8 */ int16_t sense[10]; /* 10 */ - } volt; /* 30 */ + uint16_t pyro; /* 30 */ + } volt; /* 32 */ /* AO_LOG_GPS_TIME */ struct { int32_t latitude; /* 4 */ @@ -248,8 +253,14 @@ struct ao_log_mega { uint8_t year; /* 18 */ uint8_t month; /* 19 */ uint8_t day; /* 20 */ - uint8_t pad; /* 21 */ - } gps; /* 22 */ + uint8_t course; /* 21 */ + uint16_t ground_speed; /* 22 */ + int16_t climb_rate; /* 24 */ + uint8_t pdop; /* 26 */ + uint8_t hdop; /* 27 */ + uint8_t vdop; /* 28 */ + uint8_t mode; /* 29 */ + } gps; /* 30 */ /* AO_LOG_GPS_SAT */ struct { uint16_t channels; /* 4 */ @@ -261,6 +272,98 @@ struct ao_log_mega { } u; }; +struct ao_log_metrum { + char type; /* 0 */ + uint8_t csum; /* 1 */ + uint16_t tick; /* 2 */ + union { /* 4 */ + /* AO_LOG_FLIGHT */ + struct { + uint16_t flight; /* 4 */ + int16_t ground_accel; /* 6 */ + uint32_t ground_pres; /* 8 */ + uint32_t ground_temp; /* 12 */ + } flight; /* 16 */ + /* AO_LOG_STATE */ + struct { + uint16_t state; /* 4 */ + uint16_t reason; /* 6 */ + } state; /* 8 */ + /* AO_LOG_SENSOR */ + struct { + uint32_t pres; /* 4 */ + uint32_t temp; /* 8 */ + int16_t accel; /* 12 */ + } sensor; /* 14 */ + /* AO_LOG_TEMP_VOLT */ + struct { + int16_t v_batt; /* 4 */ + int16_t sense_a; /* 6 */ + int16_t sense_m; /* 8 */ + } volt; /* 10 */ + /* AO_LOG_GPS_POS */ + struct { + int32_t latitude; /* 4 */ + int32_t longitude; /* 8 */ + int16_t altitude; /* 12 */ + } gps; /* 14 */ + /* AO_LOG_GPS_TIME */ + struct { + uint8_t hour; /* 4 */ + uint8_t minute; /* 5 */ + uint8_t second; /* 6 */ + uint8_t flags; /* 7 */ + uint8_t year; /* 8 */ + uint8_t month; /* 9 */ + uint8_t day; /* 10 */ + uint8_t pad; /* 11 */ + } gps_time; /* 12 */ + /* AO_LOG_GPS_SAT (up to three packets) */ + struct { + uint8_t channels; /* 4 */ + uint8_t more; /* 5 */ + struct { + uint8_t svid; + uint8_t c_n; + } sats[4]; /* 6 */ + } gps_sat; /* 14 */ + uint8_t raw[12]; /* 4 */ + } u; /* 16 */ +}; + +struct ao_log_mini { + char type; /* 0 */ + uint8_t csum; /* 1 */ + uint16_t tick; /* 2 */ + union { /* 4 */ + /* AO_LOG_FLIGHT */ + struct { + uint16_t flight; /* 4 */ + uint16_t r6; + uint32_t ground_pres; /* 8 */ + } flight; + /* AO_LOG_STATE */ + struct { + uint16_t state; /* 4 */ + uint16_t reason; /* 6 */ + } state; + /* AO_LOG_SENSOR */ + struct { + uint8_t pres[3]; /* 4 */ + uint8_t temp[3]; /* 7 */ + int16_t sense_a; /* 10 */ + int16_t sense_m; /* 12 */ + int16_t v_batt; /* 14 */ + } sensor; /* 16 */ + } u; /* 16 */ +}; /* 16 */ + +#define ao_log_pack24(dst,value) do { \ + (dst)[0] = (value); \ + (dst)[1] = (value) >> 8; \ + (dst)[2] = (value) >> 16; \ + } while (0) + /* Write a record to the eeprom log */ uint8_t ao_log_data(__xdata struct ao_log_record *log) __reentrant; @@ -268,7 +371,16 @@ ao_log_data(__xdata struct ao_log_record *log) __reentrant; uint8_t ao_log_mega(__xdata struct ao_log_mega *log) __reentrant; +uint8_t +ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant; + +uint8_t +ao_log_mini(__xdata struct ao_log_mini *log) __reentrant; + void ao_log_flush(void); +void +ao_gps_report_metrum_init(void); + #endif /* _AO_LOG_H_ */ diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c index abf953a6..768947d5 100644 --- a/src/core/ao_log_mega.c +++ b/src/core/ao_log_mega.c @@ -65,6 +65,7 @@ ao_log_dump_check_data(void) return 1; } +#if HAS_ADC static __data uint8_t ao_log_data_pos; /* a hack to make sure that ao_log_megas fill the eeprom block in even units */ @@ -151,6 +152,7 @@ ao_log(void) log.u.volt.n_sense = AO_ADC_NUM_SENSE; for (i = 0; i < AO_ADC_NUM_SENSE; i++) log.u.volt.sense[i] = ao_data_ring[ao_log_data_pos].adc.sense[i]; + log.u.volt.pyro = ao_pyro_fired; ao_log_mega(&log); next_other = log.tick + AO_OTHER_INTERVAL; } @@ -181,6 +183,7 @@ ao_log(void) ao_sleep(&ao_log_running); } } +#endif uint16_t ao_log_flight(uint8_t slot) diff --git a/src/core/ao_log_metrum.c b/src/core/ao_log_metrum.c new file mode 100644 index 00000000..43441e7a --- /dev/null +++ b/src/core/ao_log_metrum.c @@ -0,0 +1,175 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include <ao_log.h> +#include <ao_data.h> +#include <ao_flight.h> + +static __xdata uint8_t ao_log_mutex; +static __xdata struct ao_log_metrum log; + +__code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMETRUM; + +static uint8_t +ao_log_csum(__xdata uint8_t *b) __reentrant +{ + uint8_t sum = 0x5a; + uint8_t i; + + for (i = 0; i < sizeof (struct ao_log_metrum); i++) + sum += *b++; + return -sum; +} + +uint8_t +ao_log_metrum(__xdata struct ao_log_metrum *log) __reentrant +{ + uint8_t wrote = 0; + /* set checksum */ + log->csum = 0; + log->csum = ao_log_csum((__xdata uint8_t *) log); + ao_mutex_get(&ao_log_mutex); { + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_stop(); + if (ao_log_running) { + wrote = 1; + ao_storage_write(ao_log_current_pos, + log, + sizeof (struct ao_log_metrum)); + ao_log_current_pos += sizeof (struct ao_log_metrum); + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + +static uint8_t +ao_log_dump_check_data(void) +{ + if (ao_log_csum((uint8_t *) &log) != 0) + return 0; + return 1; +} + +#if HAS_ADC +static __data uint8_t ao_log_data_pos; + +/* a hack to make sure that ao_log_metrums fill the eeprom block in even units */ +typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_metrum))] ; + +#ifndef AO_SENSOR_INTERVAL_ASCENT +#define AO_SENSOR_INTERVAL_ASCENT 1 +#define AO_SENSOR_INTERVAL_DESCENT 10 +#define AO_OTHER_INTERVAL 32 +#endif + +void +ao_log(void) +{ + __pdata uint16_t next_sensor, next_other; + uint8_t i; + + ao_storage_setup(); + + ao_log_scan(); + + while (!ao_log_running) + ao_sleep(&ao_log_running); + +#if HAS_FLIGHT + log.type = AO_LOG_FLIGHT; + log.tick = ao_sample_tick; +#if HAS_ACCEL + log.u.flight.ground_accel = ao_ground_accel; +#endif + log.u.flight.ground_pres = ao_ground_pres; + log.u.flight.flight = ao_flight_number; + ao_log_metrum(&log); +#endif + + /* Write the whole contents of the ring to the log + * when starting up. + */ + ao_log_data_pos = ao_data_ring_next(ao_data_head); + next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; + ao_log_state = ao_flight_startup; + for (;;) { + /* Write samples to EEPROM */ + while (ao_log_data_pos != ao_data_head) { + log.tick = ao_data_ring[ao_log_data_pos].tick; + if ((int16_t) (log.tick - next_sensor) >= 0) { + log.type = AO_LOG_SENSOR; +#if HAS_MS5607 + log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607_raw.pres; + log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607_raw.temp; +#endif + log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]); + ao_log_metrum(&log); + if (ao_log_state <= ao_flight_coast) + next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; + else + next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; + } + if ((int16_t) (log.tick - next_other) >= 0) { + log.type = AO_LOG_TEMP_VOLT; + log.u.volt.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; + log.u.volt.sense_a = ao_data_ring[ao_log_data_pos].adc.sense_a; + log.u.volt.sense_m = ao_data_ring[ao_log_data_pos].adc.sense_m; + ao_log_metrum(&log); + next_other = log.tick + AO_OTHER_INTERVAL; + } + ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); + } +#if HAS_FLIGHT + /* Write state change to EEPROM */ + if (ao_flight_state != ao_log_state) { + ao_log_state = ao_flight_state; + log.type = AO_LOG_STATE; + log.tick = ao_time(); + log.u.state.state = ao_log_state; + log.u.state.reason = 0; + ao_log_metrum(&log); + + if (ao_log_state == ao_flight_landed) + ao_log_stop(); + } +#endif + + ao_log_flush(); + + /* Wait for a while */ + ao_delay(AO_MS_TO_TICKS(100)); + + /* Stop logging when told to */ + while (!ao_log_running) + ao_sleep(&ao_log_running); + } +} +#endif + +uint16_t +ao_log_flight(uint8_t slot) +{ + if (!ao_storage_read(ao_log_pos(slot), + &log, + sizeof (struct ao_log_metrum))) + return 0; + + if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) + return log.u.flight.flight; + return 0; +} diff --git a/src/micropeak/ao_log_micro.c b/src/core/ao_log_micro.c index d665efb5..d665efb5 100644 --- a/src/micropeak/ao_log_micro.c +++ b/src/core/ao_log_micro.c diff --git a/src/micropeak/ao_log_micro.h b/src/core/ao_log_micro.h index 976852ee..976852ee 100644 --- a/src/micropeak/ao_log_micro.h +++ b/src/core/ao_log_micro.h diff --git a/src/core/ao_log_mini.c b/src/core/ao_log_mini.c new file mode 100644 index 00000000..99a85982 --- /dev/null +++ b/src/core/ao_log_mini.c @@ -0,0 +1,162 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include <ao_log.h> +#include <ao_data.h> +#include <ao_flight.h> + +static __xdata uint8_t ao_log_mutex; +static __xdata struct ao_log_mini log; + +__code uint8_t ao_log_format = AO_LOG_FORMAT; + +static uint8_t +ao_log_csum(__xdata uint8_t *b) __reentrant +{ + uint8_t sum = 0x5a; + uint8_t i; + + for (i = 0; i < sizeof (struct ao_log_mini); i++) + sum += *b++; + return -sum; +} + +uint8_t +ao_log_mini(__xdata struct ao_log_mini *log) __reentrant +{ + uint8_t wrote = 0; + /* set checksum */ + log->csum = 0; + log->csum = ao_log_csum((__xdata uint8_t *) log); + ao_mutex_get(&ao_log_mutex); { + if (ao_log_current_pos >= ao_log_end_pos && ao_log_running) + ao_log_stop(); + if (ao_log_running) { + wrote = 1; + ao_storage_write(ao_log_current_pos, + log, + sizeof (struct ao_log_mini)); + ao_log_current_pos += sizeof (struct ao_log_mini); + } + } ao_mutex_put(&ao_log_mutex); + return wrote; +} + +static uint8_t +ao_log_dump_check_data(void) +{ + if (ao_log_csum((uint8_t *) &log) != 0) + return 0; + return 1; +} + +static __data uint8_t ao_log_data_pos; + +/* a hack to make sure that ao_log_minis fill the eeprom block in even units */ +typedef uint8_t check_log_size[1-(256 % sizeof(struct ao_log_mini))] ; + +#ifndef AO_SENSOR_INTERVAL_ASCENT +#define AO_SENSOR_INTERVAL_ASCENT 1 +#define AO_SENSOR_INTERVAL_DESCENT 10 +#endif + +void +ao_log(void) +{ + __pdata uint16_t next_sensor, next_other; + + ao_storage_setup(); + + ao_log_scan(); + + while (!ao_log_running) + ao_sleep(&ao_log_running); + +#if HAS_FLIGHT + log.type = AO_LOG_FLIGHT; + log.tick = ao_sample_tick; + log.u.flight.flight = ao_flight_number; + log.u.flight.ground_pres = ao_ground_pres; + ao_log_mini(&log); +#endif + + /* Write the whole contents of the ring to the log + * when starting up. + */ + ao_log_data_pos = ao_data_ring_next(ao_data_head); + next_other = next_sensor = ao_data_ring[ao_log_data_pos].tick; + ao_log_state = ao_flight_startup; + for (;;) { + /* Write samples to EEPROM */ + while (ao_log_data_pos != ao_data_head) { + log.tick = ao_data_ring[ao_log_data_pos].tick; + if ((int16_t) (log.tick - next_sensor) >= 0) { + log.type = AO_LOG_SENSOR; + ao_log_pack24(log.u.sensor.pres, + ao_data_ring[ao_log_data_pos].ms5607_raw.pres); + ao_log_pack24(log.u.sensor.temp, + ao_data_ring[ao_log_data_pos].ms5607_raw.temp); + log.u.sensor.sense_a = ao_data_ring[ao_log_data_pos].adc.sense_a; + log.u.sensor.sense_m = ao_data_ring[ao_log_data_pos].adc.sense_m; + log.u.sensor.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; + ao_log_mini(&log); + if (ao_log_state <= ao_flight_coast) + next_sensor = log.tick + AO_SENSOR_INTERVAL_ASCENT; + else + next_sensor = log.tick + AO_SENSOR_INTERVAL_DESCENT; + } + ao_log_data_pos = ao_data_ring_next(ao_log_data_pos); + } +#if HAS_FLIGHT + /* Write state change to EEPROM */ + if (ao_flight_state != ao_log_state) { + ao_log_state = ao_flight_state; + log.type = AO_LOG_STATE; + log.tick = ao_time(); + log.u.state.state = ao_log_state; + log.u.state.reason = 0; + ao_log_mini(&log); + + if (ao_log_state == ao_flight_landed) + ao_log_stop(); + } +#endif + + ao_log_flush(); + + /* Wait for a while */ + ao_delay(AO_MS_TO_TICKS(100)); + + /* Stop logging when told to */ + while (!ao_log_running) + ao_sleep(&ao_log_running); + } +} + +uint16_t +ao_log_flight(uint8_t slot) +{ + if (!ao_storage_read(ao_log_pos(slot), + &log, + sizeof (struct ao_log_mini))) + return 0; + + if (ao_log_dump_check_data() && log.type == AO_LOG_FLIGHT) + return log.u.flight.flight; + return 0; +} diff --git a/src/core/ao_log_telem.c b/src/core/ao_log_telem.c index 23ebf7dd..095aca37 100644 --- a/src/core/ao_log_telem.c +++ b/src/core/ao_log_telem.c @@ -23,7 +23,7 @@ __code uint8_t ao_log_format = AO_LOG_FORMAT_TELEMETRY; static __data uint8_t ao_log_monitor_pos; __pdata enum ao_flight_state ao_flight_state; -__pdata int16_t ao_max_height; /* max of ao_height */ +__xdata int16_t ao_max_height; /* max of ao_height */ __pdata int16_t sense_d, sense_m; __pdata uint8_t ao_igniter_present; diff --git a/src/core/ao_log_tiny.c b/src/core/ao_log_tiny.c index 492658ea..67767dc9 100644 --- a/src/core/ao_log_tiny.c +++ b/src/core/ao_log_tiny.c @@ -105,7 +105,7 @@ ao_log(void) */ ao_sleep(DATA_TO_XDATA(&ao_sample_data)); while (ao_log_data != ao_sample_data) { - sum += ao_data_ring[ao_log_data].adc.pres; + sum += ao_data_pres(&ao_data_ring[ao_log_data]); count++; ao_log_data = ao_data_ring_next(ao_log_data); } diff --git a/src/micropeak/ao_microflight.c b/src/core/ao_microflight.c index 714bb90a..f680e400 100644 --- a/src/micropeak/ao_microflight.c +++ b/src/core/ao_microflight.c @@ -33,7 +33,7 @@ ao_microsample(void) ao_microkalman_correct(); } -#define NUM_PA_HIST 16 +#define NUM_PA_HIST (GROUND_AVG) #define SKIP_PA_HIST(i,j) (((i) + (j)) & (NUM_PA_HIST - 1)) @@ -60,11 +60,11 @@ ao_microflight(void) h = 0; for (;;) { time += SAMPLE_SLEEP; - if (sample_count == 0) + if ((sample_count & 0x1f) == 0) ao_led_on(AO_LED_REPORT); ao_delay_until(time); ao_microsample(); - if (sample_count == 0) + if ((sample_count & 0x1f) == 0) ao_led_off(AO_LED_REPORT); pa_hist[h] = pa; h = SKIP_PA_HIST(h,1); @@ -85,10 +85,10 @@ ao_microflight(void) } } - /* Go back and find the first sample a decent interval above the ground */ + /* Go back and find the last sample close to 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) + for (i = SKIP_PA_HIST(h,-2); i != SKIP_PA_HIST(h,2); i = SKIP_PA_HIST(i,-2)) { + if (pa_hist[i] >= pa_min) break; } diff --git a/src/micropeak/ao_microkalman.c b/src/core/ao_microkalman.c index 0684ea2b..0684ea2b 100644 --- a/src/micropeak/ao_microkalman.c +++ b/src/core/ao_microkalman.c diff --git a/src/core/ao_product.c b/src/core/ao_product.c index ec91b978..b9327bac 100644 --- a/src/core/ao_product.c +++ b/src/core/ao_product.c @@ -27,6 +27,12 @@ const char ao_product[] = AO_iProduct_STRING; #define LE_WORD(x) ((x)&0xFF),((uint8_t) (((uint16_t) (x))>>8)) #if HAS_USB + +/* Maximum power in mA */ +#ifndef AO_USB_MAX_POWER +#define AO_USB_MAX_POWER 100 +#endif + #include "ao_usb.h" /* USB descriptors in one giant block of bytes */ AO_ROMCONFIG_SYMBOL(0x00aa) uint8_t ao_usb_descriptors [] = @@ -55,7 +61,7 @@ AO_ROMCONFIG_SYMBOL(0x00aa) uint8_t ao_usb_descriptors [] = 0x01, /* bConfigurationValue */ 0x00, /* iConfiguration */ 0xC0, /* bmAttributes */ - 0x32, /* bMaxPower */ + AO_USB_MAX_POWER >> 1, /* bMaxPower, 2mA units */ /* Control class interface */ 0x09, diff --git a/src/core/ao_pyro.c b/src/core/ao_pyro.c index aac8fda5..a260aa99 100644 --- a/src/core/ao_pyro.c +++ b/src/core/ao_pyro.c @@ -15,10 +15,12 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */ +#ifndef AO_FLIGHT_TEST #include <ao.h> -#include <ao_pyro.h> #include <ao_sample.h> #include <ao_flight.h> +#endif +#include <ao_pyro.h> #if IS_COMPANION #include <ao_companion.h> @@ -31,6 +33,42 @@ #define ao_lowbit(x) ((x) & (-x)) +#ifndef AO_FLIGHT_TEST +enum ao_igniter_status +ao_pyro_status(uint8_t p) +{ + __xdata struct ao_data packet; + __pdata int16_t value; + + ao_arch_critical( + ao_data_get(&packet); + ); + + value = (AO_IGNITER_CLOSED>>1); + value = AO_SENSE_PYRO(&packet, p); + if (value < AO_IGNITER_OPEN) + return ao_igniter_open; + else if (value > AO_IGNITER_CLOSED) + return ao_igniter_ready; + else + return ao_igniter_unknown; +} + +void +ao_pyro_print_status(void) +{ + uint8_t p; + + for(p = 0; p < AO_PYRO_NUM; p++) { + enum ao_igniter_status status = ao_pyro_status(p); + printf("Igniter: %d Status: %s\n", + p, ao_igniter_status_names[status]); + } +} +#endif + +uint16_t ao_pyro_fired; + /* * Given a pyro structure, figure out * if the current flight state satisfies all @@ -75,13 +113,13 @@ ao_pyro_ready(struct ao_pyro *pyro) continue; break; -#if HAS_ORIENT +#if HAS_GYRO case ao_pyro_orient_less: - if (ao_orient <= pyro->orient_less) + if (ao_sample_orient <= pyro->orient_less) continue; break; case ao_pyro_orient_greater: - if (ao_orient >= pyro->orient_greater) + if (ao_sample_orient >= pyro->orient_greater) continue; break; #endif @@ -130,107 +168,116 @@ ao_pyro_ready(struct ao_pyro *pyro) return TRUE; } -#define ao_pyro_fire_port(port, bit, pin) do { \ - ao_gpio_set(port, bit, pin, 1); \ - ao_delay(AO_MS_TO_TICKS(50)); \ - ao_gpio_set(port, bit, pin, 0); \ - } while (0) - - +#ifndef AO_FLIGHT_TEST static void -ao_pyro_fire(uint8_t p) +ao_pyro_pin_set(uint8_t p, uint8_t v) { switch (p) { #if AO_PYRO_NUM > 0 - case 0: ao_pyro_fire_port(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0); break; + case 0: ao_gpio_set(AO_PYRO_PORT_0, AO_PYRO_PIN_0, AO_PYRO_0, v); break; #endif #if AO_PYRO_NUM > 1 - case 1: ao_pyro_fire_port(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1); break; + case 1: ao_gpio_set(AO_PYRO_PORT_1, AO_PYRO_PIN_1, AO_PYRO_1, v); break; #endif #if AO_PYRO_NUM > 2 - case 2: ao_pyro_fire_port(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2); break; + case 2: ao_gpio_set(AO_PYRO_PORT_2, AO_PYRO_PIN_2, AO_PYRO_2, v); break; #endif #if AO_PYRO_NUM > 3 - case 3: ao_pyro_fire_port(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3); break; + case 3: ao_gpio_set(AO_PYRO_PORT_3, AO_PYRO_PIN_3, AO_PYRO_3, v); break; #endif #if AO_PYRO_NUM > 4 - case 4: ao_pyro_fire_port(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4); break; + case 4: ao_gpio_set(AO_PYRO_PORT_4, AO_PYRO_PIN_4, AO_PYRO_4, v); break; #endif #if AO_PYRO_NUM > 5 - case 5: ao_pyro_fire_port(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5); break; + case 5: ao_gpio_set(AO_PYRO_PORT_5, AO_PYRO_PIN_5, AO_PYRO_5, v); break; #endif #if AO_PYRO_NUM > 6 - case 6: ao_pyro_fire_port(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6); break; + case 6: ao_gpio_set(AO_PYRO_PORT_6, AO_PYRO_PIN_6, AO_PYRO_6, v); break; #endif #if AO_PYRO_NUM > 7 - case 7: ao_pyro_fire_port(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7); break; + case 7: ao_gpio_set(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, v); break; #endif default: break; } - ao_delay(AO_MS_TO_TICKS(50)); } +#endif uint8_t ao_pyro_wakeup; static void -ao_pyro(void) +ao_pyro_pins_fire(uint16_t fire) { - uint8_t p, any_waiting; - struct ao_pyro *pyro; + uint8_t p; - ao_config_get(); - while (ao_flight_state < ao_flight_boost) - ao_sleep(&ao_flight_state); + for (p = 0; p < AO_PYRO_NUM; p++) { + if (fire & (1 << p)) + ao_pyro_pin_set(p, 1); + } + ao_delay(AO_MS_TO_TICKS(50)); + for (p = 0; p < AO_PYRO_NUM; p++) { + if (fire & (1 << p)) { + ao_pyro_pin_set(p, 0); + ao_config.pyro[p].fired = 1; + ao_pyro_fired |= (1 << p); + } + } + ao_delay(AO_MS_TO_TICKS(50)); +} - for (;;) { - ao_alarm(AO_MS_TO_TICKS(100)); - ao_sleep(&ao_pyro_wakeup); - ao_clear_alarm(); - any_waiting = 0; - for (p = 0; p < AO_PYRO_NUM; p++) { - pyro = &ao_config.pyro[p]; +static uint8_t +ao_pyro_check(void) +{ + struct ao_pyro *pyro; + uint8_t p, any_waiting; + uint16_t fire = 0; + + any_waiting = 0; + for (p = 0; p < AO_PYRO_NUM; p++) { + pyro = &ao_config.pyro[p]; - /* Ignore igniters which have already fired - */ - if (pyro->fired) - continue; + /* Ignore igniters which have already fired + */ + if (pyro->fired) + continue; - /* Ignore disabled igniters - */ - if (!pyro->flags) - continue; + /* Ignore disabled igniters + */ + if (!pyro->flags) + continue; - any_waiting = 1; - /* Check pyro state to see if it shoule fire - */ - if (!pyro->delay_done) { - if (!ao_pyro_ready(pyro)) - continue; - - /* If there's a delay set, then remember when - * it expires - */ - if (pyro->flags & ao_pyro_delay) - pyro->delay_done = ao_time() + pyro->delay; - } + any_waiting = 1; + /* Check pyro state to see if it should fire + */ + if (!pyro->delay_done) { + if (!ao_pyro_ready(pyro)) + continue; - /* Check to see if we're just waiting for - * the delay to expire + /* If there's a delay set, then remember when + * it expires */ - if (pyro->delay_done) { - if ((int16_t) (ao_time() - pyro->delay_done) < 0) - continue; + if (pyro->flags & ao_pyro_delay) { + pyro->delay_done = ao_time() + pyro->delay; + if (!pyro->delay_done) + pyro->delay_done = 1; } + } - ao_pyro_fire(p); + /* Check to see if we're just waiting for + * the delay to expire + */ + if (pyro->delay_done) { + if ((int16_t) (ao_time() - pyro->delay_done) < 0) + continue; } - if (!any_waiting) - break; + + fire |= (1 << p); } - ao_exit(); -} -__xdata struct ao_task ao_pyro_task; + if (fire) + ao_pyro_pins_fire(fire); + + return any_waiting; +} #define NO_VALUE 0xff @@ -263,7 +310,7 @@ const struct { { "h<", ao_pyro_height_less, offsetof(struct ao_pyro, height_less), HELP("height less (m)") }, { "h>", ao_pyro_height_greater, offsetof(struct ao_pyro, height_greater), HELP("height greater (m)") }, -#if HAS_ORIENT +#if HAS_GYRO { "o<", ao_pyro_orient_less, offsetof(struct ao_pyro, orient_less), HELP("orient less (deg)") }, { "o>", ao_pyro_orient_greater, offsetof(struct ao_pyro, orient_greater), HELP("orient greater (deg)") }, #endif @@ -283,6 +330,34 @@ const struct { { "", ao_pyro_none, NO_VALUE, HELP(NULL) }, }; +#define NUM_PYRO_VALUES (sizeof ao_pyro_values / sizeof ao_pyro_values[0]) + +#ifndef AO_FLIGHT_TEST +static void +ao_pyro(void) +{ + uint8_t any_waiting; + + ao_config_get(); + while (ao_flight_state < ao_flight_boost) + ao_sleep(&ao_flight_state); + + for (;;) { + ao_alarm(AO_MS_TO_TICKS(100)); + ao_sleep(&ao_pyro_wakeup); + ao_clear_alarm(); + if (ao_flight_state >= ao_flight_landed) + break; + any_waiting = ao_pyro_check(); + if (!any_waiting) + break; + } + ao_exit(); +} + +__xdata struct ao_task ao_pyro_task; + + static void ao_pyro_print_name(uint8_t v) { @@ -400,25 +475,17 @@ ao_pyro_set(void) _ao_config_edit_finish(); } -static void -ao_pyro_manual(void) +void +ao_pyro_manual(uint8_t p) { - ao_cmd_white(); - if (!ao_match_word("DoIt")) - return; - ao_cmd_white(); - ao_cmd_decimal(); - if (ao_cmd_lex_i < 0 || AO_PYRO_NUM <= ao_cmd_lex_i) + printf ("ao_pyro_manual %d\n", p); + if (p >= AO_PYRO_NUM) { + ao_cmd_status = ao_cmd_syntax_error; return; - ao_pyro_fire(ao_cmd_lex_i); - + } + ao_pyro_pins_fire(1 << p); } -const struct ao_cmds ao_pyro_cmds[] = { - { ao_pyro_manual, "P DoIt <n>\0Fire igniter" }, - { 0, NULL } -}; - void ao_pyro_init(void) { @@ -446,6 +513,6 @@ ao_pyro_init(void) #if AO_PYRO_NUM > 7 ao_enable_output(AO_PYRO_PORT_7, AO_PYRO_PIN_7, AO_PYRO_7, 0); #endif - ao_cmd_register(&ao_pyro_cmds[0]); ao_add_task(&ao_pyro_task, ao_pyro, "pyro"); } +#endif diff --git a/src/core/ao_pyro.h b/src/core/ao_pyro.h index cde850ad..0c5642d6 100644 --- a/src/core/ao_pyro.h +++ b/src/core/ao_pyro.h @@ -63,6 +63,8 @@ struct ao_pyro { extern uint8_t ao_pyro_wakeup; +extern uint16_t ao_pyro_fired; + void ao_pyro_set(void); @@ -72,4 +74,10 @@ ao_pyro_show(void); void ao_pyro_init(void); +void +ao_pyro_manual(uint8_t p); + +void +ao_pyro_print_status(void); + #endif diff --git a/src/core/ao_quaternion.h b/src/core/ao_quaternion.h new file mode 100644 index 00000000..044f1607 --- /dev/null +++ b/src/core/ao_quaternion.h @@ -0,0 +1,249 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_QUATERNION_H_ +#define _AO_QUATERNION_H_ + +#include <math.h> + +struct ao_quaternion { + float r; /* real bit */ + float x, y, z; /* imaginary bits */ +}; + +static inline void ao_quaternion_multiply(struct ao_quaternion *r, + const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ + struct ao_quaternion t; +#define T(_a,_b) (((a)->_a) * ((b)->_b)) + +/* + * Quaternions + * + * ii = jj = kk = ijk = -1; + * + * kji = 1; + * + * ij = k; ji = -k; + * kj = -i; jk = i; + * ik = -j; ki = j; + * + * Multiplication p * q: + * + * (pr + ipx + jpy + kpz) (qr + iqx + jqy + kqz) = + * + * ( pr * qr + pr * iqx + pr * jqy + pr * kqz) + + * (ipx * qr + ipx * iqx + ipx * jqy + ipx * kqz) + + * (jpy * qr + jpy * iqx + jpy * jqy + jpy * kqz) + + * (kpz * qr + kpz * iqx + kpz * jqy + kpz * kqz) = + * + * + * (pr * qr) + i(pr * qx) + j(pr * qy) + k(pr * qz) + + * i(px * qr) - (px * qx) + k(px * qy) - j(px * qz) + + * j(py * qr) - k(py * qx) - (py * qy) + i(py * qz) + + * k(pz * qr) + j(pz * qx) - i(pz * qy) - (pz * qz) = + * + * 1 * ( (pr * qr) - (px * qx) - (py * qy) - (pz * qz) ) + + * i * ( (pr * qx) + (px * qr) + (py * qz) - (pz * qy) ) + + * j * ( (pr * qy) - (px * qz) + (py * qr) + (pz * qx) ) + + * k * ( (pr * qz) + (px * qy) - (py * qx) + (pz * qr); + */ + + t.r = T(r,r) - T(x,x) - T(y,y) - T(z,z); + t.x = T(r,x) + T(x,r) + T(y,z) - T(z,y); + t.y = T(r,y) - T(x,z) + T(y,r) + T(z,x); + t.z = T(r,z) + T(x,y) - T(y,x) + T(z,r); +#undef T + *r = t; +} + +static inline void ao_quaternion_conjugate(struct ao_quaternion *r, + const struct ao_quaternion *a) +{ + r->r = a->r; + r->x = -a->x; + r->y = -a->y; + r->z = -a->z; +} + +static inline float ao_quaternion_normal(const struct ao_quaternion *a) +{ +#define S(_a) (((a)->_a) * ((a)->_a)) + return S(r) + S(x) + S(y) + S(z); +#undef S +} + +static inline void ao_quaternion_scale(struct ao_quaternion *r, + const struct ao_quaternion *a, + float b) +{ + r->r = a->r * b; + r->x = a->x * b; + r->y = a->y * b; + r->z = a->z * b; +} + +static inline void ao_quaternion_normalize(struct ao_quaternion *r, + const struct ao_quaternion *a) +{ + float n = ao_quaternion_normal(a); + + if (n > 0) + ao_quaternion_scale(r, a, 1/sqrtf(n)); + else + *r = *a; +} + +static inline float ao_quaternion_dot(const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ +#define T(_a) (((a)->_a) * ((b)->_a)) + return T(r) + T(x) + T(y) + T(z); +#undef T +} + + +static inline void ao_quaternion_rotate(struct ao_quaternion *r, + const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ + struct ao_quaternion c; + struct ao_quaternion t; + + ao_quaternion_multiply(&t, b, a); + ao_quaternion_conjugate(&c, b); + ao_quaternion_multiply(r, &t, &c); +} + +/* + * Compute a rotation quaternion between two vectors + * + * cos(θ) + u * sin(θ) + * + * where θ is the angle between the two vectors and u + * is a unit vector axis of rotation + */ + +static inline void ao_quaternion_vectors_to_rotation(struct ao_quaternion *r, + const struct ao_quaternion *a, + const struct ao_quaternion *b) +{ + /* + * The cross product will point orthogonally to the two + * vectors, forming our rotation axis. The length will be + * sin(θ), so these values are already multiplied by that. + */ + + float x = a->y * b->z - a->z * b->y; + float y = a->z * b->x - a->x * b->z; + float z = a->x * b->y - a->y * b->x; + + float s_2 = x*x + y*y + z*z; + float s = sqrtf(s_2); + + /* cos(θ) = a · b / (|a| |b|). + * + * a and b are both unit vectors, so the divisor is one + */ + float c = a->x*b->x + a->y*b->y + a->z*b->z; + + float c_half = sqrtf ((1 + c) / 2); + float s_half = sqrtf ((1 - c) / 2); + + /* + * Divide out the sine factor from the + * cross product, then multiply in the + * half sine factor needed for the quaternion + */ + float s_scale = s_half / s; + + r->x = x * s_scale; + r->y = y * s_scale; + r->z = z * s_scale; + + r->r = c_half; + + ao_quaternion_normalize(r, r); +} + +static inline void ao_quaternion_init_vector(struct ao_quaternion *r, + float x, float y, float z) +{ + r->r = 0; + r->x = x; + r->y = y; + r->z = z; +} + +static inline void ao_quaternion_init_rotation(struct ao_quaternion *r, + float x, float y, float z, + float s, float c) +{ + r->r = c; + r->x = s * x; + r->y = s * y; + r->z = s * z; +} + +static inline void ao_quaternion_init_zero_rotation(struct ao_quaternion *r) +{ + r->r = 1; + r->x = r->y = r->z = 0; +} + +/* + * The sincosf from newlib just calls sinf and cosf. This is a bit + * faster, if slightly less precise + */ + +static inline void +ao_sincosf(float a, float *s, float *c) { + float _s = sinf(a); + *s = _s; + *c = sqrtf(1 - _s*_s); +} + +/* + * Initialize a quaternion from 1/2 euler rotation angles (in radians). + * + * Yes, it would be nicer if there were a faster way, but because we + * sample the gyros at only 100Hz, we end up getting angles too large + * to take advantage of sin(x) ≃ x. + * + * We might be able to use just a couple of elements of the sin taylor + * series though, instead of the whole sin function? + */ + +static inline void ao_quaternion_init_half_euler(struct ao_quaternion *r, + float x, float y, float z) +{ + float s_x, c_x; + float s_y, c_y; + float s_z, c_z; + + ao_sincosf(x, &s_x, &c_x); + ao_sincosf(y, &s_y, &c_y); + ao_sincosf(z, &s_z, &c_z); + + r->r = c_x * c_y * c_z + s_x * s_y * s_z; + r->x = s_x * c_y * c_z - c_x * s_y * s_z; + r->y = c_x * s_y * c_z + s_x * c_y * s_z; + r->z = c_x * c_y * s_z - s_x * s_y * c_z; +} + +#endif /* _AO_QUATERNION_H_ */ diff --git a/src/micropeak/ao_report_tiny.c b/src/core/ao_report_micro.c index 0e8e287f..0e8e287f 100644 --- a/src/micropeak/ao_report_tiny.c +++ b/src/core/ao_report_micro.c diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c index dec44f9f..adf8399d 100644 --- a/src/core/ao_sample.c +++ b/src/core/ao_sample.c @@ -20,6 +20,10 @@ #include <ao_data.h> #endif +#if HAS_GYRO +#include <ao_quaternion.h> +#endif + /* * Current sensor values */ @@ -44,8 +48,7 @@ __pdata accel_t ao_sample_accel_through; __pdata gyro_t ao_sample_roll; __pdata gyro_t ao_sample_pitch; __pdata gyro_t ao_sample_yaw; -__pdata angle_t ao_sample_angle; -__pdata angle_t ao_sample_roll_angle; +__pdata angle_t ao_sample_orient; #endif __data uint8_t ao_sample_data; @@ -67,9 +70,9 @@ __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ __pdata accel_t ao_ground_accel_along; __pdata accel_t ao_ground_accel_across; __pdata accel_t ao_ground_accel_through; -__pdata gyro_t ao_ground_pitch; -__pdata gyro_t ao_ground_yaw; -__pdata gyro_t ao_ground_roll; +__pdata int32_t ao_ground_pitch; +__pdata int32_t ao_ground_yaw; +__pdata int32_t ao_ground_roll; #endif static __pdata uint8_t ao_preflight; /* in preflight mode */ @@ -86,6 +89,7 @@ __pdata int32_t ao_sample_accel_through_sum; __pdata int32_t ao_sample_pitch_sum; __pdata int32_t ao_sample_yaw_sum; __pdata int32_t ao_sample_roll_sum; +static struct ao_quaternion ao_rotation; #endif static void @@ -120,20 +124,95 @@ ao_sample_preflight_set(void) ao_ground_accel_along = ao_sample_accel_along_sum >> 9; ao_ground_accel_across = ao_sample_accel_across_sum >> 9; ao_ground_accel_through = ao_sample_accel_through_sum >> 9; - ao_ground_pitch = ao_sample_pitch_sum >> 9; - ao_ground_yaw = ao_sample_yaw_sum >> 9; - ao_ground_roll = ao_sample_roll_sum >> 9; + ao_ground_pitch = ao_sample_pitch_sum; + ao_ground_yaw = ao_sample_yaw_sum; + ao_ground_roll = ao_sample_roll_sum; ao_sample_accel_along_sum = 0; ao_sample_accel_across_sum = 0; ao_sample_accel_through_sum = 0; ao_sample_pitch_sum = 0; ao_sample_yaw_sum = 0; ao_sample_roll_sum = 0; - ao_sample_angle = 0; + ao_sample_orient = 0; + + struct ao_quaternion orient; + + /* Take the pad IMU acceleration values and compute our current direction + */ + + ao_quaternion_init_vector(&orient, + (ao_ground_accel_across - ao_config.accel_zero_across), + (ao_ground_accel_through - ao_config.accel_zero_through), + (ao_ground_accel_along - ao_config.accel_zero_along)); + + ao_quaternion_normalize(&orient, + &orient); + + /* Here's up */ + + struct ao_quaternion up = { .r = 0, .x = 0, .y = 0, .z = 1 }; + + if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) + up.z = -1; + + /* Compute rotation to get from up to our current orientation, set + * that as the current rotation vector + */ + ao_quaternion_vectors_to_rotation(&ao_rotation, &up, &orient); #endif nsamples = 0; } +#if HAS_GYRO + +#define TIME_DIV 200.0f + +static void +ao_sample_rotate(void) +{ +#ifdef AO_FLIGHT_TEST + float dt = (ao_sample_tick - ao_sample_prev_tick) / TIME_DIV; +#else + static const float dt = 1/TIME_DIV; +#endif + float x = ao_mpu6000_gyro((float) ((ao_sample_pitch << 9) - ao_ground_pitch) / 512.0f) * dt; + float y = ao_mpu6000_gyro((float) ((ao_sample_yaw << 9) - ao_ground_yaw) / 512.0f) * dt; + float z = ao_mpu6000_gyro((float) ((ao_sample_roll << 9) - ao_ground_roll) / 512.0f) * dt; + struct ao_quaternion rot; + + ao_quaternion_init_half_euler(&rot, x, y, z); + ao_quaternion_multiply(&ao_rotation, &rot, &ao_rotation); + + /* And normalize to make sure it remains a unit vector */ + ao_quaternion_normalize(&ao_rotation, &ao_rotation); + + /* Compute pitch angle from vertical by taking the pad + * orientation vector and rotating it by the current total + * rotation value. That will be a unit vector pointing along + * the airframe axis. The Z value will be the cosine of the + * change in the angle from vertical since boost. + * + * rot = ao_rotation * vertical * ao_rotation° + * rot = ao_rotation * (0,0,0,1) * ao_rotation° + * = ((a.z, a.y, -a.x, a.r) * (a.r, -a.x, -a.y, -a.z)) .z + * + * = (-a.z * -a.z) + (a.y * -a.y) - (-a.x * -a.x) + (a.r * a.r) + * = a.z² - a.y² - a.x² + a.r² + * + * rot = ao_rotation * (0, 0, 0, -1) * ao_rotation° + * = ((-a.z, -a.y, a.x, -a.r) * (a.r, -a.x, -a.y, -a.z)) .z + * + * = (a.z * -a.z) + (-a.y * -a.y) - (a.x * -a.x) + (-a.r * a.r) + * = -a.z² + a.y² + a.x² - a.r² + */ + + float rotz; + rotz = ao_rotation.z * ao_rotation.z - ao_rotation.y * ao_rotation.y - ao_rotation.x * ao_rotation.x + ao_rotation.r * ao_rotation.r; + + ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI); +} +#endif + static void ao_sample_preflight(void) { @@ -232,9 +311,12 @@ ao_sample(void) ao_sample_preflight_update(); ao_kalman(); #if HAS_GYRO - /* do quaternion stuff here... */ + ao_sample_rotate(); #endif } +#ifdef AO_FLIGHT_TEST + ao_sample_prev_tick = ao_sample_tick; +#endif ao_sample_data = ao_data_ring_next(ao_sample_data); } return !ao_preflight; @@ -264,7 +346,7 @@ ao_sample_init(void) ao_sample_pitch = 0; ao_sample_yaw = 0; ao_sample_roll = 0; - ao_sample_angle = 0; + ao_sample_orient = 0; #endif ao_sample_data = ao_data_head; ao_preflight = TRUE; diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h index a2dac979..16d4c507 100644 --- a/src/core/ao_sample.h +++ b/src/core/ao_sample.h @@ -64,8 +64,6 @@ * for all further flight computations */ -#define GRAVITY 9.80665 - /* * Above this height, the baro sensor doesn't work */ @@ -115,9 +113,16 @@ extern __pdata int32_t ao_accel_scale; /* sensor to m/s² conversion */ extern __pdata accel_t ao_ground_accel_along; extern __pdata accel_t ao_ground_accel_across; extern __pdata accel_t ao_ground_accel_through; -extern __pdata gyro_t ao_ground_pitch; -extern __pdata gyro_t ao_ground_yaw; -extern __pdata gyro_t ao_ground_roll; +extern __pdata int32_t ao_ground_pitch; /* * 512 */ +extern __pdata int32_t ao_ground_yaw; /* * 512 */ +extern __pdata int32_t ao_ground_roll; /* * 512 */ +extern __pdata accel_t ao_sample_accel_along; +extern __pdata accel_t ao_sample_accel_across; +extern __pdata accel_t ao_sample_accel_through; +extern __pdata gyro_t ao_sample_roll; +extern __pdata gyro_t ao_sample_pitch; +extern __pdata gyro_t ao_sample_yaw; +extern __pdata angle_t ao_sample_orient; #endif void ao_sample_init(void); @@ -136,8 +141,8 @@ uint8_t ao_sample(void); extern __pdata int16_t ao_height; /* meters */ extern __pdata int16_t ao_speed; /* m/s * 16 */ extern __pdata int16_t ao_accel; /* m/s² * 16 */ -extern __pdata int16_t ao_max_height; /* max of ao_height */ -extern __pdata int16_t ao_avg_height; /* running average of height */ +extern __xdata int16_t ao_max_height; /* max of ao_height */ +extern __xdata int16_t ao_avg_height; /* running average of height */ extern __pdata int16_t ao_error_h; extern __pdata int16_t ao_error_h_sq_avg; diff --git a/src/core/ao_sample_profile.c b/src/core/ao_sample_profile.c index 1d9ed414..d3743d12 100644 --- a/src/core/ao_sample_profile.c +++ b/src/core/ao_sample_profile.c @@ -20,11 +20,11 @@ #include <ao_task.h> #ifndef AO_SAMPLE_PROFILE_LOW_PC -#define AO_SAMPLE_PROFILE_LOW_PC 0x08000000 +#define AO_SAMPLE_PROFILE_LOW_PC 0x08002000 #endif #ifndef AO_SAMPLE_PROFILE_HIGH_PC -#define AO_SAMPLE_PROFILE_HIGH_PC (AO_SAMPLE_PROFILE_LOW_PC + 44 * 1024) +#define AO_SAMPLE_PROFILE_HIGH_PC 0x0800f000 #endif #ifndef AO_SAMPLE_PROFILE_SHIFT diff --git a/src/core/ao_storage.c b/src/core/ao_storage.c index adf7e4d4..6eddae7f 100644 --- a/src/core/ao_storage.c +++ b/src/core/ao_storage.c @@ -154,7 +154,7 @@ ao_storage_zapall(void) __reentrant ao_cmd_white(); if (!ao_match_word("DoIt")) return; - for (pos = 0; pos < ao_storage_config; pos += ao_storage_block) + for (pos = 0; pos < ao_storage_log_max; pos += ao_storage_block) ao_storage_erase(pos); } diff --git a/src/core/ao_storage.h b/src/core/ao_storage.h index ea946399..6cc6fcb7 100644 --- a/src/core/ao_storage.h +++ b/src/core/ao_storage.h @@ -35,14 +35,22 @@ extern __pdata ao_pos_t ao_storage_total; /* Block size - device is erased in these units. At least 256 bytes */ extern __pdata ao_pos_t ao_storage_block; +#ifndef USE_STORAGE_CONFIG +#define USE_STORAGE_CONFIG 1 +#endif + +#if USE_STORAGE_CONFIG /* Byte offset of config block. Will be ao_storage_block bytes long */ extern __pdata ao_pos_t ao_storage_config; +#define ao_storage_log_max ao_storage_config +#else +#define ao_storage_log_max ao_storage_total +#endif + /* Storage unit size - device reads and writes must be within blocks of this size. Usually 256 bytes. */ extern __pdata uint16_t ao_storage_unit; -#define AO_STORAGE_ERASE_LOG (ao_storage_config + AO_CONFIG_MAX_SIZE) - /* Initialize above values. Can only be called once the OS is running */ void ao_storage_setup(void) __reentrant; diff --git a/src/core/ao_task.c b/src/core/ao_task.c index 0aad6508..bafb4943 100644 --- a/src/core/ao_task.c +++ b/src/core/ao_task.c @@ -109,6 +109,8 @@ ao_task_validate_alarm_queue(void) ao_panic(3); } } + if (ao_task_alarm_tick != ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm) + ao_panic(4); } #else #define ao_task_validate_alarm_queue() @@ -123,6 +125,7 @@ ao_task_to_alarm_queue(struct ao_task *task) ao_list_for_each_entry(alarm, &alarm_queue, struct ao_task, alarm_queue) { if ((int16_t) (alarm->alarm - task->alarm) >= 0) { ao_list_insert(&task->alarm_queue, alarm->alarm_queue.prev); + ao_task_alarm_tick = ao_list_first_entry(&alarm_queue, struct ao_task, alarm_queue)->alarm; ao_task_validate_alarm_queue(); return; } @@ -420,7 +423,7 @@ ao_sleep(__xdata void *wchan) } void -ao_wakeup(__xdata void *wchan) +ao_wakeup(__xdata void *wchan) __reentrant { #if HAS_TASK_QUEUE struct ao_task *sleep, *next; diff --git a/src/core/ao_task.h b/src/core/ao_task.h index 1a4b5b6b..9c56b480 100644 --- a/src/core/ao_task.h +++ b/src/core/ao_task.h @@ -45,7 +45,10 @@ struct ao_task { #endif }; +#ifndef AO_NUM_TASKS #define AO_NUM_TASKS 16 /* maximum number of tasks */ +#endif + #define AO_NO_TASK 0 /* no task id */ extern __xdata struct ao_task * __xdata ao_tasks[AO_NUM_TASKS]; @@ -67,7 +70,7 @@ ao_sleep(__xdata void *wchan); /* Wake all tasks sleeping on wchan */ void -ao_wakeup(__xdata void *wchan); +ao_wakeup(__xdata void *wchan) __reentrant; /* set an alarm to go off in 'delay' ticks */ void diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index c3bbfec5..c118d007 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -19,9 +19,16 @@ #include "ao_log.h" #include "ao_product.h" +#ifndef HAS_RDF +#define HAS_RDF 1 +#endif + static __pdata uint16_t ao_telemetry_interval; static __pdata uint8_t ao_rdf = 0; + +#if HAS_RDF static __pdata uint16_t ao_rdf_time; +#endif #if HAS_APRS static __pdata uint16_t ao_aprs_time; @@ -33,6 +40,10 @@ static __pdata uint16_t ao_aprs_time; #define AO_SEND_MEGA 1 #endif +#if defined (TELEMETRUM_V_2_0) +#define AO_SEND_METRUM 1 +#endif + #if defined(TELEMETRUM_V_0_1) || defined(TELEMETRUM_V_0_2) || defined(TELEMETRUM_V_1_0) || defined(TELEMETRUM_V_1_1) || defined(TELEBALLOON_V_1_1) || defined(TELEMETRUM_V_1_2) #define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_TELEMETRUM #endif @@ -104,6 +115,7 @@ ao_send_mega_sensor(void) telemetry.generic.tick = packet->tick; telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR; + telemetry.mega_sensor.orient = ao_sample_orient; telemetry.mega_sensor.accel = ao_data_accel(packet); telemetry.mega_sensor.pres = ao_data_pres(packet); telemetry.mega_sensor.temp = ao_data_temp(packet); @@ -164,6 +176,87 @@ ao_send_mega_data(void) } #endif /* AO_SEND_MEGA */ +#ifdef AO_SEND_METRUM +/* Send telemetrum sensor packet */ +static void +ao_send_metrum_sensor(void) +{ + __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; + + telemetry.generic.tick = packet->tick; + telemetry.generic.type = AO_TELEMETRY_METRUM_SENSOR; + + telemetry.metrum_sensor.state = ao_flight_state; + telemetry.metrum_sensor.accel = ao_data_accel(packet); + telemetry.metrum_sensor.pres = ao_data_pres(packet); + telemetry.metrum_sensor.temp = ao_data_temp(packet); + + telemetry.metrum_sensor.acceleration = ao_accel; + telemetry.metrum_sensor.speed = ao_speed; + telemetry.metrum_sensor.height = ao_height; + + telemetry.metrum_sensor.v_batt = packet->adc.v_batt; + telemetry.metrum_sensor.sense_a = packet->adc.sense_a; + telemetry.metrum_sensor.sense_m = packet->adc.sense_m; + + ao_radio_send(&telemetry, sizeof (telemetry)); +} + +static __pdata int8_t ao_telemetry_metrum_data_max; +static __pdata int8_t ao_telemetry_metrum_data_cur; + +/* Send telemetrum data packet */ +static void +ao_send_metrum_data(void) +{ + if (--ao_telemetry_metrum_data_cur <= 0) { + __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; + uint8_t i; + + telemetry.generic.tick = packet->tick; + telemetry.generic.type = AO_TELEMETRY_METRUM_DATA; + + telemetry.metrum_data.ground_pres = ao_ground_pres; + telemetry.metrum_data.ground_accel = ao_ground_accel; + telemetry.metrum_data.accel_plus_g = ao_config.accel_plus_g; + telemetry.metrum_data.accel_minus_g = ao_config.accel_minus_g; + + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_metrum_data_cur = ao_telemetry_metrum_data_max; + } +} +#endif /* AO_SEND_METRUM */ + +#ifdef AO_SEND_MINI + +static void +ao_send_mini(void) +{ + __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; + + telemetry.generic.tick = packet->tick; + telemetry.generic.type = AO_TELEMETRY_MINI; + + telemetry.mini.state = ao_flight_state; + + telemetry.mini.v_batt = packet->adc.v_batt; + telemetry.mini.sense_a = packet->adc.sense_a; + telemetry.mini.sense_m = packet->adc.sense_m; + + telemetry.mini.pres = ao_data_pres(packet); + telemetry.mini.temp = ao_data_temp(packet); + + telemetry.mini.acceleration = ao_accel; + telemetry.mini.speed = ao_speed; + telemetry.mini.height = ao_height; + + telemetry.mini.ground_pres = ao_ground_pres; + + ao_radio_send(&telemetry, sizeof (telemetry)); +} + +#endif /* AO_SEND_MINI */ + #ifdef AO_SEND_ALL_BARO static uint8_t ao_baro_sample; @@ -198,7 +291,11 @@ ao_send_configuration(void) { telemetry.generic.type = AO_TELEMETRY_CONFIGURATION; telemetry.configuration.device = AO_idProduct_NUMBER; +#if HAS_LOG telemetry.configuration.flight = ao_log_full() ? 0 : ao_flight_number; +#else + telemetry.configuration.flight = ao_flight_number; +#endif telemetry.configuration.config_major = AO_CONFIG_MAJOR; telemetry.configuration.config_minor = AO_CONFIG_MINOR; telemetry.configuration.apogee_delay = ao_config.apogee_delay; @@ -295,12 +392,14 @@ ao_telemetry(void) for (;;) { while (ao_telemetry_interval == 0) ao_sleep(&telemetry); - time = ao_rdf_time = ao_time(); + time = ao_time(); +#if HAS_RDF + ao_rdf_time = time; +#endif #if HAS_APRS ao_aprs_time = time; #endif while (ao_telemetry_interval) { - #if HAS_APRS if (!(ao_config.radio_enable & AO_RADIO_DISABLE_TELEMETRY)) #endif @@ -308,14 +407,23 @@ ao_telemetry(void) #ifdef AO_SEND_ALL_BARO ao_send_baro(); #endif + #if HAS_FLIGHT -#ifdef AO_SEND_MEGA +# ifdef AO_SEND_MEGA ao_send_mega_sensor(); ao_send_mega_data(); -#else +# endif +# ifdef AO_SEND_METRUM + ao_send_metrum_sensor(); + ao_send_metrum_data(); +# endif +# ifdef AO_SEND_MINI + ao_send_mini(); +# endif +# ifdef AO_TELEMETRY_SENSOR ao_send_sensor(); -#endif -#endif +# endif +#endif /* HAS_FLIGHT */ #if HAS_COMPANION if (ao_companion_running) @@ -328,23 +436,25 @@ ao_telemetry(void) #endif } #ifndef AO_SEND_ALL_BARO +#if HAS_RDF if (ao_rdf && #if HAS_APRS !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) && -#endif +#endif /* HAS_APRS */ (int16_t) (ao_time() - ao_rdf_time) >= 0) { #if HAS_IGNITE_REPORT uint8_t c; -#endif +#endif /* HAS_IGNITE_REPORT */ ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; #if HAS_IGNITE_REPORT if (ao_flight_state == ao_flight_pad && (c = ao_report_igniter())) ao_radio_continuity(c); else -#endif +#endif /* HAS_IGNITE_REPORT*/ ao_radio_rdf(); } +#endif /* HAS_RDF */ #if HAS_APRS if (ao_config.aprs_interval != 0 && (int16_t) (ao_time() - ao_aprs_time) >= 0) @@ -352,8 +462,8 @@ ao_telemetry(void) ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval); ao_aprs_send(); } -#endif -#endif +#endif /* HAS_APRS */ +#endif /* !AO_SEND_ALL_BARO */ time += ao_telemetry_interval; delay = time - ao_time(); if (delay > 0) { @@ -383,6 +493,12 @@ ao_telemetry_set_interval(uint16_t interval) cur++; ao_telemetry_mega_data_cur = cur; #endif +#if AO_SEND_METRUM + ao_telemetry_metrum_data_max = AO_SEC_TO_TICKS(1) / interval; + if (ao_telemetry_metrum_data_max > cur) + cur++; + ao_telemetry_metrum_data_cur = cur; +#endif #if HAS_COMPANION if (!ao_companion_setup.update_period) @@ -411,6 +527,7 @@ ao_telemetry_set_interval(uint16_t interval) ao_wakeup(&telemetry); } +#if HAS_RDF void ao_rdf_set(uint8_t rdf) { @@ -421,6 +538,7 @@ ao_rdf_set(uint8_t rdf) ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; } } +#endif __xdata struct ao_task ao_telemetry_task; diff --git a/src/core/ao_telemetry.h b/src/core/ao_telemetry.h index f2d201de..237a35ab 100644 --- a/src/core/ao_telemetry.h +++ b/src/core/ao_telemetry.h @@ -162,7 +162,7 @@ struct ao_telemetry_mega_sensor { uint16_t tick; /* 2 */ uint8_t type; /* 4 */ - uint8_t pad5; /* 5 */ + uint8_t orient; /* 5 angle from vertical */ int16_t accel; /* 6 Z axis */ int32_t pres; /* 8 Pa * 10 */ @@ -207,6 +207,72 @@ struct ao_telemetry_mega_data { }; +#define AO_TELEMETRY_METRUM_SENSOR 0x0A + +struct ao_telemetry_metrum_sensor { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t state; /* 5 flight state */ + int16_t accel; /* 6 Z axis */ + + int32_t pres; /* 8 Pa * 10 */ + int16_t temp; /* 12 °C * 100 */ + + int16_t acceleration; /* 14 m/s² * 16 */ + int16_t speed; /* 16 m/s * 16 */ + int16_t height; /* 18 m */ + + int16_t v_batt; /* 20 battery voltage */ + int16_t sense_a; /* 22 apogee continuity sense */ + int16_t sense_m; /* 24 main continuity sense */ + + uint8_t pad[6]; /* 26 */ + /* 32 */ +}; + +#define AO_TELEMETRY_METRUM_DATA 0x0B + +struct ao_telemetry_metrum_data { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + int32_t ground_pres; /* 8 average pres on pad */ + int16_t ground_accel; /* 12 average accel on pad */ + int16_t accel_plus_g; /* 14 accel calibration at +1g */ + int16_t accel_minus_g; /* 16 accel calibration at -1g */ + + uint8_t pad[14]; /* 18 */ + /* 32 */ +}; + +#define AO_TELEMETRY_MINI 0x10 + +struct ao_telemetry_mini { + uint16_t serial; /* 0 */ + uint16_t tick; /* 2 */ + uint8_t type; /* 4 */ + + uint8_t state; /* 5 flight state */ + int16_t v_batt; /* 6 battery voltage */ + int16_t sense_a; /* 8 apogee continuity */ + int16_t sense_m; /* 10 main continuity */ + + int32_t pres; /* 12 Pa * 10 */ + int16_t temp; /* 16 °C * 100 */ + + int16_t acceleration; /* 18 m/s² * 16 */ + int16_t speed; /* 20 m/s * 16 */ + int16_t height; /* 22 m */ + + int32_t ground_pres; /* 24 average pres on pad */ + + int32_t pad28; /* 28 */ + /* 32 */ +}; + /* #define AO_SEND_ALL_BARO */ #define AO_TELEMETRY_BARO 0x80 @@ -240,6 +306,9 @@ union ao_telemetry_all { struct ao_telemetry_companion companion; struct ao_telemetry_mega_sensor mega_sensor; struct ao_telemetry_mega_data mega_data; + struct ao_telemetry_metrum_sensor metrum_sensor; + struct ao_telemetry_metrum_data metrum_data; + struct ao_telemetry_mini mini; struct ao_telemetry_baro baro; }; diff --git a/src/core/ao_usb.h b/src/core/ao_usb.h index 6bc77608..35e64e65 100644 --- a/src/core/ao_usb.h +++ b/src/core/ao_usb.h @@ -102,8 +102,11 @@ extern __code __at (0x00aa) uint8_t ao_usb_descriptors []; #define AO_USB_INT_EP 1 #define AO_USB_INT_SIZE 8 +#ifndef AO_USB_OUT_EP #define AO_USB_OUT_EP 4 #define AO_USB_IN_EP 5 +#endif + /* * USB bulk packets can only come in 8, 16, 32 and 64 * byte sizes, so we'll use 64 for everything diff --git a/src/drivers/ao_74hc165.c b/src/drivers/ao_74hc165.c new file mode 100644 index 00000000..143f4e3f --- /dev/null +++ b/src/drivers/ao_74hc165.c @@ -0,0 +1,58 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +/* + * 74HC165 driver. + * Reads a single byte from the shift register + */ + +#include <ao.h> +#include <ao_74hc165.h> + +uint8_t +ao_74hc165_read(void) +{ + static __xdata state; + ao_spi_get(AO_74HC165_SPI_BUS); + ao_spi_set_speed(AO_74HC165_SPI_BUS, AO_SPI_SPEED_FAST); + AO_74HC165_CS = 1; + ao_spi_recv(&state, 1, AO_74HC165_SPI_BUS); + AO_74HC165_CS = 0; + ao_spi_put(AO_74HC165_SPI_BUS); + return state; +} + +static void +ao_74hc165_cmd(void) +{ + uint8_t v; + + v = ao_74hc165_read(); + printf ("Switches: 0x%02x\n", v); +} + +static const struct ao_cmds ao_74hc165_cmds[] = { + { ao_74hc165_cmd, "L\0Show 74hc165" }, + { 0, NULL } +}; + +void +ao_74hc165_init(void) +{ + ao_enable_output(AO_74HC165_CS_PORT, AO_74HC165_CS_PIN, AO_74HC165_CS, 0); + ao_cmd_register(&ao_74hc165_cmds[0]); +} diff --git a/src/drivers/ao_74hc165.h b/src/drivers/ao_74hc165.h new file mode 100644 index 00000000..3ae51353 --- /dev/null +++ b/src/drivers/ao_74hc165.h @@ -0,0 +1,27 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_74HC165_H_ +#define _AO_74HC165_H_ + +uint8_t +ao_74hc165_read(void); + +void +ao_74hc165_init(void); + +#endif /* _AO_74HC165_H_ */ diff --git a/src/drivers/ao_button.c b/src/drivers/ao_button.c index a507c909..25c0cd5c 100644 --- a/src/drivers/ao_button.c +++ b/src/drivers/ao_button.c @@ -18,6 +18,7 @@ #include <ao.h> #include <ao_button.h> #include <ao_exti.h> +#include <ao_debounce.h> #if AO_EVENT #include <ao_event.h> #define ao_button_queue(b,v) ao_event_put_isr(AO_EVENT_BUTTON, b, v) @@ -25,55 +26,71 @@ #define ao_button_queue(b,v) #endif -static uint8_t ao_button[AO_BUTTON_COUNT]; -static AO_TICK_TYPE ao_button_time[AO_BUTTON_COUNT]; +#define AO_BUTTON_DEBOUNCE_HOLD 10 -#define AO_DEBOUNCE AO_MS_TO_TICKS(20) +static struct ao_debounce ao_button_debounce[AO_BUTTON_COUNT]; #define port(q) AO_BUTTON_ ## q ## _PORT #define bit(q) AO_BUTTON_ ## q #define pin(q) AO_BUTTON_ ## q ## _PIN -static void -ao_button_do(uint8_t b, uint8_t v) -{ - /* Debounce */ - if ((AO_TICK_SIGNED) (ao_tick_count - ao_button_time[b]) < AO_DEBOUNCE) - return; - - /* pins are inverted */ - v = !v; - if (ao_button[b] != v) { - ao_button[b] = v; - ao_button_time[b] = ao_tick_count; - ao_button_queue(b, v); - ao_wakeup(&ao_button[b]); - } -} +/* pins are inverted */ +#define ao_button_value(b) !ao_gpio_get(port(b), bit(b), pin(b)) -#define ao_button_update(b) ao_button_do(b, ao_gpio_get(port(b), bit(b), pin(b))) - -static void -ao_button_isr(void) +static uint8_t +_ao_button_get(struct ao_debounce *debounce) { + uint8_t b = debounce - ao_button_debounce; + + switch (b) { #if AO_BUTTON_COUNT > 0 - ao_button_update(0); + case 0: return ao_button_value(0); #endif #if AO_BUTTON_COUNT > 1 - ao_button_update(1); + case 1: return ao_button_value(1); #endif #if AO_BUTTON_COUNT > 2 - ao_button_update(2); + case 2: return ao_button_value(2); #endif #if AO_BUTTON_COUNT > 3 - ao_button_update(3); + case 3: return ao_button_value(3); #endif #if AO_BUTTON_COUNT > 4 - ao_button_update(4); + case 4: return ao_button_value(4); #endif + } +} + +static void +_ao_button_set(struct ao_debounce *debounce, uint8_t value) +{ + uint8_t b = debounce - ao_button_debounce; + + ao_button_queue(b, value); +} + + +#define ao_button_update(b) ao_button_do(b, ao_gpio_get(port(b), bit(b), pin(b))) + +static void +ao_button_debounce_init(struct ao_debounce *debounce) { + ao_debounce_config(debounce, + _ao_button_get, + _ao_button_set, + AO_BUTTON_DEBOUNCE_HOLD); +} + +static void +ao_button_isr(void) +{ + uint8_t b; + + for (b = 0; b < AO_BUTTON_COUNT; b++) + _ao_debounce_start(&ao_button_debounce[b]); } #define init(b) do { \ + ao_button_debounce_init(&ao_button_debounce[b]); \ ao_enable_port(port(b)); \ \ ao_exti_setup(port(b), bit(b), \ @@ -91,4 +108,14 @@ ao_button_init(void) #if AO_BUTTON_COUNT > 1 init(1); #endif +#if AO_BUTTON_COUNT > 2 + init(2); +#endif +#if AO_BUTTON_COUNT > 3 + init(3); +#endif +#if AO_BUTTON_COUNT > 4 + init(4); +#endif + ao_debounce_init(); } diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c index 772014ee..37d04927 100644 --- a/src/drivers/ao_cc1120.c +++ b/src/drivers/ao_cc1120.c @@ -326,6 +326,8 @@ static const uint16_t packet_setup[] = { (0 << CC1120_PKT_CFG0_PKG_BIT_LEN) | (0 << CC1120_PKT_CFG0_UART_MODE_EN) | (0 << CC1120_PKT_CFG0_UART_SWAP_EN)), + CC1120_PREAMBLE_CFG1, ((CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_4_BYTES << CC1120_PREAMBLE_CFG1_NUM_PREAMBLE) | + (CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA << CC1120_PREAMBLE_CFG1_PREAMBLE_WORD)), AO_CC1120_MARC_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_MARC_MCU_WAKEUP, }; @@ -389,6 +391,8 @@ static const uint16_t rdf_setup[] = { (0 << CC1120_PKT_CFG0_PKG_BIT_LEN) | (0 << CC1120_PKT_CFG0_UART_MODE_EN) | (0 << CC1120_PKT_CFG0_UART_SWAP_EN)), + CC1120_PREAMBLE_CFG1, ((CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_NONE << CC1120_PREAMBLE_CFG1_NUM_PREAMBLE) | + (CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA << CC1120_PREAMBLE_CFG1_PREAMBLE_WORD)), }; /* @@ -433,6 +437,33 @@ static const uint16_t aprs_setup[] = { (CC1120_PKT_CFG1_ADDR_CHECK_CFG_NONE << CC1120_PKT_CFG1_ADDR_CHECK_CFG) | (CC1120_PKT_CFG1_CRC_CFG_DISABLED << CC1120_PKT_CFG1_CRC_CFG) | (0 << CC1120_PKT_CFG1_APPEND_STATUS)), + CC1120_PREAMBLE_CFG1, ((CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_NONE << CC1120_PREAMBLE_CFG1_NUM_PREAMBLE) | + (CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA << CC1120_PREAMBLE_CFG1_PREAMBLE_WORD)), +}; + +/* + * For Test mode, we want an unmodulated carrier. To do that, we + * set the deviation to zero and enable a preamble so that the radio + * turns on before we send any data + */ + +static const uint16_t test_setup[] = { + CC1120_DEVIATION_M, 0, + CC1120_MODCFG_DEV_E, ((CC1120_MODCFG_DEV_E_MODEM_MODE_NORMAL << CC1120_MODCFG_DEV_E_MODEM_MODE) | + (CC1120_MODCFG_DEV_E_MOD_FORMAT_2_GFSK << CC1120_MODCFG_DEV_E_MOD_FORMAT) | + (0 << CC1120_MODCFG_DEV_E_DEV_E)), + CC1120_DRATE2, ((APRS_DRATE_E << CC1120_DRATE2_DATARATE_E) | + (((APRS_DRATE_M >> 16) & CC1120_DRATE2_DATARATE_M_19_16_MASK) << CC1120_DRATE2_DATARATE_M_19_16)), + CC1120_DRATE1, ((APRS_DRATE_M >> 8) & 0xff), + CC1120_DRATE0, ((APRS_DRATE_M >> 0) & 0xff), + CC1120_PKT_CFG2, ((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) | + (CC1120_PKT_CFG2_PKT_FORMAT_NORMAL << CC1120_PKT_CFG2_PKT_FORMAT)), + CC1120_PKT_CFG1, ((0 << CC1120_PKT_CFG1_WHITE_DATA) | + (CC1120_PKT_CFG1_ADDR_CHECK_CFG_NONE << CC1120_PKT_CFG1_ADDR_CHECK_CFG) | + (CC1120_PKT_CFG1_CRC_CFG_DISABLED << CC1120_PKT_CFG1_CRC_CFG) | + (0 << CC1120_PKT_CFG1_APPEND_STATUS)), + CC1120_PREAMBLE_CFG1, ((CC1120_PREAMBLE_CFG1_NUM_PREAMBLE_4_BYTES << CC1120_PREAMBLE_CFG1_NUM_PREAMBLE) | + (CC1120_PREAMBLE_CFG1_PREAMBLE_WORD_AA << CC1120_PREAMBLE_CFG1_PREAMBLE_WORD)), }; #define AO_PKT_CFG0_INFINITE ((0 << CC1120_PKT_CFG0_RESERVED7) | \ @@ -456,8 +487,9 @@ static uint16_t ao_radio_mode; #define AO_RADIO_MODE_BITS_PACKET_RX 16 #define AO_RADIO_MODE_BITS_RDF 32 #define AO_RADIO_MODE_BITS_APRS 64 -#define AO_RADIO_MODE_BITS_INFINITE 128 -#define AO_RADIO_MODE_BITS_FIXED 256 +#define AO_RADIO_MODE_BITS_TEST 128 +#define AO_RADIO_MODE_BITS_INFINITE 256 +#define AO_RADIO_MODE_BITS_FIXED 512 #define AO_RADIO_MODE_NONE 0 #define AO_RADIO_MODE_PACKET_TX_BUF (AO_RADIO_MODE_BITS_PACKET | AO_RADIO_MODE_BITS_PACKET_TX | AO_RADIO_MODE_BITS_TX_BUF) @@ -467,6 +499,7 @@ static uint16_t ao_radio_mode; #define AO_RADIO_MODE_APRS_BUF (AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_INFINITE | AO_RADIO_MODE_BITS_TX_BUF) #define AO_RADIO_MODE_APRS_LAST_BUF (AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_FIXED | AO_RADIO_MODE_BITS_TX_BUF) #define AO_RADIO_MODE_APRS_FINISH (AO_RADIO_MODE_BITS_APRS | AO_RADIO_MODE_BITS_FIXED | AO_RADIO_MODE_BITS_TX_FINISH) +#define AO_RADIO_MODE_TEST (AO_RADIO_MODE_BITS_TEST | AO_RADIO_MODE_BITS_INFINITE | AO_RADIO_MODE_BITS_TX_BUF) static void ao_radio_set_mode(uint16_t new_mode) @@ -504,6 +537,10 @@ ao_radio_set_mode(uint16_t new_mode) for (i = 0; i < sizeof (aprs_setup) / sizeof (aprs_setup[0]); i += 2) ao_radio_reg_write(aprs_setup[i], aprs_setup[i+1]); + if (changes & AO_RADIO_MODE_BITS_TEST) + for (i = 0; i < sizeof (test_setup) / sizeof (test_setup[0]); i += 2) + ao_radio_reg_write(test_setup[i], test_setup[i+1]); + if (changes & AO_RADIO_MODE_BITS_INFINITE) ao_radio_reg_write(CC1120_PKT_CFG0, AO_PKT_CFG0_INFINITE); @@ -652,6 +689,7 @@ ao_radio_test_cmd(void) ao_packet_slave_stop(); #endif ao_radio_get(0xff); + ao_radio_set_mode(AO_RADIO_MODE_TEST); ao_radio_strobe(CC1120_STX); #if CC1120_TRACE { int t; diff --git a/src/drivers/ao_cc1120.h b/src/drivers/ao_cc1120.h index 5d226b64..a1d78c01 100644 --- a/src/drivers/ao_cc1120.h +++ b/src/drivers/ao_cc1120.h @@ -404,6 +404,8 @@ #define CC1120_MARC_SPARE (CC1120_EXTENDED_BIT | 0x03) #define CC1120_ECG_CFG (CC1120_EXTENDED_BIT | 0x04) #define CC1120_SOFT_TX_DATA_CFG (CC1120_EXTENDED_BIT | 0x05) +#define CC1120_SOFT_TX_DATA_CFG_SYMBOL_MAP_CFG 5 +#define CC1120_SOFT_TX_DATA_CFG_SOFT_TX_DATA_EN 0 #define CC1120_EXT_CTRL (CC1120_EXTENDED_BIT | 0x06) #define CC1120_RCCAL_FINE (CC1120_EXTENDED_BIT | 0x07) #define CC1120_RCCAL_COARSE (CC1120_EXTENDED_BIT | 0x08) diff --git a/src/drivers/ao_cc115l.c b/src/drivers/ao_cc115l.c index 05e6a762..0fa1e899 100644 --- a/src/drivers/ao_cc115l.c +++ b/src/drivers/ao_cc115l.c @@ -245,6 +245,8 @@ ao_radio_idle(void) uint8_t state = ao_radio_strobe(CC115L_SIDLE); if ((state >> CC115L_STATUS_STATE) == CC115L_STATUS_STATE_IDLE) break; + if ((state >> CC115L_STATUS_STATE) == CC115L_STATUS_STATE_TX_FIFO_UNDERFLOW) + ao_radio_strobe(CC115L_SFTX); } /* Flush any pending TX bytes */ ao_radio_strobe(CC115L_SFTX); @@ -476,6 +478,8 @@ ao_radio_setup(void) ao_config_get(); + ao_radio_strobe(CC115L_SCAL); + ao_radio_configured = 1; } @@ -494,7 +498,6 @@ static void ao_radio_get(void) { static uint32_t last_radio_setting; - static uint8_t last_power_setting; ao_mutex_get(&ao_radio_mutex); if (!ao_radio_configured) @@ -505,10 +508,6 @@ ao_radio_get(void) ao_radio_reg_write(CC115L_FREQ0, ao_config.radio_setting); last_radio_setting = ao_config.radio_setting; } - if (ao_config.radio_power != last_power_setting) { - ao_radio_reg_write(CC115L_PA, ao_config.radio_power); - last_power_setting = ao_config.radio_power; - } } static void @@ -614,6 +613,20 @@ ao_radio_rdf_abort(void) ao_wakeup(&ao_radio_wake); } +#define POWER_STEP 0x08 + +static void +ao_radio_stx(void) +{ + uint8_t power; + ao_radio_pa_on(); + ao_radio_reg_write(CC115L_PA, 0); + ao_radio_strobe(CC115L_STX); + for (power = POWER_STEP; power < ao_config.radio_power; power += POWER_STEP) + ao_radio_reg_write(CC115L_PA, power); + ao_radio_reg_write(CC115L_PA, ao_config.radio_power); +} + static void ao_radio_test_cmd(void) { @@ -633,11 +646,10 @@ ao_radio_test_cmd(void) ao_packet_slave_stop(); #endif ao_radio_get(); - ao_radio_set_len(0xff); - ao_radio_set_mode(AO_RADIO_MODE_RDF|AO_RADIO_MODE_BITS_FIXED); ao_radio_strobe(CC115L_SFTX); - ao_radio_pa_on(); - ao_radio_strobe(CC115L_STX); + ao_radio_set_len(0xff); + ao_radio_set_mode(AO_RADIO_MODE_RDF); + ao_radio_stx(); radio_on = 1; } if (mode == 3) { @@ -655,12 +667,14 @@ ao_radio_test_cmd(void) } } +#if CC115L_TRACE static inline int16_t ao_radio_gpio_bits(void) { return AO_CC115L_DONE_INT_PORT->idr & ((1 << AO_CC115L_FIFO_INT_PIN) | (1 << AO_CC115L_DONE_INT_PIN)); } +#endif static void ao_radio_wait_fifo(void) @@ -738,6 +752,10 @@ _ao_radio_send_lots(ao_radio_fill_func fill, uint8_t mode) uint8_t fifo_space; fifo_space = CC115L_FIFO_SIZE; + ao_radio_abort = 0; + + ao_radio_strobe(CC115L_SFTX); + ao_radio_done = 0; ao_radio_fifo = 0; while (!done) { @@ -784,8 +802,7 @@ _ao_radio_send_lots(ao_radio_fill_func fill, uint8_t mode) ao_exti_enable(AO_CC115L_DONE_INT_PORT, AO_CC115L_DONE_INT_PIN); if (!started) { - ao_radio_pa_on(); - ao_radio_strobe(CC115L_STX); + ao_radio_stx(); started = 1; } } diff --git a/src/drivers/ao_event.c b/src/drivers/ao_event.c index 440ef2de..c428125d 100644 --- a/src/drivers/ao_event.c +++ b/src/drivers/ao_event.c @@ -25,11 +25,6 @@ #define ao_event_queue_empty() (ao_event_queue_insert == ao_event_queue_remove) #define ao_event_queue_full() (ao_event_queue_next(ao_event_queue_insert) == ao_event_queue_remove) -/* - * Whether a sequence of events from the same device should be collapsed - */ -#define ao_event_can_collapse(type) ((type) == AO_EVENT_QUADRATURE) - struct ao_event ao_event_queue[AO_EVENT_QUEUE]; uint8_t ao_event_queue_insert; uint8_t ao_event_queue_remove; @@ -48,17 +43,9 @@ ao_event_get(struct ao_event *ev) /* called with interrupts disabled */ void -ao_event_put_isr(uint8_t type, uint8_t unit, uint32_t value) +ao_event_put_isr(uint8_t type, uint8_t unit, int32_t value) { if (!ao_event_queue_full()) { - - if (ao_event_can_collapse(type) && !ao_event_queue_empty()) { - uint8_t prev = ao_event_queue_prev(ao_event_queue_insert); - - if (ao_event_queue[prev].type == type && - ao_event_queue[prev].unit == unit) - ao_event_queue_insert = prev; - } ao_event_queue[ao_event_queue_insert] = (struct ao_event) { .type = type, .unit = unit, @@ -71,7 +58,7 @@ ao_event_put_isr(uint8_t type, uint8_t unit, uint32_t value) } void -ao_event_put(uint8_t type, uint8_t unit, uint32_t value) +ao_event_put(uint8_t type, uint8_t unit, int32_t value) { ao_arch_critical(ao_event_put_isr(type, unit, value);); } diff --git a/src/drivers/ao_event.h b/src/drivers/ao_event.h index 25c49c35..ed9a7433 100644 --- a/src/drivers/ao_event.h +++ b/src/drivers/ao_event.h @@ -26,16 +26,16 @@ struct ao_event { uint8_t type; uint8_t unit; uint16_t tick; - uint32_t value; + int32_t value; }; uint8_t ao_event_get(struct ao_event *ev); void -ao_event_put_isr(uint8_t type, uint8_t unit, uint32_t value); +ao_event_put_isr(uint8_t type, uint8_t unit, int32_t value); void -ao_event_put(uint8_t type, uint8_t unit, uint32_t value); +ao_event_put(uint8_t type, uint8_t unit, int32_t value); #endif /* _AO_EVENT_H_ */ diff --git a/src/drivers/ao_gps_sirf.c b/src/drivers/ao_gps_sirf.c index 91fc948b..d89435b9 100644 --- a/src/drivers/ao_gps_sirf.c +++ b/src/drivers/ao_gps_sirf.c @@ -19,6 +19,7 @@ #include "ao.h" #endif +__xdata uint8_t ao_gps_new; __xdata uint8_t ao_gps_mutex; __pdata uint16_t ao_gps_tick; __xdata struct ao_telemetry_location ao_gps_data; @@ -422,8 +423,9 @@ ao_gps(void) __reentrant else ao_gps_data.v_error = ao_sirf_data.v_error / 100; #endif + ao_gps_new |= AO_GPS_NEW_DATA; ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_data); + ao_wakeup(&ao_gps_new); break; case 4: ao_mutex_get(&ao_gps_mutex); @@ -432,8 +434,9 @@ ao_gps(void) __reentrant ao_gps_tracking_data.sats[i].svid = ao_sirf_tracker_data.sats[i].svid; ao_gps_tracking_data.sats[i].c_n_1 = ao_sirf_tracker_data.sats[i].c_n_1; } + ao_gps_new |= AO_GPS_NEW_TRACKING; ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_tracking_data); + ao_wakeup(&ao_gps_new); break; } } diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index d2f67e6b..944a37f9 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -32,6 +32,7 @@ #define ao_gps_set_speed ao_serial1_set_speed #endif +__xdata uint8_t ao_gps_new; __xdata uint8_t ao_gps_mutex; static __data char ao_gps_char; static __data uint8_t ao_gps_cksum; @@ -293,10 +294,11 @@ ao_nmea_gga(void) if (!ao_gps_error) { ao_mutex_get(&ao_gps_mutex); + ao_gps_new |= AO_GPS_NEW_DATA; ao_gps_tick = ao_gps_next_tick; ao_xmemcpy(&ao_gps_data, PDATA_TO_XDATA(&ao_gps_next), sizeof (ao_gps_data)); ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_data); + ao_wakeup(&ao_gps_new); } } @@ -352,9 +354,10 @@ ao_nmea_gsv(void) ao_gps_tracking_next.channels = 0; else if (done) { ao_mutex_get(&ao_gps_mutex); + ao_gps_new |= AO_GPS_NEW_TRACKING; ao_xmemcpy(&ao_gps_tracking_data, PDATA_TO_XDATA(&ao_gps_tracking_next), sizeof(ao_gps_tracking_data)); ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_tracking_data); + ao_wakeup(&ao_gps_new); } } @@ -483,25 +486,6 @@ ao_gps(void) __reentrant __xdata struct ao_task ao_gps_task; -static void -gps_dump(void) __reentrant -{ - uint8_t i; - ao_mutex_get(&ao_gps_mutex); - printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day); - printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second); - printf ("Lat/Lon: %ld %ld\n", (long) ao_gps_data.latitude, (long) ao_gps_data.longitude); - printf ("Alt: %d\n", ao_gps_data.altitude); - printf ("Flags: 0x%x\n", ao_gps_data.flags); - printf ("Sats: %d", ao_gps_tracking_data.channels); - for (i = 0; i < ao_gps_tracking_data.channels; i++) - printf (" %d %d", - ao_gps_tracking_data.sats[i].svid, - ao_gps_tracking_data.sats[i].c_n_1); - printf ("\ndone\n"); - ao_mutex_put(&ao_gps_mutex); -} - static __code uint8_t ao_gps_115200[] = { SKYTRAQ_MSG_3(5,0,5,0) /* Set to 115200 baud */ }; @@ -532,7 +516,7 @@ gps_update(void) __reentrant } __code struct ao_cmds ao_gps_cmds[] = { - { gps_dump, "g\0Display GPS" }, + { ao_gps_show, "g\0Display GPS" }, { gps_update, "U\0Update GPS firmware" }, { 0, NULL }, }; diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 22300df3..4fb90746 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -21,16 +21,37 @@ #include "ao_gps_ublox.h" +#define AO_UBLOX_DEBUG 0 + +#include <stdarg.h> + +__xdata uint8_t ao_gps_new; __xdata uint8_t ao_gps_mutex; __pdata uint16_t ao_gps_tick; __xdata struct ao_telemetry_location ao_gps_data; __xdata struct ao_telemetry_satellite ao_gps_tracking_data; -static const char ao_gps_set_nmea[] = "\r\n$PUBX,41,1,3,1,57600,0*2d\r\n"; +#undef AO_SERIAL_SPEED_UBLOX -const char ao_gps_config[] = { +#ifndef AO_SERIAL_SPEED_UBLOX +#define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_9600 +#endif -}; +#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_57600 +#define SERIAL_SPEED_STRING "57600" +#define SERIAL_SPEED_CHECKSUM "2d" +#endif +#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_19200 +#define SERIAL_SPEED_STRING "19200" +#define SERIAL_SPEED_CHECKSUM "23" +#endif +#if AO_SERIAL_SPEED_UBLOX == AO_SERIAL_SPEED_9600 +#define SERIAL_SPEED_STRING "9600" +#define SERIAL_SPEED_CHECKSUM "16" +#endif + +static const char ao_gps_set_nmea[] = + "\r\n$PUBX,41,1,3,1," SERIAL_SPEED_STRING ",0*" SERIAL_SPEED_CHECKSUM "\r\n"; struct ao_ublox_cksum { uint8_t a, b; @@ -39,13 +60,34 @@ struct ao_ublox_cksum { static __pdata struct ao_ublox_cksum ao_ublox_cksum; static __pdata uint16_t ao_ublox_len; -#ifndef ao_ublox_getchar -#define ao_ublox_getchar ao_serial1_getchar -#define ao_ublox_putchar ao_serial1_putchar -#define ao_ublox_set_speed ao_serial1_set_speed +#if AO_UBLOX_DEBUG + +static uint8_t ao_gps_dbg_enable; + +#define DBG_PROTO 1 +#define DBG_CHAR 2 +#define DBG_INIT 4 + +static void ao_gps_dbg(int level, char *format, ...) { + va_list a; + + if (level & ao_gps_dbg_enable) { + va_start(a, format); + vprintf(format, a); + va_end(a); + flush(); + } +} + +#else +#define ao_gps_dbg(fmt, ...) #endif -#define ao_ublox_byte() ((uint8_t) ao_ublox_getchar()) +static inline uint8_t ao_ublox_byte(void) { + uint8_t c = (uint8_t) ao_gps_getchar(); + ao_gps_dbg(DBG_CHAR, " %02x", c); + return c; +} static inline void add_cksum(struct ao_ublox_cksum *cksum, uint8_t c) { @@ -61,7 +103,8 @@ static void ao_ublox_init_cksum(void) static void ao_ublox_put_u8(uint8_t c) { add_cksum(&ao_ublox_cksum, c); - ao_ublox_putchar(c); + ao_gps_dbg(DBG_CHAR, " (%02x)", c); + ao_gps_putchar(c); } static void ao_ublox_put_i8(int8_t c) @@ -183,6 +226,7 @@ ao_ublox_parse(void __xdata *target, const struct ublox_packet_parse *parse) __r break; } } + ao_gps_dbg(DBG_PROTO, "\n"); } /* @@ -326,6 +370,7 @@ ao_ublox_parse_nav_svinfo(void) { uint8_t nsat; nav_svinfo_nsat = 0; + ao_ublox_parse(&nav_svinfo, nav_svinfo_packet); for (nsat = 0; nsat < nav_svinfo.num_ch && ao_ublox_len >= 12; nsat++) { if (nsat < NAV_SVINFO_MAX_SAT) { @@ -334,6 +379,17 @@ ao_ublox_parse_nav_svinfo(void) ublox_discard(12); } } +#if AO_UBLOX_DEBUG + ao_gps_dbg(DBG_PROTO, "svinfo num_ch %d flags %02x\n", nav_svinfo.num_ch, nav_svinfo.flags); + for (nsat = 0; nsat < nav_svinfo.num_ch; nsat++) + ao_gps_dbg(DBG_PROTO, "\t%d: chn %d svid %d flags %02x quality %d cno %d\n", + nsat, + nav_svinfo_sat[nsat].chn, + nav_svinfo_sat[nsat].svid, + nav_svinfo_sat[nsat].flags, + nav_svinfo_sat[nsat].quality, + nav_svinfo_sat[nsat].cno); +#endif } /* @@ -402,45 +458,60 @@ ao_ublox_parse_nav_velned(void) */ static void -ao_gps_setup(void) +ao_gps_delay(void) { - uint8_t i, k; - ao_ublox_set_speed(AO_SERIAL_SPEED_9600); + uint8_t i; /* * A bunch of nulls so the start bit * is clear */ + for (i = 0; i < 64; i++) - ao_ublox_putchar(0x00); + ao_gps_putchar(0x00); +} + +static void +ao_gps_setup(void) +{ + uint8_t i, k; + + ao_delay(AO_SEC_TO_TICKS(3)); + + ao_gps_dbg(DBG_INIT, "Set speed 9600\n"); + ao_gps_set_speed(AO_SERIAL_SPEED_9600); /* * Send the baud-rate setting and protocol-setting * command three times */ - for (k = 0; k < 3; k++) + for (k = 0; k < 3; k++) { + ao_gps_delay(); + + ao_gps_dbg(DBG_INIT, "Send initial setting\n"); for (i = 0; i < sizeof (ao_gps_set_nmea); i++) - ao_ublox_putchar(ao_gps_set_nmea[i]); + ao_gps_putchar(ao_gps_set_nmea[i]); + } + + ao_gps_delay(); +#if AO_SERIAL_SPEED_UBLOX != AO_SERIAL_SPEED_9600 + ao_gps_dbg(DBG_INIT, "Set speed high\n"); /* * Increase the baud rate */ - ao_ublox_set_speed(AO_SERIAL_SPEED_57600); + ao_gps_set_speed(AO_SERIAL_SPEED_UBLOX); +#endif - /* - * Pad with nulls to give the chip - * time to see the baud rate switch - */ - for (i = 0; i < 64; i++) - ao_ublox_putchar(0x00); + ao_gps_delay(); } static void ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len) { ao_ublox_init_cksum(); - ao_ublox_putchar(0xb5); - ao_ublox_putchar(0x62); + ao_gps_putchar(0xb5); + ao_gps_putchar(0x62); ao_ublox_put_u8(class); ao_ublox_put_u8(id); ao_ublox_put_u8(len); @@ -450,8 +521,8 @@ ao_ublox_putstart(uint8_t class, uint8_t id, uint16_t len) static void ao_ublox_putend(void) { - ao_ublox_putchar(ao_ublox_cksum.a); - ao_ublox_putchar(ao_ublox_cksum.b); + ao_gps_putchar(ao_ublox_cksum.a); + ao_gps_putchar(ao_ublox_cksum.b); } static void @@ -547,7 +618,7 @@ ao_gps(void) __reentrant /* Enable all of the messages we want */ for (i = 0; i < sizeof (ublox_enable_nav); i++) ao_ublox_set_message_rate(UBLOX_NAV, ublox_enable_nav[i], 1); - + ao_ublox_set_navigation_settings((1 << UBLOX_CFG_NAV5_MASK_DYN) | (1 << UBLOX_CFG_NAV5_MASK_FIXMODE), UBLOX_CFG_NAV5_DYNMODEL_AIRBORNE_4G, UBLOX_CFG_NAV5_FIXMODE_3D, @@ -578,6 +649,8 @@ ao_gps(void) __reentrant ao_ublox_len = header_byte(); ao_ublox_len |= header_byte() << 8; + ao_gps_dbg(DBG_PROTO, "class %02x id %02x len %d\n", class, id, ao_ublox_len); + if (ao_ublox_len > 1023) continue; @@ -618,8 +691,10 @@ ao_gps(void) __reentrant break; } - if (ao_ublox_len != 0) + if (ao_ublox_len != 0) { + ao_gps_dbg(DBG_PROTO, "len left %d\n", ao_ublox_len); continue; + } /* verify checksum and end sequence */ cksum.a = ao_ublox_byte(); @@ -628,9 +703,9 @@ ao_gps(void) __reentrant continue; switch (class) { - case 0x01: + case UBLOX_NAV: switch (id) { - case 0x21: + case UBLOX_NAV_TIMEUTC: ao_mutex_get(&ao_gps_mutex); ao_gps_tick = ao_time(); @@ -645,7 +720,7 @@ ao_gps(void) __reentrant } if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC)) ao_gps_data.flags |= AO_GPS_DATE_VALID; - + ao_gps_data.altitude = nav_posllh.alt_msl / 1000; ao_gps_data.latitude = nav_posllh.lat; ao_gps_data.longitude = nav_posllh.lon; @@ -667,7 +742,7 @@ ao_gps(void) __reentrant ao_gps_data.ground_speed = nav_velned.g_speed; ao_gps_data.climb_rate = -nav_velned.vel_d; ao_gps_data.course = nav_velned.heading / 200000; - + ao_gps_tracking_data.channels = 0; struct ao_telemetry_satellite_info *dst = &ao_gps_tracking_data.sats[0]; @@ -686,8 +761,8 @@ ao_gps(void) __reentrant } ao_mutex_put(&ao_gps_mutex); - ao_wakeup(&ao_gps_data); - ao_wakeup(&ao_gps_tracking_data); + ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING; + ao_wakeup(&ao_gps_new); break; } break; @@ -695,10 +770,32 @@ ao_gps(void) __reentrant } } +#if AO_UBLOX_DEBUG +static void ao_gps_option(void) +{ + ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) { + ao_cmd_status = ao_cmd_success; + ao_gps_show(); + } else { + ao_gps_dbg_enable = ao_cmd_lex_i; + printf ("gps debug set to %d\n", ao_gps_dbg_enable); + } +} +#else +#define ao_gps_option ao_gps_show +#endif + +__code struct ao_cmds ao_gps_cmds[] = { + { ao_gps_option, "g\0Display GPS" }, + { 0, NULL }, +}; + __xdata struct ao_task ao_gps_task; void ao_gps_init(void) { + ao_cmd_register(&ao_gps_cmds[0]); ao_add_task(&ao_gps_task, ao_gps, "gps"); } diff --git a/src/drivers/ao_m25.c b/src/drivers/ao_m25.c index 390637d7..e6c7bb4d 100644 --- a/src/drivers/ao_m25.c +++ b/src/drivers/ao_m25.c @@ -82,11 +82,11 @@ __pdata uint16_t ao_storage_unit; #if M25_MAX_CHIPS > 1 static uint8_t ao_m25_size[M25_MAX_CHIPS]; /* number of sectors in each chip */ -static uint8_t ao_m25_pin[M25_MAX_CHIPS]; /* chip select pin for each chip */ +static ao_port_t ao_m25_pin[M25_MAX_CHIPS]; /* chip select pin for each chip */ static uint8_t ao_m25_numchips; /* number of chips detected */ #endif static uint8_t ao_m25_total; /* total sectors available */ -static uint8_t ao_m25_wip; /* write in progress */ +static ao_port_t ao_m25_wip; /* write in progress */ static __xdata uint8_t ao_m25_mutex; @@ -112,7 +112,7 @@ static __xdata uint8_t ao_m25_instruction[4]; * Block until the specified chip is done writing */ static void -ao_m25_wait_wip(uint8_t cs) +ao_m25_wait_wip(ao_port_t cs) { if (ao_m25_wip & cs) { M25_SELECT(cs); @@ -132,7 +132,7 @@ ao_m25_wait_wip(uint8_t cs) * so that future operations will block until the WIP bit goes off */ static void -ao_m25_write_enable(uint8_t cs) +ao_m25_write_enable(ao_port_t cs) { M25_SELECT(cs); ao_m25_instruction[0] = M25_WREN; @@ -146,7 +146,7 @@ ao_m25_write_enable(uint8_t cs) * Returns the number of 64kB sectors */ static uint8_t -ao_m25_read_capacity(uint8_t cs) +ao_m25_read_capacity(ao_port_t cs) { uint8_t capacity; M25_SELECT(cs); @@ -166,12 +166,13 @@ ao_m25_read_capacity(uint8_t cs) return 1 << (capacity - 0x10); } -static uint8_t +static ao_port_t ao_m25_set_address(uint32_t pos) { - uint8_t chip; + ao_port_t mask; #if M25_MAX_CHIPS > 1 uint8_t size; + uint8_t chip; for (chip = 0; chip < ao_m25_numchips; chip++) { size = ao_m25_size[chip]; @@ -182,16 +183,16 @@ ao_m25_set_address(uint32_t pos) if (chip == ao_m25_numchips) return 0xff; - chip = ao_m25_pin[chip]; + mask = ao_m25_pin[chip]; #else - chip = AO_M25_SPI_CS_MASK; + mask = AO_M25_SPI_CS_MASK; #endif - ao_m25_wait_wip(chip); + ao_m25_wait_wip(mask); ao_m25_instruction[1] = pos >> 16; ao_m25_instruction[2] = pos >> 8; ao_m25_instruction[3] = pos; - return chip; + return mask; } /* @@ -239,7 +240,7 @@ ao_m25_scan(void) uint8_t ao_storage_erase(uint32_t pos) __reentrant { - uint8_t cs; + ao_port_t cs; if (pos >= ao_storage_total || pos + ao_storage_block > ao_storage_total) return 0; @@ -249,7 +250,6 @@ ao_storage_erase(uint32_t pos) __reentrant cs = ao_m25_set_address(pos); - ao_m25_wait_wip(cs); ao_m25_write_enable(cs); ao_m25_instruction[0] = M25_SE; @@ -268,7 +268,7 @@ ao_storage_erase(uint32_t pos) __reentrant uint8_t ao_storage_device_write(uint32_t pos, __xdata void *d, uint16_t len) __reentrant { - uint8_t cs; + ao_port_t cs; if (pos >= ao_storage_total || pos + len > ao_storage_total) return 0; @@ -295,7 +295,7 @@ ao_storage_device_write(uint32_t pos, __xdata void *d, uint16_t len) __reentrant uint8_t ao_storage_device_read(uint32_t pos, __xdata void *d, uint16_t len) __reentrant { - uint8_t cs; + ao_port_t cs; if (pos >= ao_storage_total || pos + len > ao_storage_total) return 0; @@ -332,7 +332,7 @@ void ao_storage_device_info(void) __reentrant { #if M25_DEBUG - uint8_t cs; + ao_port_t cs; #endif #if M25_MAX_CHIPS > 1 uint8_t chip; diff --git a/src/drivers/ao_mma655x.c b/src/drivers/ao_mma655x.c index 28fe1e08..ce83a5a3 100644 --- a/src/drivers/ao_mma655x.c +++ b/src/drivers/ao_mma655x.c @@ -206,10 +206,7 @@ ao_mma655x_setup(void) ao_mma655x_reg_write(AO_MMA655X_AXISCFG, AXISCFG_VALUE | (1 << AO_MMA655X_AXISCFG_ST)); - for (i = 0; i < 10; i++) { - a_st = ao_mma655x_value(); - printf ("SELF-TEST %2d = %6d\n", i, a_st); - } + a_st = ao_mma655x_value(); stdefl = ao_mma655x_reg_read(AO_MMA655X_STDEFL); @@ -218,11 +215,6 @@ ao_mma655x_setup(void) (0 << AO_MMA655X_AXISCFG_ST)); a = ao_mma655x_value(); - for (i = 0; i < 10; i++) { - a = ao_mma655x_value(); - printf("NORMAL %2d = %6d\n", i, a); - } - ao_mma655x_reg_write(AO_MMA655X_DEVCFG, DEVCFG_VALUE | (1 << AO_MMA655X_DEVCFG_ENDINIT)); s0 = ao_mma655x_reg_read(AO_MMA655X_SN0); @@ -234,8 +226,6 @@ ao_mma655x_setup(void) serial = lot & 0x1fff; lot >>= 12; pn = ao_mma655x_reg_read(AO_MMA655X_PN); - printf ("MMA655X lot %d serial %d number %d\n", lot, serial, pn); - } uint16_t ao_mma655x_current; diff --git a/src/drivers/ao_mpu6000.c b/src/drivers/ao_mpu6000.c index c65aecbc..f8ce7346 100644 --- a/src/drivers/ao_mpu6000.c +++ b/src/drivers/ao_mpu6000.c @@ -19,84 +19,94 @@ #include <ao_mpu6000.h> #include <ao_exti.h> +#if HAS_MPU6000 + static uint8_t ao_mpu6000_wake; static uint8_t ao_mpu6000_configured; -#define ao_mpu6000_spi_get() ao_spi_get_bit(AO_MPU6000_SPI_CS_PORT, \ - AO_MPU6000_SPI_CS_PIN, \ - AO_MPU6000_SPI_CS, \ - AO_MPU6000_SPI_BUS, \ - AO_SPI_SPEED_1MHz) +#ifndef AO_MPU6000_I2C_INDEX +#define AO_MPU6000_SPI 1 +#else +#define AO_MPU6000_SPI 0 +#endif + +#if AO_MPU6000_SPI + +#define ao_mpu6000_spi_get() ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz) +#define ao_mpu6000_spi_put() ao_spi_put(AO_MPU6000_SPI_BUS) + +#define ao_mpu6000_spi_start() ao_spi_set_cs(AO_MPU6000_SPI_CS_PORT, \ + (1 << AO_MPU6000_SPI_CS_PIN)) + +#define ao_mpu6000_spi_end() ao_spi_clr_cs(AO_MPU6000_SPI_CS_PORT, \ + (1 << AO_MPU6000_SPI_CS_PIN)) -#define ao_mpu6000_spi_put() ao_spi_put_bit(AO_MPU6000_SPI_CS_PORT, \ - AO_MPU6000_SPI_CS_PIN, \ - AO_MPU6000_SPI_CS, \ - AO_MPU6000_SPI_BUS) +#endif static void -ao_mpu6000_reg_write(uint8_t addr, uint8_t value) +_ao_mpu6000_reg_write(uint8_t addr, uint8_t value) { uint8_t d[2] = { addr, value }; -#ifdef AO_MPU6000_I2C_INDEX +#if AO_MPU6000_SPI + ao_mpu6000_spi_start(); + ao_spi_send(d, 2, AO_MPU6000_SPI_BUS); + ao_mpu6000_spi_end(); +#else ao_i2c_get(AO_MPU6000_I2C_INDEX); ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE); ao_i2c_send(d, 2, AO_MPU6000_I2C_INDEX, TRUE); ao_i2c_put(AO_MPU6000_I2C_INDEX); -#else - ao_mpu6000_spi_get(); - ao_spi_send(d, 2, AO_MPU6000_SPI_BUS); - ao_mpu6000_spi_put(); #endif } static void -ao_mpu6000_read(uint8_t addr, void *data, uint8_t len) +_ao_mpu6000_read(uint8_t addr, void *data, uint8_t len) { -#ifdef AO_MPU6000_I2C_INDEX +#if AO_MPU6000_SPI + addr |= 0x80; + ao_mpu6000_spi_start(); + ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS); + ao_spi_recv(data, len, AO_MPU6000_SPI_BUS); + ao_mpu6000_spi_end(); +#else ao_i2c_get(AO_MPU6000_I2C_INDEX); ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE); ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE); ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ); ao_i2c_recv(data, len, AO_MPU6000_I2C_INDEX, TRUE); ao_i2c_put(AO_MPU6000_I2C_INDEX); -#else - addr |= 0x80; - ao_mpu6000_spi_get(); - ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS); - ao_spi_recv(data, len, AO_MPU6000_SPI_BUS); - ao_mpu6000_spi_put(); #endif } static uint8_t -ao_mpu6000_reg_read(uint8_t addr) +_ao_mpu6000_reg_read(uint8_t addr) { uint8_t value; -#ifdef AO_MPU6000_I2C_INDEX +#if AO_MPU6000_SPI + addr |= 0x80; + ao_mpu6000_spi_start(); + ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS); + ao_spi_recv(&value, 1, AO_MPU6000_SPI_BUS); + ao_mpu6000_spi_end(); +#else ao_i2c_get(AO_MPU6000_I2C_INDEX); ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_WRITE); ao_i2c_send(&addr, 1, AO_MPU6000_I2C_INDEX, FALSE); ao_i2c_start(AO_MPU6000_I2C_INDEX, MPU6000_ADDR_READ); ao_i2c_recv(&value, 1, AO_MPU6000_I2C_INDEX, TRUE); ao_i2c_put(AO_MPU6000_I2C_INDEX); -#else - addr |= 0x80; - ao_mpu6000_spi_get(); - ao_spi_send(&addr, 1, AO_MPU6000_SPI_BUS); - ao_spi_recv(&value, 1, AO_MPU6000_SPI_BUS); - ao_mpu6000_spi_put(); #endif return value; } static void -ao_mpu6000_sample(struct ao_mpu6000_sample *sample) +_ao_mpu6000_sample(struct ao_mpu6000_sample *sample) { uint16_t *d = (uint16_t *) sample; int i = sizeof (*sample) / 2; - ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample)); + _ao_mpu6000_read(MPU6000_ACCEL_XOUT_H, sample, sizeof (*sample)); #if __BYTE_ORDER == __LITTLE_ENDIAN /* byte swap */ while (i--) { @@ -108,6 +118,7 @@ ao_mpu6000_sample(struct ao_mpu6000_sample *sample) #define G 981 /* in cm/s² */ +#if 0 static int16_t /* cm/s² */ ao_mpu6000_accel(int16_t v) { @@ -119,16 +130,17 @@ ao_mpu6000_gyro(int16_t v) { return (int16_t) ((v * (int32_t) 20000) / 32767); } +#endif static uint8_t ao_mpu6000_accel_check(int16_t normal, int16_t test, char *which) { int16_t diff = test - normal; - if (diff < MPU6000_ST_ACCEL(16) / 2) { + if (diff < MPU6000_ST_ACCEL(16) / 4) { return 1; } - if (diff > MPU6000_ST_ACCEL(16) * 2) { + if (diff > MPU6000_ST_ACCEL(16) * 4) { return 1; } return 0; @@ -141,142 +153,168 @@ ao_mpu6000_gyro_check(int16_t normal, int16_t test, char *which) if (diff < 0) diff = -diff; - if (diff < MPU6000_ST_GYRO(2000) / 2) { + if (diff < MPU6000_ST_GYRO(2000) / 4) { return 1; } - if (diff > MPU6000_ST_GYRO(2000) * 2) { + if (diff > MPU6000_ST_GYRO(2000) * 4) { return 1; } return 0; } static void -ao_mpu6000_setup(void) +_ao_mpu6000_wait_alive(void) +{ + uint8_t i; + + /* Wait for the chip to wake up */ + for (i = 0; i < 30; i++) { + ao_delay(AO_MS_TO_TICKS(100)); + if (_ao_mpu6000_reg_read(MPU6000_WHO_AM_I) == 0x68) + break; + } + if (i == 30) + ao_panic(AO_PANIC_SELF_TEST_MPU6000); +} + +#define ST_TRIES 10 + +static void +_ao_mpu6000_setup(void) { struct ao_mpu6000_sample normal_mode, test_mode; - int errors =0; + int errors; + int st_tries; if (ao_mpu6000_configured) return; + _ao_mpu6000_wait_alive(); + /* Reset the whole chip */ - ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1, - (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET)); + _ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1, + (1 << MPU6000_PWR_MGMT_1_DEVICE_RESET)); /* Wait for it to reset. If we talk too quickly, it appears to get confused */ - ao_delay(AO_MS_TO_TICKS(100)); - /* Reset signal conditioning */ - ao_mpu6000_reg_write(MPU6000_USER_CONTROL, - (0 << MPU6000_USER_CONTROL_FIFO_EN) | - (0 << MPU6000_USER_CONTROL_I2C_MST_EN) | - (0 << MPU6000_USER_CONTROL_I2C_IF_DIS) | - (0 << MPU6000_USER_CONTROL_FIFO_RESET) | - (0 << MPU6000_USER_CONTROL_I2C_MST_RESET) | - (1 << MPU6000_USER_CONTROL_SIG_COND_RESET)); + _ao_mpu6000_wait_alive(); - while (ao_mpu6000_reg_read(MPU6000_USER_CONTROL) & (1 << MPU6000_USER_CONTROL_SIG_COND_RESET)) - ao_yield(); + /* Reset signal conditioning, disabling I2C on SPI systems */ + _ao_mpu6000_reg_write(MPU6000_USER_CTRL, + (0 << MPU6000_USER_CTRL_FIFO_EN) | + (0 << MPU6000_USER_CTRL_I2C_MST_EN) | + (AO_MPU6000_SPI << MPU6000_USER_CTRL_I2C_IF_DIS) | + (0 << MPU6000_USER_CTRL_FIFO_RESET) | + (0 << MPU6000_USER_CTRL_I2C_MST_RESET) | + (1 << MPU6000_USER_CTRL_SIG_COND_RESET)); + + while (_ao_mpu6000_reg_read(MPU6000_USER_CTRL) & (1 << MPU6000_USER_CTRL_SIG_COND_RESET)) + ao_delay(AO_MS_TO_TICKS(10)); /* Reset signal paths */ - ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET, - (1 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) | - (1 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) | - (1 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET)); + _ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET, + (1 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) | + (1 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) | + (1 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET)); - ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET, - (0 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) | - (0 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) | - (0 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET)); + _ao_mpu6000_reg_write(MPU6000_SIGNAL_PATH_RESET, + (0 << MPU6000_SIGNAL_PATH_RESET_GYRO_RESET) | + (0 << MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET) | + (0 << MPU6000_SIGNAL_PATH_RESET_TEMP_RESET)); /* Select clocks, disable sleep */ - ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1, - (0 << MPU6000_PWR_MGMT_1_DEVICE_RESET) | - (0 << MPU6000_PWR_MGMT_1_SLEEP) | - (0 << MPU6000_PWR_MGMT_1_CYCLE) | - (0 << MPU6000_PWR_MGMT_1_TEMP_DIS) | - (MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU6000_PWR_MGMT_1_CLKSEL)); + _ao_mpu6000_reg_write(MPU6000_PWR_MGMT_1, + (0 << MPU6000_PWR_MGMT_1_DEVICE_RESET) | + (0 << MPU6000_PWR_MGMT_1_SLEEP) | + (0 << MPU6000_PWR_MGMT_1_CYCLE) | + (0 << MPU6000_PWR_MGMT_1_TEMP_DIS) | + (MPU6000_PWR_MGMT_1_CLKSEL_PLL_X_AXIS << MPU6000_PWR_MGMT_1_CLKSEL)); - /* Set sample rate divider to sample at full speed - ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, 0); + /* Set sample rate divider to sample at full speed */ + _ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, 0); /* Disable filtering */ - ao_mpu6000_reg_write(MPU6000_CONFIG, - (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) | - (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG)); - - /* Configure accelerometer to +/-16G in self-test mode */ - ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG, - (1 << MPU600_ACCEL_CONFIG_XA_ST) | - (1 << MPU600_ACCEL_CONFIG_YA_ST) | - (1 << MPU600_ACCEL_CONFIG_ZA_ST) | - (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL)); - - /* Configure gyro to +/- 2000°/s in self-test mode */ - ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG, - (1 << MPU600_GYRO_CONFIG_XG_ST) | - (1 << MPU600_GYRO_CONFIG_YG_ST) | - (1 << MPU600_GYRO_CONFIG_ZG_ST) | - (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL)); - - ao_delay(AO_MS_TO_TICKS(200)); - ao_mpu6000_sample(&test_mode); + _ao_mpu6000_reg_write(MPU6000_CONFIG, + (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) | + (MPU6000_CONFIG_DLPF_CFG_260_256 << MPU6000_CONFIG_DLPF_CFG)); #if TRIDGE // read the product ID rev c has 1/2 the sensitivity of rev d - _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID); - //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id); - - if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) || - (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) { - // Accel scale 8g (4096 LSB/g) - // Rev C has different scaling than rev D - register_write(MPUREG_ACCEL_CONFIG,1<<3); - } else { - // Accel scale 8g (4096 LSB/g) - register_write(MPUREG_ACCEL_CONFIG,2<<3); - } - hal.scheduler->delay(1); - + _mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID); + //Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id); + + if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) || + (_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) { + // Accel scale 8g (4096 LSB/g) + // Rev C has different scaling than rev D + register_write(MPUREG_ACCEL_CONFIG,1<<3); + } else { + // Accel scale 8g (4096 LSB/g) + register_write(MPUREG_ACCEL_CONFIG,2<<3); + } + hal.scheduler->delay(1); #endif - /* Configure accelerometer to +/-16G */ - ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG, - (0 << MPU600_ACCEL_CONFIG_XA_ST) | - (0 << MPU600_ACCEL_CONFIG_YA_ST) | - (0 << MPU600_ACCEL_CONFIG_ZA_ST) | - (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL)); - - /* Configure gyro to +/- 2000°/s */ - ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG, - (0 << MPU600_GYRO_CONFIG_XG_ST) | - (0 << MPU600_GYRO_CONFIG_YG_ST) | - (0 << MPU600_GYRO_CONFIG_ZG_ST) | - (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL)); - - ao_delay(AO_MS_TO_TICKS(10)); - ao_mpu6000_sample(&normal_mode); + for (st_tries = 0; st_tries < ST_TRIES; st_tries++) { + errors = 0; + + /* Configure accelerometer to +/-16G in self-test mode */ + _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG, + (1 << MPU600_ACCEL_CONFIG_XA_ST) | + (1 << MPU600_ACCEL_CONFIG_YA_ST) | + (1 << MPU600_ACCEL_CONFIG_ZA_ST) | + (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL)); + + /* Configure gyro to +/- 2000°/s in self-test mode */ + _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG, + (1 << MPU600_GYRO_CONFIG_XG_ST) | + (1 << MPU600_GYRO_CONFIG_YG_ST) | + (1 << MPU600_GYRO_CONFIG_ZG_ST) | + (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL)); + + ao_delay(AO_MS_TO_TICKS(200)); + _ao_mpu6000_sample(&test_mode); + + /* Configure accelerometer to +/-16G */ + _ao_mpu6000_reg_write(MPU6000_ACCEL_CONFIG, + (0 << MPU600_ACCEL_CONFIG_XA_ST) | + (0 << MPU600_ACCEL_CONFIG_YA_ST) | + (0 << MPU600_ACCEL_CONFIG_ZA_ST) | + (MPU600_ACCEL_CONFIG_AFS_SEL_16G << MPU600_ACCEL_CONFIG_AFS_SEL)); + + /* Configure gyro to +/- 2000°/s */ + _ao_mpu6000_reg_write(MPU6000_GYRO_CONFIG, + (0 << MPU600_GYRO_CONFIG_XG_ST) | + (0 << MPU600_GYRO_CONFIG_YG_ST) | + (0 << MPU600_GYRO_CONFIG_ZG_ST) | + (MPU600_GYRO_CONFIG_FS_SEL_2000 << MPU600_GYRO_CONFIG_FS_SEL)); + + ao_delay(AO_MS_TO_TICKS(200)); + _ao_mpu6000_sample(&normal_mode); - errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x"); - errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y"); - errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z"); - - errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x"); - errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y"); - errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z"); + errors += ao_mpu6000_accel_check(normal_mode.accel_x, test_mode.accel_x, "x"); + errors += ao_mpu6000_accel_check(normal_mode.accel_y, test_mode.accel_y, "y"); + errors += ao_mpu6000_accel_check(normal_mode.accel_z, test_mode.accel_z, "z"); + + errors += ao_mpu6000_gyro_check(normal_mode.gyro_x, test_mode.gyro_x, "x"); + errors += ao_mpu6000_gyro_check(normal_mode.gyro_y, test_mode.gyro_y, "y"); + errors += ao_mpu6000_gyro_check(normal_mode.gyro_z, test_mode.gyro_z, "z"); + if (!errors) + break; + } - if (errors) - ao_panic(AO_PANIC_SELF_TEST_MPU6000); + if (st_tries == ST_TRIES) + ao_sensor_errors = 1; /* Filter to about 100Hz, which also sets the gyro rate to 1000Hz */ - ao_mpu6000_reg_write(MPU6000_CONFIG, - (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) | - (MPU6000_CONFIG_DLPF_CFG_94_98 << MPU6000_CONFIG_DLPF_CFG)); + _ao_mpu6000_reg_write(MPU6000_CONFIG, + (MPU6000_CONFIG_EXT_SYNC_SET_DISABLED << MPU6000_CONFIG_EXT_SYNC_SET) | + (MPU6000_CONFIG_DLPF_CFG_94_98 << MPU6000_CONFIG_DLPF_CFG)); /* Set sample rate divider to sample at 200Hz (v = gyro/rate - 1) */ - ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, - 1000 / 200 - 1); + _ao_mpu6000_reg_write(MPU6000_SMPRT_DIV, + 1000 / 200 - 1); ao_delay(AO_MS_TO_TICKS(100)); ao_mpu6000_configured = 1; @@ -287,10 +325,20 @@ struct ao_mpu6000_sample ao_mpu6000_current; static void ao_mpu6000(void) { - ao_mpu6000_setup(); + /* ao_mpu6000_init already grabbed the SPI bus and mutex */ + _ao_mpu6000_setup(); +#if AO_MPU6000_SPI + ao_mpu6000_spi_put(); +#endif for (;;) { - ao_mpu6000_sample(&ao_mpu6000_current); +#if AO_MPU6000_SPI + ao_mpu6000_spi_get(); +#endif + _ao_mpu6000_sample(&ao_mpu6000_current); +#if AO_MPU6000_SPI + ao_mpu6000_spi_put(); +#endif ao_arch_critical( AO_DATA_PRESENT(AO_DATA_MPU6000); AO_DATA_WAIT(); @@ -326,5 +374,20 @@ ao_mpu6000_init(void) ao_mpu6000_configured = 0; ao_add_task(&ao_mpu6000_task, ao_mpu6000, "mpu6000"); + +#if AO_MPU6000_SPI + ao_spi_init_cs(AO_MPU6000_SPI_CS_PORT, (1 << AO_MPU6000_SPI_CS_PIN)); + + /* Pretend to be the mpu6000 task. Grab the SPI bus right away and + * hold it for the task so that nothing else uses the SPI bus before + * we get the I2C mode disabled in the chip + */ + + ao_cur_task = &ao_mpu6000_task; + ao_spi_get(AO_MPU6000_SPI_BUS, AO_SPI_SPEED_1MHz); + ao_cur_task = NULL; +#endif + ao_cmd_register(&ao_mpu6000_cmds[0]); } +#endif diff --git a/src/drivers/ao_mpu6000.h b/src/drivers/ao_mpu6000.h index f01e9e83..dc3a9fbf 100644 --- a/src/drivers/ao_mpu6000.h +++ b/src/drivers/ao_mpu6000.h @@ -18,6 +18,10 @@ #ifndef _AO_MPU6000_H_ #define _AO_MPU6000_H_ +#ifndef M_PI +#define M_PI 3.1415926535897832384626433 +#endif + #define MPU6000_ADDR_WRITE 0xd0 #define MPU6000_ADDR_READ 0xd1 @@ -133,13 +137,13 @@ #define MPU6000_SIGNAL_PATH_RESET_ACCEL_RESET 1 #define MPU6000_SIGNAL_PATH_RESET_TEMP_RESET 0 -#define MPU6000_USER_CONTROL 0x6a -#define MPU6000_USER_CONTROL_FIFO_EN 6 -#define MPU6000_USER_CONTROL_I2C_MST_EN 5 -#define MPU6000_USER_CONTROL_I2C_IF_DIS 4 -#define MPU6000_USER_CONTROL_FIFO_RESET 2 -#define MPU6000_USER_CONTROL_I2C_MST_RESET 1 -#define MPU6000_USER_CONTROL_SIG_COND_RESET 0 +#define MPU6000_USER_CTRL 0x6a +#define MPU6000_USER_CTRL_FIFO_EN 6 +#define MPU6000_USER_CTRL_I2C_MST_EN 5 +#define MPU6000_USER_CTRL_I2C_IF_DIS 4 +#define MPU6000_USER_CTRL_FIFO_RESET 2 +#define MPU6000_USER_CTRL_I2C_MST_RESET 1 +#define MPU6000_USER_CTRL_SIG_COND_RESET 0 #define MPU6000_PWR_MGMT_1 0x6b #define MPU6000_PWR_MGMT_1_DEVICE_RESET 7 @@ -166,9 +170,20 @@ /* Self test gyro is approximately 50°/s */ #define MPU6000_ST_GYRO(full_scale) ((int16_t) (((int32_t) 32767 * (int32_t) 50) / (full_scale))) -#define MPU6000_GYRO_FULLSCALE 2000 +#define MPU6000_GYRO_FULLSCALE ((float) 2000 * M_PI/180.0) + +static inline float +ao_mpu6000_gyro(float sensor) { + return sensor * ((float) (MPU6000_GYRO_FULLSCALE / 32767.0)); +} + #define MPU6000_ACCEL_FULLSCALE 16 +static inline float +ao_mpu6000_accel(int16_t sensor) { + return (float) sensor * ((float) (MPU6000_ACCEL_FULLSCALE * GRAVITY / 32767.0)); +} + struct ao_mpu6000_sample { int16_t accel_x; int16_t accel_y; diff --git a/src/drivers/ao_ms5607.c b/src/drivers/ao_ms5607.c index bd57400e..58ab9197 100644 --- a/src/drivers/ao_ms5607.c +++ b/src/drivers/ao_ms5607.c @@ -21,19 +21,17 @@ #if HAS_MS5607 || HAS_MS5611 -static struct ao_ms5607_prom ms5607_prom; -static uint8_t ms5607_configured; +static __xdata struct ao_ms5607_prom ms5607_prom; +static __xdata uint8_t ms5607_configured; static void ao_ms5607_start(void) { - ao_spi_get(AO_MS5607_SPI_INDEX,AO_SPI_SPEED_FAST); - ao_gpio_set(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, 0); + ao_spi_get_bit(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, AO_MS5607_SPI_INDEX, AO_SPI_SPEED_FAST); } static void ao_ms5607_stop(void) { - ao_gpio_set(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, 1); - ao_spi_put(AO_MS5607_SPI_INDEX); + ao_spi_put_bit(AO_MS5607_CS_PORT, AO_MS5607_CS_PIN, AO_MS5607_CS, AO_MS5607_SPI_INDEX); } static void @@ -42,7 +40,7 @@ ao_ms5607_reset(void) { cmd = AO_MS5607_RESET; ao_ms5607_start(); - ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX); + ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX); ao_delay(AO_MS_TO_TICKS(10)); ao_ms5607_stop(); } @@ -71,17 +69,17 @@ ao_ms5607_crc(uint8_t *prom) } static void -ao_ms5607_prom_read(struct ao_ms5607_prom *prom) +ao_ms5607_prom_read(__xdata struct ao_ms5607_prom *prom) { - uint8_t addr; - uint8_t crc; - uint16_t *r; + uint8_t addr; + uint8_t crc; + __xdata uint16_t *r; - r = (uint16_t *) prom; + r = (__xdata uint16_t *) prom; for (addr = 0; addr < 8; addr++) { uint8_t cmd = AO_MS5607_PROM_READ(addr); ao_ms5607_start(); - ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX); + ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX); ao_spi_recv(r, 2, AO_MS5607_SPI_INDEX); ao_ms5607_stop(); r++; @@ -116,25 +114,25 @@ ao_ms5607_setup(void) ao_ms5607_prom_read(&ms5607_prom); } -static volatile uint8_t ao_ms5607_done; +static __xdata volatile uint8_t ao_ms5607_done; static void ao_ms5607_isr(void) { ao_exti_disable(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN); ao_ms5607_done = 1; - ao_wakeup((void *) &ao_ms5607_done); + ao_wakeup((__xdata void *) &ao_ms5607_done); } static uint32_t ao_ms5607_get_sample(uint8_t cmd) { - uint8_t reply[3]; - uint8_t read; + __xdata uint8_t reply[3]; + __xdata uint8_t read; ao_ms5607_done = 0; ao_ms5607_start(); - ao_spi_send(&cmd, 1, AO_MS5607_SPI_INDEX); + ao_spi_send(DATA_TO_XDATA(&cmd), 1, AO_MS5607_SPI_INDEX); ao_exti_enable(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN); @@ -142,7 +140,8 @@ ao_ms5607_get_sample(uint8_t cmd) { ao_spi_put(AO_MS5607_SPI_INDEX); #endif ao_arch_block_interrupts(); - while (!ao_ms5607_done) + while (!ao_gpio_get(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN, AO_MS5607_MISO) && + !ao_ms5607_done) ao_sleep((void *) &ao_ms5607_done); ao_arch_release_interrupts(); #if AO_MS5607_PRIVATE_PINS @@ -175,16 +174,20 @@ ao_ms5607_get_sample(uint8_t cmd) { #define AO_CONVERT_D2 token_evaluator(AO_MS5607_CONVERT_D2_, AO_MS5607_TEMP_OVERSAMPLE) void -ao_ms5607_sample(struct ao_ms5607_sample *sample) +ao_ms5607_sample(__xdata struct ao_ms5607_sample *sample) { sample->pres = ao_ms5607_get_sample(AO_CONVERT_D1); sample->temp = ao_ms5607_get_sample(AO_CONVERT_D2); } +#ifdef _CC1111_H_ +#include "ao_ms5607_convert_8051.c" +#else #include "ao_ms5607_convert.c" +#endif #if HAS_TASK -struct ao_ms5607_sample ao_ms5607_current; +__xdata struct ao_ms5607_sample ao_ms5607_current; static void ao_ms5607(void) @@ -193,10 +196,10 @@ ao_ms5607(void) for (;;) { ao_ms5607_sample(&ao_ms5607_current); - ao_arch_critical( - AO_DATA_PRESENT(AO_DATA_MS5607); - AO_DATA_WAIT(); - ); + ao_arch_block_interrupts(); + AO_DATA_PRESENT(AO_DATA_MS5607); + AO_DATA_WAIT(); + ao_arch_release_interrupts(); } } @@ -218,11 +221,11 @@ ao_ms5607_info(void) static void ao_ms5607_dump(void) { - struct ao_ms5607_value value; + __xdata struct ao_ms5607_value value; ao_ms5607_convert(&ao_ms5607_current, &value); - printf ("Pressure: %8u %8d\n", ao_ms5607_current.pres, value.pres); - printf ("Temperature: %8u %8d\n", ao_ms5607_current.temp, value.temp); + printf ("Pressure: %8lu %8ld\n", ao_ms5607_current.pres, value.pres); + printf ("Temperature: %8lu %8ld\n", ao_ms5607_current.temp, value.temp); printf ("Altitude: %ld\n", ao_pa_to_altitude(value.pres)); } @@ -249,17 +252,9 @@ ao_ms5607_init(void) */ ao_exti_setup(AO_MS5607_MISO_PORT, AO_MS5607_MISO_PIN, - AO_EXTI_MODE_RISING, + AO_EXTI_MODE_RISING| + AO_EXTI_PIN_NOCONFIGURE, ao_ms5607_isr); - -#ifdef STM_MODER_ALTERNATE - /* Reset the pin from INPUT to ALTERNATE so that SPI works - * This needs an abstraction at some point... - */ - stm_moder_set(AO_MS5607_MISO_PORT, - AO_MS5607_MISO_PIN, - STM_MODER_ALTERNATE); -#endif } #endif diff --git a/src/drivers/ao_ms5607.h b/src/drivers/ao_ms5607.h index b2f98a59..206efd64 100644 --- a/src/drivers/ao_ms5607.h +++ b/src/drivers/ao_ms5607.h @@ -56,7 +56,7 @@ struct ao_ms5607_value { int32_t temp; /* in °C * 100 */ }; -extern struct ao_ms5607_sample ao_ms5607_current; +extern __xdata struct ao_ms5607_sample ao_ms5607_current; void ao_ms5607_setup(void); @@ -68,12 +68,13 @@ void ao_ms5607_info(void); void -ao_ms5607_sample(struct ao_ms5607_sample *sample); +ao_ms5607_sample(__xdata struct ao_ms5607_sample *sample); void -ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value); +ao_ms5607_convert(__xdata struct ao_ms5607_sample *sample, + __xdata struct ao_ms5607_value *value); void -ao_ms5607_get_prom(struct ao_ms5607_prom *prom); +ao_ms5607_get_prom(__data struct ao_ms5607_prom *prom); #endif /* _AO_MS5607_H_ */ diff --git a/src/drivers/ao_ms5607_convert.c b/src/drivers/ao_ms5607_convert.c index e61d19ed..bfb952a4 100644 --- a/src/drivers/ao_ms5607_convert.c +++ b/src/drivers/ao_ms5607_convert.c @@ -42,11 +42,14 @@ ao_ms5607_convert(struct ao_ms5607_sample *sample, struct ao_ms5607_value *value int32_t TEMPM = TEMP - 2000; int64_t OFF2 = (61 * (int64_t) TEMPM * (int64_t) TEMPM) >> 4; int64_t SENS2 = 2 * (int64_t) TEMPM * (int64_t) TEMPM; - if (TEMP < 1500) { + if (TEMP < -1500) { int32_t TEMPP = TEMP + 1500; - int64_t TEMPP2 = TEMPP * TEMPP; - OFF2 = OFF2 + 15 * TEMPP2; - SENS2 = SENS2 + 8 * TEMPP2; + /* You'd think this would need a 64-bit int, but + * that would imply a temperature below -327.67°C... + */ + int32_t TEMPP2 = TEMPP * TEMPP; + OFF2 = OFF2 + (int64_t) 15 * TEMPP2; + SENS2 = SENS2 + (int64_t) 8 * TEMPP2; } TEMP -= T2; OFF -= OFF2; diff --git a/src/drivers/ao_ms5607_convert_8051.c b/src/drivers/ao_ms5607_convert_8051.c new file mode 100644 index 00000000..f3a48c46 --- /dev/null +++ b/src/drivers/ao_ms5607_convert_8051.c @@ -0,0 +1,136 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao_ms5607.h> +#include <ao_int64.h> + +#if HAS_MS5611 +#define SHIFT_OFF 16 +#define SHIFT_TCO 7 +#define SHIFT_SENS 15 +#define SHFIT_TCS 8 +#else +#define SHIFT_OFF 17 +#define SHIFT_TCO 6 +#define SHIFT_SENS 16 +#define SHIFT_TCS 7 +#endif + +void +ao_ms5607_convert(__xdata struct ao_ms5607_sample *sample, + __xdata struct ao_ms5607_value *value) +{ + __LOCAL int32_t dT; + __LOCAL int32_t TEMP; + __LOCAL ao_int64_t OFF; + __LOCAL ao_int64_t SENS; + __LOCAL ao_int64_t a; + + dT = sample->temp - ((int32_t) ms5607_prom.tref << 8); + + /* TEMP = 2000 + (((int64_t) dT * ms5607_prom.tempsens) >> 23); */ + ao_mul64_32_32(&a, dT, ms5607_prom.tempsens); + ao_rshift64(&a, &a, 23); + TEMP = 2000 + a.low; + /* */ + + /* OFF = ((int64_t) ms5607_prom.off << SHIFT_OFF) + (((int64_t) ms5607_prom.tco * dT) >> SHIFT_TCO);*/ +#if SHIFT_OFF > 16 + OFF.high = ms5607_prom.off >> (32 - SHIFT_OFF); +#else + OFF.high = 0; +#endif + OFF.low = (uint32_t) ms5607_prom.off << SHIFT_OFF; + ao_mul64_32_32(&a, ms5607_prom.tco, dT); + ao_rshift64(&a, &a, SHIFT_TCO); + ao_plus64(&OFF, &OFF, &a); + /**/ + + /* SENS = ((int64_t) ms5607_prom.sens << SHIFT_SENS) + (((int64_t) ms5607_prom.tcs * dT) >> SHIFT_TCS); */ + SENS.high = 0; + SENS.low = (uint32_t) ms5607_prom.sens << SHIFT_SENS; + ao_mul64_32_32(&a, ms5607_prom.tcs, dT); + ao_rshift64(&a, &a, SHIFT_TCS); + ao_plus64(&SENS, &SENS, &a); + /**/ + + if (TEMP < 2000) { + __LOCAL int32_t T2; + __LOCAL int32_t TEMPM; + __LOCAL ao_int64_t OFF2; + __LOCAL ao_int64_t SENS2; + + /* T2 = ((int64_t) dT * (int64_t) dT) >> 31; */ + ao_mul64_32_32(&a, dT, dT); + T2 = (a.low >> 31) | (a.high << 1); + /**/ + + TEMPM = TEMP - 2000; + + /* OFF2 = (61 * (int64_t) TEMPM * (int64_t) TEMPM) >> 4; */ + ao_mul64_32_32(&OFF2, TEMPM, TEMPM); + ao_mul64_64_16(&OFF2, &OFF2, 61); + ao_rshift64(&OFF2, &OFF2, 4); + /**/ + + /* SENS2 = 2 * (int64_t) TEMPM * (int64_t) TEMPM; */ + ao_mul64_32_32(&SENS2, TEMPM, TEMPM); + ao_lshift64(&SENS2, &SENS2, 1); + /**/ + + if (TEMP < -1500) { + int32_t TEMPP; + int32_t TEMPP2; + + TEMPP = TEMP + 1500; + TEMPP2 = TEMPP * TEMPP; + + /* OFF2 = OFF2 + 15 * TEMPP2; */ + ao_mul64_32_32(&a, 15, TEMPP2); + ao_plus64(&OFF2, &OFF2, &a); + /**/ + + /* SENS2 = SENS2 + 8 * TEMPP2; */ + a.high = 0; + a.low = TEMPP2; + ao_lshift64(&a, &a, 3); + ao_plus64(&SENS2, &SENS2, &a); + /**/ + } + TEMP -= T2; + + /* OFF -= OFF2; */ + ao_minus64(&OFF, &OFF, &OFF2); + /**/ + + /* SENS -= SENS2; */ + ao_minus64(&SENS, &SENS, &SENS2); + /**/ + } + + /* value->pres = ((((int64_t) sample->pres * SENS) >> 21) - OFF) >> 15; */ + a.high = 0; + a.low = sample->pres; + ao_mul64(&a, &a, &SENS); + ao_rshift64(&a, &a, 21); + ao_minus64(&a, &a, &OFF); + ao_rshift64(&a, &a, 15); + value->pres = a.low; + /**/ + + value->temp = TEMP; +} diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c index 6cec98ab..62ae68e9 100644 --- a/src/drivers/ao_pad.c +++ b/src/drivers/ao_pad.c @@ -17,7 +17,7 @@ #include <ao.h> #include <ao_pad.h> -#include <ao_74hc497.h> +#include <ao_74hc165.h> #include <ao_radio_cmac.h> static __xdata uint8_t ao_pad_ignite; @@ -27,6 +27,7 @@ static __pdata uint8_t ao_pad_armed; static __pdata uint16_t ao_pad_arm_time; static __pdata uint8_t ao_pad_box; static __xdata uint8_t ao_pad_disabled; +static __pdata uint16_t ao_pad_packet_time; #define DEBUG 1 @@ -135,6 +136,12 @@ ao_pad_monitor(void) query.arm_status = AO_PAD_ARM_STATUS_UNKNOWN; arm_beep_time = 0; } + if ((ao_time() - ao_pad_packet_time) > AO_SEC_TO_TICKS(2)) + cur |= AO_LED_RED; + else if (ao_radio_cmac_rssi < -90) + cur |= AO_LED_AMBER; + else + cur |= AO_LED_GREEN; for (c = 0; c < AO_PAD_NUM; c++) { int16_t sense = packet->adc.sense[c]; @@ -171,9 +178,10 @@ ao_pad_monitor(void) query.igniter_status[c] = status; } if (cur != prev) { - PRINTD("change leds from %02x to %02x mask %02x\n", - prev, cur, AO_LED_CONTINUITY_MASK|AO_LED_ARMED); - ao_led_set_mask(cur, AO_LED_CONTINUITY_MASK | AO_LED_ARMED); + PRINTD("change leds from %02x to %02x\n", + prev, cur); + FLUSHD(); + ao_led_set(cur); prev = cur; } @@ -182,10 +190,7 @@ ao_pad_monitor(void) if (ao_pad_armed) { ao_strobe(1); - if (sample & 2) - ao_siren(1); - else - ao_siren(0); + ao_siren(1); beeping = 1; } else if (query.arm_status == AO_PAD_ARM_STATUS_ARMED && !beeping) { if (arm_beep_time == 0) { @@ -218,6 +223,21 @@ ao_pad_enable(void) ao_wakeup (&ao_pad_disabled); } +#if HAS_74HC165 +static uint8_t +ao_pad_read_box(void) +{ + uint8_t byte = ao_74hc165_read(); + uint8_t h, l; + + h = byte >> 4; + l = byte & 0xf; + return h * 10 + l; +} +#else +#define ao_pad_read_box() 0 +#endif + static void ao_pad(void) { @@ -226,18 +246,20 @@ ao_pad(void) ao_pad_box = 0; ao_led_set(0); - ao_led_on(AO_LED_POWER); for (;;) { FLUSHD(); while (ao_pad_disabled) ao_sleep(&ao_pad_disabled); ret = ao_radio_cmac_recv(&command, sizeof (command), 0); - PRINTD ("cmac_recv %d\n", ret); + PRINTD ("cmac_recv %d %d\n", ret, ao_radio_cmac_rssi); if (ret != AO_RADIO_CMAC_OK) continue; + ao_pad_packet_time = ao_time(); - PRINTD ("tick %d box %d cmd %d channels %02x\n", - command.tick, command.box, command.cmd, command.channels); + ao_pad_box = ao_pad_read_box(); + + PRINTD ("tick %d box %d (me %d) cmd %d channels %02x\n", + command.tick, command.box, ao_pad_box, command.cmd, command.channels); switch (command.cmd) { case AO_LAUNCH_ARM: @@ -327,7 +349,7 @@ ao_pad_test(void) } for (c = 0; c < AO_PAD_NUM; c++) { - printf ("Pad %d: "); + printf ("Pad %d: ", c); switch (query.igniter_status[c]) { case AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_CLOSED: printf ("No igniter. Relay closed\n"); break; case AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN: printf ("No igniter. Relay open\n"); break; @@ -362,6 +384,26 @@ ao_pad_set_debug(void) if (ao_cmd_status == ao_cmd_success) ao_pad_debug = ao_cmd_lex_i != 0; } + + +static void +ao_pad_alarm_debug(void) +{ + uint8_t which, value; + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + which = ao_cmd_lex_i; + ao_cmd_decimal(); + if (ao_cmd_status != ao_cmd_success) + return; + value = ao_cmd_lex_i; + printf ("Set %s to %d\n", which ? "siren" : "strobe", value); + if (which) + ao_siren(value); + else + ao_strobe(value); +} #endif __code struct ao_cmds ao_pad_cmds[] = { @@ -369,6 +411,7 @@ __code struct ao_cmds ao_pad_cmds[] = { { ao_pad_manual, "i <key> <n>\0Fire igniter. <key> is doit with D&I" }, #if DEBUG { ao_pad_set_debug, "D <0 off, 1 on>\0Debug" }, + { ao_pad_alarm_debug, "S <0 strobe, 1 siren> <0 off, 1 on>\0Set alarm output" }, #endif { 0, NULL } }; diff --git a/src/drivers/ao_pca9922.c b/src/drivers/ao_pca9922.c index 6d8d18d8..d376b968 100644 --- a/src/drivers/ao_pca9922.c +++ b/src/drivers/ao_pca9922.c @@ -30,9 +30,12 @@ ao_led_apply(void) /* Don't try the SPI bus during initialization */ if (!ao_cur_task) return; - ao_spi_get_bit(AO_PCA9922_CS_PORT, AO_PCA9922_CS_PIN, AO_PCA9922_CS, AO_PCA9922_SPI_BUS, AO_SPI_SPEED_FAST); + ao_spi_get(AO_PCA9922_SPI_BUS); + ao_spi_set_speed(AO_PCA9922_SPI_BUS,AO_SPI_SPEED_FAST); + AO_PCA9922_CS = 1; ao_spi_send(&ao_led_state, 1, AO_PCA9922_SPI_BUS); - ao_spi_put_bit(AO_PCA9922_CS_PORT, AO_PCA9922_CS_PIN, AO_PCA9922_CS, AO_PCA9922_SPI_BUS); + AO_PCA9922_CS = 0; + ao_spi_put(AO_PCA9922_SPI_BUS); } void diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index 6cc2467a..d07488d0 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -18,16 +18,12 @@ #include <ao.h> #include <ao_quadrature.h> #include <ao_exti.h> -#if AO_EVENT +#include <ao_fast_timer.h> #include <ao_event.h> -#define ao_quadrature_queue(q) ao_event_put_isr(AO_EVENT_QUADRATURE, q, ao_quadrature_count[q]) -#else -#define ao_quadrature_queue(q) -#endif __xdata int32_t ao_quadrature_count[AO_QUADRATURE_COUNT]; - static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT]; +static int8_t ao_quadrature_raw[AO_QUADRATURE_COUNT]; #define BIT(a,b) ((a) | ((b) << 1)) #define STATE(old_a, old_b, new_a, new_b) (((BIT(old_a, old_b) << 2) | BIT(new_a, new_b))) @@ -35,41 +31,76 @@ static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT]; #define port(q) AO_QUADRATURE_ ## q ## _PORT #define bita(q) AO_QUADRATURE_ ## q ## _A #define bitb(q) AO_QUADRATURE_ ## q ## _B +#define pina(q) AO_QUADRATURE_ ## q ## _A ## _PIN +#define pinb(q) AO_QUADRATURE_ ## q ## _B ## _PIN +#define isr(q) ao_quadrature_isr_ ## q -#define ao_quadrature_update(q) do { \ - ao_quadrature_state[q] = ((ao_quadrature_state[q] & 3) << 2); \ - ao_quadrature_state[q] |= ao_gpio_get(port(q), bita(q), 0); \ - ao_quadrature_state[q] |= ao_gpio_get(port(q), bitb(q), 0) << 1; \ - } while (0) - +static inline uint16_t +ao_quadrature_read(struct stm_gpio *gpio, uint8_t pin_a, uint8_t pin_b) { + uint16_t v = stm_gpio_get_all(gpio); + + return ~((((v >> pin_a) & 1) | (((v >> pin_b) & 1) << 1))) & 3; +} + +#define _ao_quadrature_get(q) ao_quadrature_read(port(q), bita(q), bitb(q)) static void -ao_quadrature_isr(void) +_ao_quadrature_queue(uint8_t q, int8_t step) { - uint8_t q; -#if AO_QUADRATURE_COUNT > 0 - ao_quadrature_update(0); -#endif -#if AO_QUADRATURE_COUNT > 1 - ao_quadrature_update(1); + ao_quadrature_count[q] += step; +#if AO_EVENT + ao_event_put_isr(AO_EVENT_QUADRATURE, q, step); #endif + ao_wakeup(&ao_quadrature_count[q]); +} - for (q = 0; q < AO_QUADRATURE_COUNT; q++) { - switch (ao_quadrature_state[q]) { - case STATE(0, 1, 0, 0): - ao_quadrature_count[q]++; - break; - case STATE(1, 0, 0, 0): - ao_quadrature_count[q]--; - break; - default: - continue; - } - ao_quadrature_queue(q); - ao_wakeup(&ao_quadrature_count[q]); +static const int8_t step[16] = { + [STATE(0,0,0,0)] = 0, + [STATE(0,0,0,1)] = -1, + [STATE(0,0,1,0)] = 1, + [STATE(0,0,1,1)] = 0, + [STATE(0,1,0,0)] = 1, + [STATE(0,1,1,0)] = 0, + [STATE(0,1,1,1)] = -1, + [STATE(1,0,0,0)] = -1, + [STATE(1,0,0,1)] = 0, + [STATE(1,0,1,0)] = 0, + [STATE(1,0,1,1)] = 1, + [STATE(1,1,0,0)] = 0, + [STATE(1,1,0,1)] = 1, + [STATE(1,1,1,0)] = -1, + [STATE(1,1,1,1)] = 0 +}; + +static void +_ao_quadrature_set(uint8_t q, uint8_t value) { + uint8_t v; + + v = ao_quadrature_state[q] & 3; + value = value & 3; + + if (v == value) + return; + + ao_quadrature_state[q] = (v << 2) | value; + + ao_quadrature_raw[q] += step[ao_quadrature_state[q]]; + if (value == 0) { + if (ao_quadrature_raw[q] == 4) + _ao_quadrature_queue(q, 1); + else if (ao_quadrature_raw[q] == -4) + _ao_quadrature_queue(q, -1); + ao_quadrature_raw[q] = 0; } } +static void +ao_quadrature_isr(void) +{ + _ao_quadrature_set(0, _ao_quadrature_get(0)); + _ao_quadrature_set(1, _ao_quadrature_get(1)); +} + int32_t ao_quadrature_poll(uint8_t q) { @@ -92,6 +123,15 @@ ao_quadrature_test(void) ao_cmd_decimal(); q = ao_cmd_lex_i; + if (q >= AO_QUADRATURE_COUNT) { + ao_cmd_status = ao_cmd_syntax_error; + return; + } + printf ("count %d state %x raw %d\n", + ao_quadrature_count[q], + ao_quadrature_state[q], + ao_quadrature_raw[q]); +#if 0 for (;;) { int32_t c; flush(); @@ -100,6 +140,7 @@ ao_quadrature_test(void) if (c == 100) break; } +#endif } static const struct ao_cmds ao_quadrature_cmds[] = { @@ -107,18 +148,9 @@ static const struct ao_cmds ao_quadrature_cmds[] = { { 0, NULL } }; -#define init(q) do { \ - ao_enable_port(port(q)); \ - \ - ao_exti_setup(port(q), bita(q), \ - AO_QUADRATURE_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \ - ao_quadrature_isr); \ - ao_exti_enable(port(q), bita(q)); \ - \ - ao_exti_setup(port(q), bitb(q), \ - AO_QUADRATURE_MODE|AO_EXTI_MODE_FALLING|AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_MED, \ - ao_quadrature_isr); \ - ao_exti_enable(port(q), bitb(q)); \ +#define init(q) do { \ + ao_enable_input(port(q), bita(q), 0); \ + ao_enable_input(port(q), bitb(q), 0); \ } while (0) void @@ -130,5 +162,7 @@ ao_quadrature_init(void) #if AO_QUADRATURE_COUNT > 1 init(1); #endif + ao_fast_timer_init(); + ao_fast_timer_on(ao_quadrature_isr); ao_cmd_register(&ao_quadrature_cmds[0]); } diff --git a/src/drivers/ublox-csum.5c b/src/drivers/ublox-csum.5c new file mode 100644 index 00000000..4e0c7c5a --- /dev/null +++ b/src/drivers/ublox-csum.5c @@ -0,0 +1,23 @@ +#!/usr/bin/nickle +string[] speeds = { "57600", "19200", "9600" }; + +string make_set_nmea(string speed) { + return sprintf ("PUBX,41,1,3,1,%s,0", speed); +} + +int csum(string x) { + int csum = 0; + for (int i = 0; i < String::length(x); i++) + csum ^= x[i]; + return csum; +} + +for (int i = 0; i < dim(speeds); i++) { + string s = make_set_nmea(speeds[i]); + int c = csum(s); + printf ("/* $%s* */\n", s); + printf ("#define SERIAL_SPEED_STRING \"%s\"\n", speeds[i]); + printf ("#define SERIAL_SPEED_CHECKSUM \"%02x\"\n", c); +} + + diff --git a/src/easymini-v1.0/.gitignore b/src/easymini-v1.0/.gitignore new file mode 100644 index 00000000..e5f7d586 --- /dev/null +++ b/src/easymini-v1.0/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +*.elf diff --git a/src/easymini-v1.0/Makefile b/src/easymini-v1.0/Makefile new file mode 100644 index 00000000..7dae2692 --- /dev/null +++ b/src/easymini-v1.0/Makefile @@ -0,0 +1,83 @@ +# +# AltOS build +# +# + +include ../lpc/Makefile.defs + +INC = \ + ao.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_pins.h \ + ao_product.h \ + lpc.h + +# +# Common AltOS sources +# +ALTOS_SRC = \ + ao_interrupt.c \ + ao_boot_chain.c \ + ao_romconfig.c \ + ao_product.c \ + ao_mutex.c \ + ao_panic.c \ + ao_stdio.c \ + ao_storage.c \ + ao_report.c \ + ao_ignite.c \ + ao_flight.c \ + ao_kalman.c \ + ao_sample.c \ + ao_data.c \ + ao_convert_pa.c \ + ao_task.c \ + ao_log.c \ + ao_log_mini.c \ + ao_cmd.c \ + ao_config.c \ + ao_timer_lpc.c \ + ao_exti_lpc.c \ + ao_usb_lpc.c \ + ao_spi_lpc.c \ + ao_adc_lpc.c \ + ao_beep_lpc.c \ + ao_m25.c \ + ao_ms5607.c + +PRODUCT=EasyMini-v1.0 +PRODUCT_DEF=-DEASYMINI_V_1_0 +IDPRODUCT=0x0026 + +CFLAGS = $(PRODUCT_DEF) $(LPC_CFLAGS) -g -Os + +PROGNAME=easymini-v1.0 +PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx + +SRC=$(ALTOS_SRC) ao_easymini.c +OBJ=$(SRC:.c=.o) + +all: $(PROG) $(HEX) + +$(PROG): Makefile $(OBJ) altos.ld + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) + +ao_product.h: ao-make-product.5c ../Version + $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ + +$(OBJ): $(INC) + +load: $(PROG) + lpc-load $(PROG) + +distclean: clean + +clean: + rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx + rm -f ao_product.h + +install: + +uninstall: diff --git a/src/easymini-v1.0/ao_easymini.c b/src/easymini-v1.0/ao_easymini.c new file mode 100644 index 00000000..97230b61 --- /dev/null +++ b/src/easymini-v1.0/ao_easymini.c @@ -0,0 +1,46 @@ +/* + * Copyright © 2011 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_exti.h> + +void +main(void) +{ + ao_clock_init(); + ao_task_init(); + ao_timer_init(); + ao_exti_init(); + + ao_beep_init(); + + ao_adc_init(); + ao_spi_init(); + ao_storage_init(); + + ao_usb_init(); + + ao_cmd_init(); + ao_flight_init(); + ao_ms5607_init(); + ao_log_init(); + ao_report_init(); + ao_igniter_init(); + ao_config_init(); + + ao_start_scheduler(); +} diff --git a/src/easymini-v1.0/ao_pins.h b/src/easymini-v1.0/ao_pins.h new file mode 100644 index 00000000..e721030d --- /dev/null +++ b/src/easymini-v1.0/ao_pins.h @@ -0,0 +1,136 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#define HAS_BEEP 1 +#define HAS_LED 0 + +#define AO_STACK_SIZE 384 + +#define IS_FLASH_LOADER 0 + +/* Crystal on the board */ +#define AO_LPC_CLKIN 12000000 + +/* Main clock frequency. 48MHz for USB so we don't use the USB PLL */ +#define AO_LPC_CLKOUT 48000000 + +/* System clock frequency */ +#define AO_LPC_SYSCLK 24000000 + +#define HAS_USB 1 + +#define HAS_USB_CONNECT 0 +#define HAS_USB_VBUS 0 +#define HAS_USB_PULLUP 1 +#define AO_USB_PULLUP_PORT 0 +#define AO_USB_PULLUP_PIN 20 + +#define PACKET_HAS_SLAVE 0 + +#define AO_LOG_FORMAT AO_LOG_FORMAT_EASYMINI + +/* USART */ + +#define HAS_SERIAL 0 +#define USE_SERIAL_0_STDIN 1 +#define SERIAL_0_18_19 1 +#define SERIAL_0_14_15 0 +#define SERIAL_0_17_18 0 +#define SERIAL_0_26_27 0 + +/* SPI */ + +#define HAS_SPI_0 1 +#define SPI_SCK0_P0_6 1 +#define HAS_SPI_1 1 +#define SPI_SCK1_P1_15 1 +#define SPI_MISO1_P0_22 1 +#define SPI_MOSI1_P0_21 1 + +/* M25 */ + +#define M25_MAX_CHIPS 1 +#define AO_M25_SPI_CS_PORT 0 +#define AO_M25_SPI_CS_MASK (1 << 23) +#define AO_M25_SPI_BUS 1 + +/* MS5607 */ + +#define HAS_MS5607 1 +#define HAS_MS5611 0 +#define AO_MS5607_PRIVATE_PINS 0 +#define AO_MS5607_CS_PORT 0 +#define AO_MS5607_CS_PIN 7 +#define AO_MS5607_CS_MASK (1 << AO_MS5607_CS_PIN) +#define AO_MS5607_MISO_PORT 0 +#define AO_MS5607_MISO_PIN 8 +#define AO_MS5607_MISO_MASK (1 << AO_MS5607_MISO_PIN) +#define AO_MS5607_SPI_INDEX 0 + +#define HAS_ACCEL 0 +#define HAS_GPS 0 +#define HAS_RADIO 0 +#define HAS_FLIGHT 1 +#define HAS_EEPROM 1 +#define HAS_TELEMETRY 0 +#define HAS_APRS 0 +#define HAS_LOG 1 +#define USE_INTERNAL_FLASH 0 +#define HAS_IGNITE 1 +#define HAS_IGNITE_REPORT 1 + +#define AO_DATA_RING 16 + +/* + * ADC + */ + +#define HAS_ADC 1 + +#define AO_NUM_ADC 3 + +#define AO_ADC_0 1 +#define AO_ADC_1 1 +#define AO_ADC_2 1 + +struct ao_adc { + int16_t sense_a; + int16_t sense_m; + int16_t v_batt; +}; + +/* + * Igniter + */ + +#define AO_IGNITER_CLOSED 400 +#define AO_IGNITER_OPEN 60 + +#define AO_IGNITER_DROGUE_PORT 0 +#define AO_IGNITER_DROGUE_PIN 2 +#define AO_IGNITER_SET_DROGUE(v) ao_gpio_set(AO_IGNITER_DROGUE_PORT, AO_IGNITER_DROGUE_PIN, AO_IGNITER_DROGUE, v) + +#define AO_IGNITER_MAIN_PORT 0 +#define AO_IGNITER_MAIN_PIN 3 +#define AO_IGNITER_SET_MAIN(v) ao_gpio_set(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, AO_IGNITER_MAIN, v) + +#define AO_SENSE_DROGUE(p) ((p)->adc.sense_a) +#define AO_SENSE_MAIN(p) ((p)->adc.sense_m) + +#define AO_ADC_DUMP(p) \ + printf("tick: %5u apogee: %5d main: %5d batt: %5d\n", \ + (p)->tick, (p)->adc.sense_a, (p)->adc.sense_m, (p)->adc.v_batt) diff --git a/src/easymini-v1.0/flash-loader/Makefile b/src/easymini-v1.0/flash-loader/Makefile new file mode 100644 index 00000000..78bb4092 --- /dev/null +++ b/src/easymini-v1.0/flash-loader/Makefile @@ -0,0 +1,8 @@ +# +# AltOS flash loader build +# +# + +TOPDIR=../.. +HARDWARE=easymini-v1.0 +include $(TOPDIR)/lpc/Makefile-flash.defs diff --git a/src/easymini-v1.0/flash-loader/ao_pins.h b/src/easymini-v1.0/flash-loader/ao_pins.h new file mode 100644 index 00000000..4330151d --- /dev/null +++ b/src/easymini-v1.0/flash-loader/ao_pins.h @@ -0,0 +1,33 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +#include <ao_flash_lpc_pins.h> + +#define AO_BOOT_PIN 1 +#define AO_BOOT_APPLICATION_GPIO 0 +#define AO_BOOT_APPLICATION_PIN 19 +#define AO_BOOT_APPLICATION_VALUE 1 +#define AO_BOOT_APPLICATION_MODE AO_EXTI_MODE_PULL_UP + +#define HAS_USB_PULLUP 1 +#define AO_USB_PULLUP_PORT 0 +#define AO_USB_PULLUP_PIN 20 + +#endif /* _AO_PINS_H_ */ diff --git a/src/lpc/Makefile-flash.defs b/src/lpc/Makefile-flash.defs new file mode 100644 index 00000000..4a245d10 --- /dev/null +++ b/src/lpc/Makefile-flash.defs @@ -0,0 +1,61 @@ +include $(TOPDIR)/lpc/Makefile-lpc.defs + +INC = \ + ao.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_flash_pins.h \ + ao_flash_lpc_pins.h \ + ao_flash_task.h \ + ao_pins.h \ + ao_product.h \ + Makefile + +# +# Common AltOS sources +# +SRC = \ + ao_interrupt.c \ + ao_romconfig.c \ + ao_boot_chain.c \ + ao_boot_pin.c \ + ao_product.c \ + ao_notask.c \ + ao_timer_lpc.c \ + ao_usb_lpc.c \ + ao_flash_lpc.c \ + ao_flash_task.c \ + ao_flash_loader_lpc.c + +OBJ=$(SRC:.c=.o) + +PRODUCT=AltosFlash +PRODUCT_DEF=-DALTOS_FLASH +IDPRODUCT=0x000a + +CFLAGS = $(PRODUCT_DEF) $(LPC_CFLAGS) -g -Os + +LDFLAGS=$(CFLAGS) -L$(TOPDIR)/lpc -Wl,-Taltos-loader.ld + +PROGNAME=altos-flash +PROG=$(HARDWARE)-$(PROGNAME)-$(VERSION).elf + +$(PROG): Makefile $(OBJ) altos-loader.ld + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) + +ao_product.h: ao-make-product.5c $(TOPDIR)/Version + $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ + +$(OBJ): $(INC) + +all: $(PROG) + +distclean: clean + +clean: + rm -f *.o $(HARDWARE)-$(PROGNAME)-*.elf + rm -f ao_product.h + +install: + +uninstall: diff --git a/src/lpc/Makefile-lpc.defs b/src/lpc/Makefile-lpc.defs new file mode 100644 index 00000000..3d55cf67 --- /dev/null +++ b/src/lpc/Makefile-lpc.defs @@ -0,0 +1,45 @@ +ifndef TOPDIR +TOPDIR=.. +endif + +include $(TOPDIR)/Makedefs + +vpath % $(TOPDIR)/lpc:$(TOPDIR)/product:$(TOPDIR)/drivers:$(TOPDIR)/core:$(TOPDIR)/util:$(TOPDIR)/kalman:$(TOPDIR/aes):$(TOPDIR) +vpath make-altitude $(TOPDIR)/util +vpath make-kalman $(TOPDIR)/util +vpath kalman.5c $(TOPDIR)/kalman +vpath kalman_filter.5c $(TOPDIR)/kalman +vpath load_csv.5c $(TOPDIR)/kalman +vpath matrix.5c $(TOPDIR)/kalman +vpath ao-make-product.5c $(TOPDIR)/util + +.SUFFIXES: .elf .ihx + +.elf.ihx: + $(ELFTOHEX) --output=$@ $*.elf + + +ifndef VERSION +include $(TOPDIR)/Version +endif + +ELFTOHEX=$(TOPDIR)/../ao-tools/ao-elftohex/ao-elftohex +CC=$(ARM_CC) + +AO_CFLAGS=-I. -I$(TOPDIR)/lpc -I$(TOPDIR)/core -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) $(PDCLIB_INCLUDES) +LPC_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m0 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS) + +NICKLE=nickle + +LIBS=$(PDCLIB_LIBS_M0) -lgcc + +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) + +.c.o: + $(call quiet,CC) -c $(CFLAGS) -o $@ $< diff --git a/src/lpc/Makefile.defs b/src/lpc/Makefile.defs new file mode 100644 index 00000000..b6d739c2 --- /dev/null +++ b/src/lpc/Makefile.defs @@ -0,0 +1,15 @@ +ifndef TOPDIR +TOPDIR=.. +endif + +include $(TOPDIR)/lpc/Makefile-lpc.defs +include $(TOPDIR)/Makedefs + +LDFLAGS=$(CFLAGS) -L$(TOPDIR)/lpc -Wl,-Taltos.ld + +ao_serial_lpc.h: $(TOPDIR)/lpc/baud_rate ao_pins.h + nickle $(TOPDIR)/lpc/baud_rate `awk '/AO_LPC_CLKOUT/{print $$3}' ao_pins.h` > $@ + +ao_serial_lpc.o: ao_serial_lpc.h + +.DEFAULT_GOAL=all diff --git a/src/lpc/altos-loader.ld b/src/lpc/altos-loader.ld new file mode 100644 index 00000000..9df6e456 --- /dev/null +++ b/src/lpc/altos-loader.ld @@ -0,0 +1,80 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +MEMORY { + rom : ORIGIN = 0x00000000, LENGTH = 4K + ram : ORIGIN = 0x10000000, LENGTH = 4k - 128 - 32 + usb (!x) : ORIGIN = 0x20004000 + 2K - 256, LENGTH = 256 + stack (!w) : ORIGIN = 0x10000000 + 4K - 128 - 32, LENGTH = 128 +} + +INCLUDE registers.ld + +EXTERN (lpc_interrupt_vector) + +SECTIONS { + /* + * Rom contents + */ + + .interrupt : { + __text_start__ = .; + *(.interrupt) /* Interrupt vectors */ + + } > rom + + .text ORIGIN(rom) + 0x100 : { + ao_romconfig.o(.romconfig*) + ao_product.o(.romconfig*) + + *(.text*) /* Executable code */ + *(.ARM.exidx* .gnu.linkonce.armexidx.*) + *(.rodata*) /* Constants */ + __text_end__ = .; + } > rom + + /* Boot data which must live at the start of ram so that + * the application and bootloader share the same addresses. + * This must be all uninitialized data + */ + .boot ORIGIN(ram) + SIZEOF(.interrupt) (NOLOAD) : { + __boot_start__ = .; + *(.boot*) + __boot_end__ = .; + } >ram + + /* Data -- relocated to RAM, but written to ROM + */ + .data : { + __data_start__ = .; + *(.data*) /* initialized data */ + __data_end__ = .; + } >ram AT>rom + + + .bss : { + __bss_start__ = .; + *(.bss*) + *(COMMON*) + __bss_end__ = .; + } >ram + + PROVIDE(__stack__ = ORIGIN(ram) + LENGTH(ram)); + PROVIDE(end = .); +} + +ENTRY(start); diff --git a/src/lpc/altos-standalone.ld b/src/lpc/altos-standalone.ld new file mode 100644 index 00000000..032406f8 --- /dev/null +++ b/src/lpc/altos-standalone.ld @@ -0,0 +1,85 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +MEMORY { + rom (rx) : ORIGIN = 0x00000000, LENGTH = 32K + ram (!w) : ORIGIN = 0x10000000, LENGTH = 4K - 128 - 32 + usb (!x) : ORIGIN = 0x20004000 + 2K - 256, LENGTH = 256 + stack (!w) : ORIGIN = 0x10000000 + 4K - 128 - 32, LENGTH = 128 +} + +INCLUDE registers.ld + +EXTERN (lpc_interrupt_vector) + +SECTIONS { + /* + * Rom contents + */ + + .text ORIGIN(rom) : { + __text_start__ = .; + *(.interrupt) /* Interrupt vectors */ + + . = ORIGIN(rom) + 0x100; + + ao_romconfig.o(.romconfig*) + ao_product.o(.romconfig*) + + *(.text*) /* Executable code */ + *(.rodata*) /* Constants */ + + } > rom + + .ARM.exidx : { + *(.ARM.exidx* .gnu.linkonce.armexidx.*) + __text_end__ = .; + } > rom + + /* Boot data which must live at the start of ram so that + * the application and bootloader share the same addresses. + * This must be all uninitialized data + */ + .boot (NOLOAD) : { + __boot_start__ = .; + *(.boot) + . = ALIGN(4); + __boot_end__ = .; + } >ram + + /* Data -- relocated to RAM, but written to ROM + */ + .data ORIGIN(ram) : AT (ADDR(.ARM.exidx) + SIZEOF (.ARM.exidx)) { + __data_start__ = .; + *(.data) /* initialized data */ + __data_end__ = .; + __bss_start__ = .; + } >ram + + .bss : { + *(.bss) + *(COMMON) + __bss_end__ = .; + } >ram + PROVIDE(end = .); + + PROVIDE(__stack__ = ORIGIN(stack) + LENGTH(stack)); +} + +ENTRY(start); + + diff --git a/src/lpc/altos.ld b/src/lpc/altos.ld new file mode 100644 index 00000000..00d4f18a --- /dev/null +++ b/src/lpc/altos.ld @@ -0,0 +1,90 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +MEMORY { + rom (rx) : ORIGIN = 0x00001000, LENGTH = 28K + ram (!w) : ORIGIN = 0x10000000, LENGTH = 4K - 128 + usb (!x) : ORIGIN = 0x20004000 + 2K - 256, LENGTH = 256 + stack (!w) : ORIGIN = 0x10000000 + 4K - 128, LENGTH = 128 +} + +INCLUDE registers.ld + +EXTERN (lpc_interrupt_vector) + +SECTIONS { + /* + * Rom contents + */ + + .interrupt ORIGIN(ram) : AT (ORIGIN(rom)) { + __interrupt_start__ = .; + __interrupt_rom__ = ORIGIN(rom); + *(.interrupt) /* Interrupt vectors */ + __interrupt_end__ = .; + } > ram + + .text ORIGIN(rom) + 0x100 : { + __text_start__ = .; + + ao_romconfig.o(.romconfig*) + ao_product.o(.romconfig*) + + *(.text*) /* Executable code */ + *(.rodata*) /* Constants */ + + } > rom + + .ARM.exidx : { + *(.ARM.exidx* .gnu.linkonce.armexidx.*) + __text_end__ = .; + } > rom + + /* Boot data which must live at the start of ram so that + * the application and bootloader share the same addresses. + * This must be all uninitialized data + */ + .boot : { + __boot_start__ = .; + *(.boot) + . = ALIGN(4); + __boot_end__ = .; + } >ram + + /* Data -- relocated to RAM, but written to ROM + */ + .data : AT (ADDR(.ARM.exidx) + SIZEOF (.ARM.exidx)) { + __data_start__ = .; + *(.data) /* initialized data */ + __data_end__ = .; + __bss_start__ = .; + } >ram + + .bss : { + __bss_start__ = .; + *(.bss) + *(COMMON) + __bss_end__ = .; + } >ram + PROVIDE(end = .); + + PROVIDE(__stack__ = ORIGIN(stack) + LENGTH(stack)); +} + +ENTRY(start); + + diff --git a/src/lpc/ao_adc_lpc.c b/src/lpc/ao_adc_lpc.c new file mode 100644 index 00000000..7005f86e --- /dev/null +++ b/src/lpc/ao_adc_lpc.c @@ -0,0 +1,216 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_data.h> + +#ifndef AO_ADC_0 +#define AO_ADC_0 0 +#endif + +#ifndef AO_ADC_1 +#define AO_ADC_1 0 +#endif + +#ifndef AO_ADC_2 +#define AO_ADC_2 0 +#endif + +#ifndef AO_ADC_3 +#define AO_ADC_3 0 +#endif + +#ifndef AO_ADC_4 +#define AO_ADC_4 0 +#endif + +#ifndef AO_ADC_5 +#define AO_ADC_5 0 +#endif + +#ifndef AO_ADC_6 +#define AO_ADC_6 0 +#endif + +#ifndef AO_ADC_7 +#define AO_ADC_7 0 +#endif + +#define AO_ADC_NUM (AO_ADC_0 + AO_ADC_1 + AO_ADC_2 + AO_ADC_3 + \ + AO_ADC_4 + AO_ADC_5 + AO_ADC_6 + AO_ADC_7) + +/* ADC clock is divided by this value + 1, which ensures that + * the ADC clock will be strictly less than 4.5MHz as required + */ +#define AO_ADC_CLKDIV (AO_LPC_SYSCLK / 450000) + +static uint8_t ao_adc_ready; +static uint8_t ao_adc_sequence; + +static const uint8_t ao_adc_mask_seq[AO_ADC_NUM] = { +#if AO_ADC_0 + 1 << 0, +#endif +#if AO_ADC_1 + 1 << 1, +#endif +#if AO_ADC_2 + 1 << 2, +#endif +#if AO_ADC_3 + 1 << 3, +#endif +#if AO_ADC_4 + 1 << 4, +#endif +#if AO_ADC_5 + 1 << 6, +#endif +#if AO_ADC_6 + 1 << 6, +#endif +#if AO_ADC_7 + 1 << 7, +#endif +}; + +#define sample(id) (*out++ = (uint16_t) lpc_adc.dr[id] >> 1) + +static inline void lpc_adc_start(void) { + lpc_adc.cr = ((ao_adc_mask_seq[ao_adc_sequence] << LPC_ADC_CR_SEL) | + (AO_ADC_CLKDIV << LPC_ADC_CR_CLKDIV) | + (0 << LPC_ADC_CR_BURST) | + (LPC_ADC_CR_CLKS_11 << LPC_ADC_CR_CLKS) | + (LPC_ADC_CR_START_NOW << LPC_ADC_CR_START)); +} + +void lpc_adc_isr(void) +{ + uint16_t *out; + + /* Store converted value in packet */ + out = (uint16_t *) &ao_data_ring[ao_data_head].adc; + out[ao_adc_sequence] = (uint16_t) lpc_adc.gdr >> 1; + if (++ao_adc_sequence < AO_ADC_NUM) { + lpc_adc_start(); + return; + } + + AO_DATA_PRESENT(AO_DATA_ADC); + if (ao_data_present == AO_DATA_ALL) { +#if HAS_MS5607 + ao_data_ring[ao_data_head].ms5607_raw = ao_ms5607_current; +#endif +#if HAS_MMA655X + ao_data_ring[ao_data_head].mma655x = ao_mma655x_current; +#endif +#if HAS_HMC5883 + ao_data_ring[ao_data_head].hmc5883 = ao_hmc5883_current; +#endif +#if HAS_MPU6000 + ao_data_ring[ao_data_head].mpu6000 = ao_mpu6000_current; +#endif + ao_data_ring[ao_data_head].tick = ao_tick_count; + ao_data_head = ao_data_ring_next(ao_data_head); + ao_wakeup((void *) &ao_data_head); + } + ao_adc_ready = 1; +} + + +/* + * Start the ADC sequence using burst mode + */ +void +ao_adc_poll(void) +{ + if (!ao_adc_ready) + return; + ao_adc_ready = 0; + ao_adc_sequence = 0; + lpc_adc_start(); +} + +static void +ao_adc_dump(void) __reentrant +{ + struct ao_data packet; + int16_t *d; + uint8_t i; + + ao_data_get(&packet); +#ifdef AO_ADC_DUMP + AO_ADC_DUMP(&packet); +#else + printf("tick: %5u", packet.tick); + d = (int16_t *) (&packet.adc); + for (i = 0; i < AO_NUM_ADC; i++) + printf (" %2d: %5d", i, d[i]); + printf("\n"); +#endif +} + +__code struct ao_cmds ao_adc_cmds[] = { + { ao_adc_dump, "a\0Display current ADC values" }, + { 0, NULL }, +}; + +void +ao_adc_init(void) +{ + lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_ADC); + lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_ADC_PD); + + /* Enable interrupt when channel is complete */ + lpc_adc.inten = (1 << LPC_ADC_INTEN_ADGINTEN); + + lpc_nvic_set_enable(LPC_ISR_ADC_POS); + lpc_nvic_set_priority(LPC_ISR_ADC_POS, AO_LPC_NVIC_CLOCK_PRIORITY); +#if AO_ADC_0 + ao_enable_analog(0, 11, 0); +#endif +#if AO_ADC_1 + ao_enable_analog(0, 12, 1); +#endif +#if AO_ADC_2 + ao_enable_analog(0, 13, 2); +#endif +#if AO_ADC_3 + ao_enable_analog(0, 14, 3); +#endif +#if AO_ADC_4 + ao_enable_analog(0, 15, 4); +#endif +#if AO_ADC_5 + ao_enable_analog(0, 16, 5); +#endif +#if AO_ADC_6 + ao_enable_analog(0, 22, 6); +#endif +#if AO_ADC_7 + ao_enable_analog(0, 23, 7); +#endif + + lpc_adc.cr = ((0 << LPC_ADC_CR_SEL) | + (AO_ADC_CLKDIV << LPC_ADC_CR_CLKDIV) | + (0 << LPC_ADC_CR_BURST) | + (LPC_ADC_CR_CLKS_11 << LPC_ADC_CR_CLKS)); + + ao_cmd_register(&ao_adc_cmds[0]); + + ao_adc_ready = 1; +} diff --git a/src/lpc/ao_arch.h b/src/lpc/ao_arch.h new file mode 100644 index 00000000..d04bf2c8 --- /dev/null +++ b/src/lpc/ao_arch.h @@ -0,0 +1,146 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_ARCH_H_ +#define _AO_ARCH_H_ + +#include <lpc.h> + +/* + * LPC11U14 definitions and code fragments for AltOS + */ + +#ifndef AO_STACK_SIZE +#define AO_STACK_SIZE 512 +#endif + +#define AO_LED_TYPE uint16_t + +#define AO_PORT_TYPE uint32_t + +#ifndef AO_TICK_TYPE +#define AO_TICK_TYPE uint16_t +#define AO_TICK_SIGNED int16_t +#endif + +/* Various definitions to make GCC look more like SDCC */ + +#define ao_arch_naked_declare __attribute__((naked)) +#define ao_arch_naked_define +#define __pdata +#define __data +#define __xdata +#define __code const +#define __reentrant +#define __interrupt(n) +#define __at(n) + +#define ao_arch_reboot() arm_scb.aircr = ((0x05fa << 16) | \ + (0 << 15) | \ + (1 << 2) | \ + (0 << 1)) + +#define ao_arch_nop() asm("nop") + +#define ao_arch_interrupt(n) /* nothing */ + +#undef putchar +#undef getchar +#define putchar(c) ao_putchar(c) +#define getchar ao_getchar + +extern void putchar(char c); +extern char getchar(void); + +/* + * ao_romconfig.c + */ + +#define AO_ROMCONFIG_VERSION 2 + +#define AO_ROMCONFIG_SYMBOL(a) __attribute__((section(".romconfig"))) const + +extern const uint16_t ao_romconfig_version; +extern const uint16_t ao_romconfig_check; +extern const uint16_t ao_serial_number; +extern const uint32_t ao_radio_cal; + +#define ao_arch_task_members\ + uint32_t *sp; /* saved stack pointer */ + +#define ao_arch_block_interrupts() asm("cpsid i") +#define ao_arch_release_interrupts() asm("cpsie i") + +/* + * For now, we're running at a weird frequency + */ + +#if AO_HSE +#define AO_PLLSRC AO_HSE +#else +#define AO_PLLSRC STM_HSI_FREQ +#endif + +#define AO_PLLVCO (AO_PLLSRC * AO_PLLMUL) +#define AO_SYSCLK (AO_PLLVCO / AO_PLLDIV) +#define AO_HCLK (AO_SYSCLK / AO_AHB_PRESCALER) +#define AO_PCLK1 (AO_HCLK / AO_APB1_PRESCALER) +#define AO_PCLK2 (AO_HCLK / AO_APB2_PRESCALER) + +#if AO_APB1_PRESCALER == 1 +#define AO_TIM23467_CLK AO_PCLK1 +#else +#define AO_TIM23467_CLK (2 * AO_PCLK1) +#endif + +#if AO_APB2_PRESCALER == 1 +#define AO_TIM91011_CLK AO_PCLK2 +#else +#define AO_TIM91011_CLK (2 * AO_PCLK2) +#endif + +#define AO_LPC_NVIC_HIGH_PRIORITY 0 +#define AO_LPC_NVIC_CLOCK_PRIORITY 1 +#define AO_LPC_NVIC_MED_PRIORITY 2 +#define AO_LPC_NVIC_LOW_PRIORITY 3 + +void +ao_adc_init(void); + +#define AO_USB_OUT_EP 2 +#define AO_USB_IN_EP 3 + +void +ao_serial_init(void); + +/* SPI definitions */ + +#define AO_SPI_SPEED_12MHz 4 +#define AO_SPI_SPEED_6MHz 8 +#define AO_SPI_SPEED_4MHz 12 +#define AO_SPI_SPEED_2MHz 24 +#define AO_SPI_SPEED_1MHz 48 +#define AO_SPI_SPEED_500kHz 96 +#define AO_SPI_SPEED_250kHz 192 + +#define AO_SPI_SPEED_FAST AO_SPI_SPEED_12MHz + +#define AO_BOOT_APPLICATION_BASE ((uint32_t *) 0x00001000) +#define AO_BOOT_LOADER_BASE ((uint32_t *) 0x00000000) +#define HAS_BOOT_LOADER 1 + +#endif /* _AO_ARCH_H_ */ diff --git a/src/lpc/ao_arch_funcs.h b/src/lpc/ao_arch_funcs.h new file mode 100644 index 00000000..0891903e --- /dev/null +++ b/src/lpc/ao_arch_funcs.h @@ -0,0 +1,241 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_ARCH_FUNCS_H_ +#define _AO_ARCH_FUNCS_H_ + +#define ao_spi_get_bit(reg,bit,pin,bus,speed) ao_spi_get_mask(reg,(1<<bit),bus,speed) +#define ao_spi_put_bit(reg,bit,pin,bus) ao_spi_put_mask(reg,(1<<bit),bus) + +#define ao_enable_port(port) (lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_GPIO)) +#define ao_disable_port(port) (lpc_scb.sysahbclkctrl &= ~(1 << LPC_SCB_SYSAHBCLKCTRL_GPIO)) + +#define lpc_all_bit(port,bit) (((port) << 5) | (bit)) + +#define ao_gpio_set(port, bit, pin, v) (lpc_gpio.byte[lpc_all_bit(port,bit)] = (v)) + +#define ao_gpio_get(port, bit, pin) (lpc_gpio.byte[lpc_all_bit(port,bit)]) + +#define ao_enable_output(port,bit,pin,v) do { \ + ao_enable_port(port); \ + ao_gpio_set(port, bit, pin, v); \ + lpc_gpio.dir[port] |= (1 << bit); \ + } while (0) + +#define ao_gpio_set_mode(port,bit,mode) do { \ + vuint32_t *_ioconf = &lpc_ioconf.pio0_0 + ((port)*24+(bit)); \ + vuint32_t _mode; \ + if (mode == AO_EXTI_MODE_PULL_UP) \ + _mode = LPC_IOCONF_MODE_PULL_UP << LPC_IOCONF_MODE; \ + else if (mode == AO_EXTI_MODE_PULL_DOWN) \ + _mode = LPC_IOCONF_MODE_PULL_UP << LPC_IOCONF_MODE; \ + else \ + _mode = LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE; \ + *_ioconf = ((*_ioconf & ~(LPC_IOCONF_MODE_MASK << LPC_IOCONF_MODE)) | \ + _mode | \ + (1 << LPC_IOCONF_ADMODE)); \ + } while (0) + +#define ao_enable_input(port,bit,mode) do { \ + ao_enable_port(port); \ + lpc_gpio.dir[port] &= ~(1 << bit); \ + ao_gpio_set_mode(port,bit,mode); \ + } while (0) + +#define lpc_token_paster_2(x,y) x ## y +#define lpc_token_evaluator_2(x,y) lpc_token_paster_2(x,y) +#define lpc_token_paster_3(x,y,z) x ## y ## z +#define lpc_token_evaluator_3(x,y,z) lpc_token_paster_3(x,y,z) +#define lpc_token_paster_4(w,x,y,z) w ## x ## y ## z +#define lpc_token_evaluator_4(w,x,y,z) lpc_token_paster_4(w,x,y,z) +#define analog_reg(port,bit) lpc_token_evaluator_4(pio,port,_,bit) +#define analog_func(id) lpc_token_evaluator_2(LPC_IOCONF_FUNC_AD,id) + +#define ao_enable_analog(port,bit,id) do { \ + ao_enable_port(port); \ + lpc_gpio.dir[port] &= ~(1 << bit); \ + lpc_ioconf.analog_reg(port,bit) = ((analog_func(id) << LPC_IOCONF_FUNC) | \ + (0 << LPC_IOCONF_ADMODE)); \ + } while (0) + +#define ARM_PUSH32(stack, val) (*(--(stack)) = (val)) + +static inline uint32_t +ao_arch_irqsave(void) { + uint32_t primask; + asm("mrs %0,primask" : "=&r" (primask)); + ao_arch_block_interrupts(); + return primask; +} + +static inline void +ao_arch_irqrestore(uint32_t primask) { + asm("msr primask,%0" : : "r" (primask)); +} + +static inline void +ao_arch_memory_barrier() { + asm volatile("" ::: "memory"); +} + +#if HAS_TASK +static inline void +ao_arch_init_stack(struct ao_task *task, void *start) +{ + uint32_t *sp = (uint32_t *) (task->stack + AO_STACK_SIZE); + uint32_t a = (uint32_t) start; + int i; + + /* Return address (goes into LR) */ + ARM_PUSH32(sp, a); + + /* Clear register values r0-r7 */ + i = 8; + while (i--) + ARM_PUSH32(sp, 0); + + /* APSR */ + ARM_PUSH32(sp, 0); + + /* PRIMASK with interrupts enabled */ + ARM_PUSH32(sp, 0); + + task->sp = sp; +} + +static inline void ao_arch_save_regs(void) { + /* Save general registers */ + asm("push {r0-r7,lr}\n"); + + /* Save APSR */ + asm("mrs r0,apsr"); + asm("push {r0}"); + + /* Save PRIMASK */ + asm("mrs r0,primask"); + asm("push {r0}"); +} + +static inline void ao_arch_save_stack(void) { + uint32_t *sp; + asm("mov %0,sp" : "=&r" (sp) ); + ao_cur_task->sp = (sp); + if ((uint8_t *) sp < &ao_cur_task->stack[0]) + ao_panic (AO_PANIC_STACK); +} + +static inline void ao_arch_restore_stack(void) { + uint32_t sp; + sp = (uint32_t) ao_cur_task->sp; + + /* Switch stacks */ + asm("mov sp, %0" : : "r" (sp) ); + + /* Restore PRIMASK */ + asm("pop {r0}"); + asm("msr primask,r0"); + + /* Restore APSR */ + asm("pop {r0}"); + asm("msr apsr_nczvq,r0"); + + /* Restore general registers and return */ + asm("pop {r0-r7,pc}\n"); +} + +#define ao_arch_isr_stack() + +#endif /* HAS_TASK */ + +#define ao_arch_wait_interrupt() do { \ + asm(".global ao_idle_loc\n\twfi\nao_idle_loc:"); \ + ao_arch_release_interrupts(); \ + ao_arch_block_interrupts(); \ + } while (0) + +#define ao_arch_critical(b) do { \ + ao_arch_block_interrupts(); \ + do { b } while (0); \ + ao_arch_release_interrupts(); \ + } while (0) + +/* + * SPI + */ + +#define ao_spi_set_cs(port,mask) (lpc_gpio.clr[port] = (mask)) +#define ao_spi_clr_cs(port,mask) (lpc_gpio.set[port] = (mask)) + +#define ao_spi_get_mask(port,mask,bus,speed) do { \ + ao_spi_get(bus, speed); \ + ao_spi_set_cs(port, mask); \ + } while (0) + +#define ao_spi_put_mask(reg,mask,bus) do { \ + ao_spi_clr_cs(reg,mask); \ + ao_spi_put(bus); \ + } while (0) + +#define ao_spi_get_bit(reg,bit,pin,bus,speed) ao_spi_get_mask(reg,(1<<bit),bus,speed) +#define ao_spi_put_bit(reg,bit,pin,bus) ao_spi_put_mask(reg,(1<<bit),bus) + +void +ao_spi_get(uint8_t spi_index, uint32_t speed); + +void +ao_spi_put(uint8_t spi_index); + +void +ao_spi_send(void *block, uint16_t len, uint8_t spi_index); + +void +ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t spi_index); + +void +ao_spi_recv(void *block, uint16_t len, uint8_t spi_index); + +void +ao_spi_duplex(void *out, void *in, uint16_t len, uint8_t spi_index); + +extern uint16_t ao_spi_speed[LPC_NUM_SPI]; + +void +ao_spi_init(void); + +#define ao_spi_init_cs(port, mask) do { \ + uint8_t __bit__; \ + for (__bit__ = 0; __bit__ < 32; __bit__++) { \ + if (mask & (1 << __bit__)) \ + ao_enable_output(port, __bit__, PIN, 1); \ + } \ + } while (0) + +#define HAS_ARCH_START_SCHEDULER 1 + +static inline void ao_arch_start_scheduler(void) { + uint32_t sp; + uint32_t control; + + asm("mrs %0,msp" : "=&r" (sp)); + asm("msr psp,%0" : : "r" (sp)); + asm("mrs %0,control" : "=&r" (control)); + control |= (1 << 1); + asm("msr control,%0" : : "r" (control)); + asm("isb"); +} + +#endif /* _AO_ARCH_FUNCS_H_ */ diff --git a/src/lpc/ao_beep_lpc.c b/src/lpc/ao_beep_lpc.c new file mode 100644 index 00000000..eb9132b8 --- /dev/null +++ b/src/lpc/ao_beep_lpc.c @@ -0,0 +1,87 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +void +ao_beep(uint8_t beep) +{ + if (beep == 0) { + lpc_ct32b1.tcr = ((0 << LPC_CT32B_TCR_CEN) | + (1 << LPC_CT32B_TCR_CRST)); + lpc_scb.sysahbclkctrl &= ~(1 << LPC_SCB_SYSAHBCLKCTRL_CT32B1); + } else { + lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_CT32B1); + + /* Set prescaler to match cc1111 clocks + */ + lpc_ct32b1.pr = AO_LPC_SYSCLK / 750000 - 1; + + /* Write the desired data in the match registers */ + + /* Reset after two time units */ + lpc_ct32b1.mr[0] = beep << 1; + + /* PWM width is half of that */ + lpc_ct32b1.mr[1] = beep; + + /* Flip output 1 on PWM match */ + lpc_ct32b1.emr = (LPC_CT32B_EMR_EMC_TOGGLE << LPC_CT32B_EMR_EMC1); + + /* Reset on match 0 */ + lpc_ct32b1.mcr = (1 << LPC_CT32B_MCR_MR0R); + + /* PWM on match 1 */ + lpc_ct32b1.pwmc = (1 << LPC_CT32B_PWMC_PWMEN1); + + /* timer mode */ + lpc_ct32b1.ctcr = 0; + + /* And turn the timer on */ + lpc_ct32b1.tcr = ((1 << LPC_CT32B_TCR_CEN) | + (0 << LPC_CT32B_TCR_CRST)); + } +} + +void +ao_beep_for(uint8_t beep, uint16_t ticks) __reentrant +{ + ao_beep(beep); + ao_delay(ticks); + ao_beep(0); +} + +void +ao_beep_init(void) +{ + /* Our beeper is on c32b1_mat1 + * which is on pin pio0_14 + */ + + lpc_ioconf.pio0_14 = ((LPC_IOCONF_FUNC_PIO0_14_CT32B1_MAT1 << LPC_IOCONF_FUNC) | + (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) | + (0 << LPC_IOCONF_HYS) | + (0 << LPC_IOCONF_INV) | + (1 << LPC_IOCONF_ADMODE) | + (0 << LPC_IOCONF_OD)); + + lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_CT32B1); + + /* Disable the counter and reset the value */ + lpc_ct32b1.tcr = ((0 << LPC_CT32B_TCR_CEN) | + (1 << LPC_CT32B_TCR_CRST)); +} diff --git a/src/lpc/ao_boot.h b/src/lpc/ao_boot.h new file mode 100644 index 00000000..e0ed4de7 --- /dev/null +++ b/src/lpc/ao_boot.h @@ -0,0 +1,39 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_BOOT_H_ +#define _AO_BOOT_H_ + +void +ao_boot_chain(uint32_t *base); + +void +ao_boot_check_pin(void); + +/* Return true to switch to application (if present) */ +int +ao_boot_check_chain(void); + +void +ao_boot_reboot(uint32_t *base); + +static inline void +ao_boot_loader(void) { + ao_boot_reboot(AO_BOOT_LOADER_BASE); +} + +#endif /* _AO_BOOT_H_ */ diff --git a/src/lpc/ao_boot_chain.c b/src/lpc/ao_boot_chain.c new file mode 100644 index 00000000..a08d1f2c --- /dev/null +++ b/src/lpc/ao_boot_chain.c @@ -0,0 +1,67 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_boot.h> + +void +ao_boot_chain(uint32_t *base) +{ + uint32_t sp; + uint32_t pc; + + sp = base[0]; + pc = base[1]; + if (0x00000100 <= pc && pc <= 0x00008000 && (pc & 1) == 1) { + asm ("mov sp, %0" : : "r" (sp)); + asm ("mov lr, %0" : : "r" (pc)); + asm ("bx lr"); + } +} + +#define AO_BOOT_SIGNAL 0x5a5aa5a5 +#define AO_BOOT_CHECK 0xc3c33c3c + +struct ao_boot { + uint32_t *base; + uint32_t signal; + uint32_t check; +}; + +static struct ao_boot __attribute__ ((section(".boot"))) ao_boot; + +int +ao_boot_check_chain(void) +{ + if (ao_boot.signal == AO_BOOT_SIGNAL && ao_boot.check == AO_BOOT_CHECK) { + ao_boot.signal = 0; + ao_boot.check = 0; + if (ao_boot.base == 0) + return 0; + ao_boot_chain(ao_boot.base); + } + return 1; +} + +void +ao_boot_reboot(uint32_t *base) +{ + ao_boot.base = base; + ao_boot.signal = AO_BOOT_SIGNAL; + ao_boot.check = AO_BOOT_CHECK; + ao_arch_reboot(); +} diff --git a/src/lpc/ao_boot_pin.c b/src/lpc/ao_boot_pin.c new file mode 100644 index 00000000..51ecc0a9 --- /dev/null +++ b/src/lpc/ao_boot_pin.c @@ -0,0 +1,46 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_boot.h> +#include <ao_exti.h> + +void +ao_boot_check_pin(void) +{ + uint16_t v; + + /* Enable power interface clock */ +// stm_rcc.apb1enr |= (1 << STM_RCC_APB1ENR_PWREN); + + /* Enable the input pin */ + ao_enable_input(AO_BOOT_APPLICATION_GPIO, AO_BOOT_APPLICATION_PIN, + AO_BOOT_APPLICATION_MODE); + + for (v = 0; v < 100; v++) + ao_arch_nop(); + + /* Read the value */ + v = ao_gpio_get(AO_BOOT_APPLICATION_GPIO, AO_BOOT_APPLICATION_PIN, AO_BOOT_APPLICATION); + + /* Reset the chip to turn off the port and the power interface clock */ + ao_gpio_set_mode(AO_BOOT_APPLICATION_GPIO, AO_BOOT_APPLICATION_PIN, 0); + ao_disable_port(AO_BOOT_APPLICATION_GPIO); +// stm_rcc.apb1enr &= ~(1 << STM_RCC_APB1ENR_PWREN); + if (v == AO_BOOT_APPLICATION_VALUE) + ao_boot_chain(AO_BOOT_APPLICATION_BASE); +} diff --git a/src/lpc/ao_exti.h b/src/lpc/ao_exti.h new file mode 100644 index 00000000..e8599eb4 --- /dev/null +++ b/src/lpc/ao_exti.h @@ -0,0 +1,48 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_EXTI_H_ +#define _AO_EXTI_H_ + +#define AO_EXTI_MODE_RISING 1 +#define AO_EXTI_MODE_FALLING 2 +#define AO_EXTI_MODE_PULL_UP 4 +#define AO_EXTI_MODE_PULL_DOWN 8 +#define AO_EXTI_PRIORITY_LOW 16 +#define AO_EXTI_PRIORITY_MED 0 +#define AO_EXTI_PRIORITY_HIGH 32 +#define AO_EXTI_PIN_NOCONFIGURE 64 + +void +ao_exti_setup(uint8_t gpio, uint8_t pin, uint8_t mode, void (*callback)()); + +void +ao_exti_set_mode(uint8_t gpio, uint8_t pin, uint8_t mode); + +void +ao_exti_set_callback(uint8_t gpio, uint8_t pin, void (*callback)()); + +void +ao_exti_enable(uint8_t gpio, uint8_t pin); + +void +ao_exti_disable(uint8_t gpio, uint8_t pin); + +void +ao_exti_init(void); + +#endif /* _AO_EXTI_H_ */ diff --git a/src/lpc/ao_exti_lpc.c b/src/lpc/ao_exti_lpc.c new file mode 100644 index 00000000..588cf58c --- /dev/null +++ b/src/lpc/ao_exti_lpc.c @@ -0,0 +1,178 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_exti.h> + +#define LPC_NUM_PINS 56 +#define LPC_NUM_PINT 8 + +static void (*ao_exti_callback[LPC_NUM_PINT])(void); + +static uint8_t ao_pint_map[LPC_NUM_PINS]; +static uint8_t ao_pint_mode[LPC_NUM_PINS]; +static uint8_t ao_pint_inuse; +static uint8_t ao_pint_enabled; + +static void +ao_exti_isr(uint8_t pint) +{ + uint8_t mask = 1 << pint; + + if (lpc_gpio_pin.ist & mask) { + lpc_gpio_pin.ist = mask; + lpc_gpio_pin.rise = mask; + lpc_gpio_pin.fall = mask; + + (*ao_exti_callback[pint]) (); + } +} + +#define pin_isr(n) void lpc_pin_int ## n ## _isr(void) { ao_exti_isr(n); } +pin_isr(0) +pin_isr(1) +pin_isr(2) +pin_isr(3) +pin_isr(4) +pin_isr(5) +pin_isr(6) +pin_isr(7) + +#define pin_id(port,pin) ((port) * 24 + (pin)); + +static void +_ao_exti_set_enable(uint8_t pint) +{ + uint8_t mask = 1 << pint; + uint8_t mode; + + if (ao_pint_enabled & mask) + mode = ao_pint_mode[pint]; + else + mode = 0; + + if (mode & AO_EXTI_MODE_RISING) + lpc_gpio_pin.sienr = mask; + else + lpc_gpio_pin.cienr = mask; + + if (mode & AO_EXTI_MODE_FALLING) + lpc_gpio_pin.sienf = mask; + else + lpc_gpio_pin.cienf = mask; + lpc_gpio_pin.rise = mask; + lpc_gpio_pin.fall = mask; +} + +void +ao_exti_setup (uint8_t port, uint8_t pin, uint8_t mode, void (*callback)(void)) { + uint8_t id = pin_id(port,pin); + uint8_t pint; + uint32_t mask; + uint8_t prio; + + for (pint = 0; pint < LPC_NUM_PINT; pint++) + if ((ao_pint_inuse & (1 << pint)) == 0) + break; + if (pint == LPC_NUM_PINT) + ao_panic(AO_PANIC_EXTI); + + if (!mode & AO_EXTI_PIN_NOCONFIGURE) + ao_enable_input(port, pin, mode); + + ao_arch_block_interrupts(); + mask = (1 << pint); + ao_pint_inuse |= mask; + ao_pint_enabled &= ~mask; + + ao_pint_map[id] = pint; + ao_exti_callback[pint] = callback; + + /* configure gpio to interrupt routing */ + lpc_scb.pintsel[pint] = id; + + /* Set edge triggered */ + lpc_gpio_pin.isel &= ~mask; + + ao_pint_enabled &= ~mask; + ao_pint_mode[pint] = mode; + _ao_exti_set_enable(pint); + + /* Set interrupt mask and rising/falling mode */ + + prio = AO_LPC_NVIC_MED_PRIORITY; + if (mode & AO_EXTI_PRIORITY_LOW) + prio = AO_LPC_NVIC_LOW_PRIORITY; + else if (mode & AO_EXTI_PRIORITY_HIGH) + prio = AO_LPC_NVIC_HIGH_PRIORITY; + + /* Set priority and enable */ + lpc_nvic_set_priority(LPC_ISR_PIN_INT0_POS + pint, prio); + lpc_nvic_set_enable(LPC_ISR_PIN_INT0_POS + pint); + ao_arch_release_interrupts(); +} + +void +ao_exti_set_mode(uint8_t port, uint8_t pin, uint8_t mode) +{ + uint8_t id = pin_id(port,pin); + uint8_t pint = ao_pint_map[id]; + + ao_arch_block_interrupts(); + ao_pint_mode[pint] = mode; + _ao_exti_set_enable(pint); + ao_arch_release_interrupts(); +} + +void +ao_exti_set_callback(uint8_t port, uint8_t pin, void (*callback)()) { + uint8_t id = pin_id(port,pin); + uint8_t pint = ao_pint_map[id]; + + ao_exti_callback[pint] = callback; +} + +void +ao_exti_enable(uint8_t port, uint8_t pin) +{ + uint8_t id = pin_id(port,pin); + uint8_t pint = ao_pint_map[id]; + uint8_t mask = 1 << pint; + + ao_arch_block_interrupts(); + ao_pint_enabled |= mask; + _ao_exti_set_enable(pint); + ao_arch_release_interrupts(); +} + +void +ao_exti_disable(uint8_t port, uint8_t pin) { + uint8_t id = pin_id(port,pin); + uint8_t pint = ao_pint_map[id]; + uint8_t mask = 1 << pint; + + ao_arch_block_interrupts(); + ao_pint_enabled &= ~mask; + _ao_exti_set_enable(pint); + ao_arch_release_interrupts(); +} + +void +ao_exti_init(void) +{ + lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_PINT); +} diff --git a/src/lpc/ao_flash.h b/src/lpc/ao_flash.h new file mode 100644 index 00000000..aaf66b39 --- /dev/null +++ b/src/lpc/ao_flash.h @@ -0,0 +1,30 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_FLASH_H_ +#define _AO_FLASH_H_ + +uint32_t +ao_flash_erase_page(uint8_t *page); + +uint32_t +ao_flash_page(uint8_t *page, uint8_t *src); + +uint32_t +ao_lpc_read_part_id(void); + +#endif /* _AO_FLASH_H_ */ diff --git a/src/lpc/ao_flash_loader_lpc.c b/src/lpc/ao_flash_loader_lpc.c new file mode 100644 index 00000000..2ab548cf --- /dev/null +++ b/src/lpc/ao_flash_loader_lpc.c @@ -0,0 +1,32 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include <ao_exti.h> +#include <ao_boot.h> +#include <ao_flash_task.h> + +int +main(void) +{ + ao_clock_init(); + + ao_usb_init(); + + ao_flash_task(); + return 0; +} diff --git a/src/lpc/ao_flash_lpc.c b/src/lpc/ao_flash_lpc.c new file mode 100644 index 00000000..5a31f39f --- /dev/null +++ b/src/lpc/ao_flash_lpc.c @@ -0,0 +1,158 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_flash.h> + +#define IAP_LOCATION 0x1fff1ff1 + +typedef void (*iap_func)(uint32_t *in, uint32_t *out); + +static void +iap(uint32_t *in, uint32_t *out) +{ + ao_arch_block_interrupts(); + lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_FLASHREG); + ((iap_func) IAP_LOCATION)(in, out); + ao_arch_release_interrupts(); +} + +#define LPC_IAP_PREPARE_WRITE 50 +#define LPC_IAP_COPY_RAM_TO_FLASH 51 +#define LPC_IAP_ERASE_SECTOR 52 +#define LPC_IAP_BLANK_CHECK 53 +#define LPC_IAP_READ_PART_ID 54 +#define LPC_IAP_READ_BOOT_CODE_VERSION 55 +#define LPC_IAP_COMPARE 56 +#define LPC_IAP_REINVOKE_ISP 57 +#define LPC_IAP_READ_UID 58 +#define LPC_IAP_ERASE_PAGE 59 +#define LPC_IAP_EEPROM_WRITE 61 +#define LPC_IAP_EEPROM_READ 62 + +#define LPC_IAP_CMD_SUCCESS 0 +#define LPC_IAP_INVALID_COMMAND 1 +#define LPC_IAP_SRC_ADDR_ERROR 2 +#define LPC_IAP_DST_ADDR_ERROR 3 +#define LPC_IAP_SRC_ADDR_NOT_MAPPED 4 +#define LPC_IAP_DST_ADDR_NOT_MAPPED 5 +#define LPC_IAP_COUNT_ERROR 6 +#define LPC_IAP_INVALID_SECTOR 7 +#define LPC_IAP_SECTOR_NOT_BLANK 8 +#define LPC_IAP_SECTOR_NOT_PREPARED_FOR_WRITE_OPERATION 9 +#define LPC_IAP_COMPARE_ERROR 10 +#define LPC_IAP_BUSY 11 +#define LPC_IAP_PARAM_ERROR 12 +#define LPC_IAP_ADDR_ERROR 13 +#define LPC_IAP_ADDR_NOT_MAPPED 14 +#define LPC_IAP_CMD_LOCKED 15 +#define LPC_IAP_INVALID_CODE 16 +#define LPC_IAP_INVALID_BAUD_RATE 17 +#define LPC_IAP_INVALID_STOP_BIT 18 +#define LPC_IAP_CODE_READ_PROTECTION_ENABLED 19 + +#define LPC_FLASH_BASE ((uint8_t *) 0x0) +#define LPC_FLASH_SECTOR 4096 +#define LPC_FLASH_SECTOR_MASK (LPC_FLASH_SECTOR - 1) +#define LPC_FLASH_SECTOR_SHIFT 12 + +static uint32_t iap_in[5], iap_out[5]; + +static uint32_t +ao_lpc_addr_to_sector(uint8_t *addr) +{ + uint32_t off = addr - LPC_FLASH_BASE; + + return off >> LPC_FLASH_SECTOR_SHIFT; +} + +static uint8_t +ao_lpc_addr_is_sector_aligned(uint8_t *addr) +{ + uint32_t off = addr - LPC_FLASH_BASE; + return (off & LPC_FLASH_SECTOR_MASK) == 0; +} + +static uint32_t +ao_lpc_prepare_write(uint32_t start_sector, uint32_t end_sector) +{ + iap_in[0] = LPC_IAP_PREPARE_WRITE; + iap_in[1] = start_sector; + iap_in[2] = end_sector; + iap(iap_in,iap_out); + return iap_out[0]; +} + +static uint32_t +ao_lpc_copy_ram_to_flash(uint8_t *dst, uint8_t *src, uint32_t len, uint32_t freq) +{ + iap_in[0] = LPC_IAP_COPY_RAM_TO_FLASH; + iap_in[1] = (uint32_t) dst; + iap_in[2] = (uint32_t) src; + iap_in[3] = len; + iap_in[4] = freq; + iap(iap_in,iap_out); + return iap_out[0]; +} + +static uint32_t +ao_lpc_erase_sector(uint32_t start_sector, uint32_t end_sector, uint32_t freq) +{ + iap_in[0] = LPC_IAP_ERASE_SECTOR; + iap_in[1] = start_sector; + iap_in[2] = end_sector; + iap_in[3] = freq; + iap(iap_in,iap_out); + return iap_out[0]; +} + +uint32_t +ao_lpc_read_part_id(void) +{ + iap_in[0] = LPC_IAP_READ_PART_ID; + iap(iap_in,iap_out); + return iap_out[1]; +} + +uint32_t +ao_flash_erase_page(uint8_t *page) +{ + uint32_t ret = LPC_IAP_CMD_SUCCESS; + if (ao_lpc_addr_is_sector_aligned(page)) { + uint32_t sector = ao_lpc_addr_to_sector(page); + ret = ao_lpc_prepare_write(sector, sector); + if (ret == LPC_IAP_CMD_SUCCESS) + ret = ao_lpc_erase_sector(sector, sector, AO_LPC_SYSCLK / 1000); + } + return ret; +} + +uint32_t +ao_flash_page(uint8_t *page, uint8_t *src) +{ + uint32_t sector = ao_lpc_addr_to_sector(page); + uint32_t ret; + + ret = ao_flash_erase_page(page); + if (ret != LPC_IAP_CMD_SUCCESS) + return ret; + ret = ao_lpc_prepare_write(sector, sector); + if (ret != LPC_IAP_CMD_SUCCESS) + return ret; + ret = ao_lpc_copy_ram_to_flash(page, src, 256, AO_LPC_SYSCLK / 1000); + return ret; +} diff --git a/src/lpc/ao_flash_lpc_pins.h b/src/lpc/ao_flash_lpc_pins.h new file mode 100644 index 00000000..e2243d5c --- /dev/null +++ b/src/lpc/ao_flash_lpc_pins.h @@ -0,0 +1,32 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_FLASH_LPC_PINS_H_ +#define _AO_FLASH_LPC_PINS_H_ + +#include <ao_flash_pins.h> + +/* Crystal on the board */ +#define AO_LPC_CLKIN 12000000 + +/* Main clock frequency. 48MHz for USB so we don't use the USB PLL */ +#define AO_LPC_CLKOUT 48000000 + +/* System clock frequency */ +#define AO_LPC_SYSCLK 24000000 + +#endif /* _AO_FLASH_STM_PINS_H_ */ diff --git a/src/lpc/ao_interrupt.c b/src/lpc/ao_interrupt.c new file mode 100644 index 00000000..c4dc7867 --- /dev/null +++ b/src/lpc/ao_interrupt.c @@ -0,0 +1,176 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <string.h> +#include <ao_boot.h> + +#ifndef IS_FLASH_LOADER +#error Should define IS_FLASH_LOADER +#define IS_FLASH_LOADER 0 +#endif + +#if !IS_FLASH_LOADER +#define RELOCATE_INTERRUPT 1 +#endif + +extern void main(void); +extern char __stack__; +extern char __text_start__, __text_end__; +extern char __data_start__, __data_end__; +extern char __bss_start__, __bss_end__; +#if RELOCATE_INTERRUPT +extern char __interrupt_rom__, __interrupt_start__, __interrupt_end__; +#endif + +/* Interrupt functions */ + +void lpc_halt_isr(void) +{ + ao_panic(AO_PANIC_CRASH); +} + +void lpc_ignore_isr(void) +{ +} + +void start(void) { +#ifdef AO_BOOT_CHAIN + if (ao_boot_check_chain()) { +#ifdef AO_BOOT_PIN + ao_boot_check_pin(); +#endif + } +#endif +#if RELOCATE_INTERRUPT + memcpy(&__interrupt_start__, &__interrupt_rom__, &__interrupt_end__ - &__interrupt_start__); + lpc_scb.sysmemremap = LPC_SCB_SYSMEMREMAP_MAP_RAM << LPC_SCB_SYSMEMREMAP_MAP; +#endif + memcpy(&__data_start__, &__text_end__, &__data_end__ - &__data_start__); + memset(&__bss_start__, '\0', &__bss_end__ - &__bss_start__); + main(); +} + +#define STRINGIFY(x) #x + +#define isr(name) \ + void __attribute__ ((weak)) lpc_ ## name ## _isr(void); \ + _Pragma(STRINGIFY(weak lpc_ ## name ## _isr = lpc_ignore_isr)) + +#define isr_halt(name) \ + void __attribute__ ((weak)) lpc_ ## name ## _isr(void); \ + _Pragma(STRINGIFY(weak lpc_ ## name ## _isr = lpc_halt_isr)) + +isr(nmi) +isr_halt(hardfault) +isr_halt(memmanage) +isr_halt(busfault) +isr_halt(usagefault) +isr(svc) +isr(debugmon) +isr(pendsv) +isr(systick) + +isr(pin_int0) /* IRQ0 */ +isr(pin_int1) +isr(pin_int2) +isr(pin_int3) +isr(pin_int4) /* IRQ4 */ +isr(pin_int5) +isr(pin_int6) +isr(pin_int7) + +isr(gint0) /* IRQ8 */ +isr(gint1) +isr(ssp1) +isr(i2c) + +isr(ct16b0) /* IRQ16 */ +isr(ct16b1) +isr(ct32b0) +isr(ct32b1) +isr(ssp0) /* IRQ20 */ +isr(usart) +isr(usb_irq) +isr(usb_fiq) + +isr(adc) /* IRQ24 */ +isr(wwdt) +isr(bod) +isr(flash) + +isr(usb_wakeup) + +#define i(addr,name) [(addr)/4] = lpc_ ## name ## _isr +#define c(addr,value) [(addr)/4] = (value) + +__attribute__ ((section(".interrupt"))) +const void *lpc_interrupt_vector[] = { + [0] = &__stack__, + [1] = start, + i(0x08, nmi), + i(0x0c, hardfault), + c(0x10, 0), + c(0x14, 0), + c(0x18, 0), + c(0x1c, 0), + c(0x20, 0), + c(0x24, 0), + c(0x28, 0), + i(0x2c, svc), + i(0x30, hardfault), + i(0x34, hardfault), + i(0x38, pendsv), + i(0x3c, systick), + + i(0x40, pin_int0), /* IRQ0 */ + i(0x44, pin_int1), + i(0x48, pin_int2), + i(0x4c, pin_int3), + i(0x50, pin_int4), /* IRQ4 */ + i(0x54, pin_int5), + i(0x58, pin_int6), + i(0x5c, pin_int7), + + i(0x60, gint0), /* IRQ8 */ + i(0x64, gint1), + i(0x68, hardfault), + i(0x6c, hardfault), + i(0x70, hardfault), /* IRQ12 */ + i(0x74, hardfault), + i(0x78, ssp1), + i(0x7c, i2c), + + i(0x80, ct16b0), /* IRQ16 */ + i(0x84, ct16b1), + i(0x88, ct32b0), + i(0x8c, ct32b1), + i(0x90, ssp0), /* IRQ20 */ + i(0x94, usart), + i(0x98, usb_irq), + i(0x9c, usb_fiq), + + i(0xa0, adc), /* IRQ24 */ + i(0xa4, wwdt), + i(0xa8, bod), + i(0xac, flash), + + i(0xb0, hardfault), /* IRQ28 */ + i(0xb4, hardfault), + i(0xb8, usb_wakeup), + i(0xbc, hardfault), +}; diff --git a/src/lpc/ao_led_lpc.c b/src/lpc/ao_led_lpc.c new file mode 100644 index 00000000..7bef51ba --- /dev/null +++ b/src/lpc/ao_led_lpc.c @@ -0,0 +1,66 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> + +__pdata uint16_t ao_led_enable; + +void +ao_led_on(uint16_t colors) +{ + lpc_gpio.pin[LED_PORT] |= colors; +} + +void +ao_led_off(uint16_t colors) +{ + lpc_gpio.pin[LED_PORT] &= ~colors; +} + +void +ao_led_set(uint16_t colors) +{ + uint16_t on = colors & ao_led_enable; + uint16_t off = ~colors & ao_led_enable; + + ao_led_off(off); + ao_led_on(on); +} + +void +ao_led_toggle(uint16_t colors) +{ + lpc_gpio.pin[LED_PORT] ^= colors; +} + +void +ao_led_for(uint16_t colors, uint16_t ticks) __reentrant +{ + ao_led_on(colors); + ao_delay(ticks); + ao_led_off(colors); +} + +void +ao_led_init(uint16_t enable) +{ + int bit; + + ao_led_enable = enable; + lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_GPIO); + lpc_gpio.dir[LED_PORT] |= enable; +} diff --git a/src/lpc/ao_romconfig.c b/src/lpc/ao_romconfig.c new file mode 100644 index 00000000..cbb922ec --- /dev/null +++ b/src/lpc/ao_romconfig.c @@ -0,0 +1,25 @@ +/* + * Copyright © 2011 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" + +AO_ROMCONFIG_SYMBOL (0) uint16_t ao_romconfig_version = AO_ROMCONFIG_VERSION; +AO_ROMCONFIG_SYMBOL (0) uint16_t ao_romconfig_check = ~AO_ROMCONFIG_VERSION; +AO_ROMCONFIG_SYMBOL (0) uint16_t ao_serial_number = 0; +#ifdef AO_RADIO_CAL_DEFAULT +AO_ROMCONFIG_SYMBOL (0) uint32_t ao_radio_cal = AO_RADIO_CAL_DEFAULT; +#endif diff --git a/src/lpc/ao_serial_lpc.c b/src/lpc/ao_serial_lpc.c new file mode 100644 index 00000000..431ae98a --- /dev/null +++ b/src/lpc/ao_serial_lpc.c @@ -0,0 +1,213 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_serial.h> + +struct ao_fifo ao_usart_rx_fifo; +struct ao_fifo ao_usart_tx_fifo; +uint8_t ao_usart_tx_avail; +uint8_t ao_usart_tx_avail_min; + +#define LPC_USART_TX_FIFO_SIZE 16 + +void +ao_debug_out(char c) +{ + if (c == '\n') + ao_debug_out('\r'); + while (!(lpc_usart.lsr & (1 << LPC_USART_LSR_TEMT))) + ; + lpc_usart.rbr_thr = c; +} + +static void +_ao_serial_tx_start(void) +{ + if (!ao_fifo_empty(ao_usart_tx_fifo) && ao_usart_tx_avail) { + ao_usart_tx_avail--; + if (ao_usart_tx_avail < ao_usart_tx_avail_min) + ao_usart_tx_avail_min = ao_usart_tx_avail; + ao_fifo_remove(ao_usart_tx_fifo, lpc_usart.rbr_thr); + } +} + +void +lpc_usart_isr(void) +{ + uint8_t wake_input = 0; + (void) lpc_usart.iir_fcr; + + while (lpc_usart.lsr & (1 << LPC_USART_LSR_RDR)) { + char c = lpc_usart.rbr_thr; + if (!ao_fifo_full(ao_usart_rx_fifo)) + ao_fifo_insert(ao_usart_rx_fifo, c); + wake_input = 1; + } + if (lpc_usart.lsr & (1 << LPC_USART_LSR_THRE)) { + ao_usart_tx_avail = LPC_USART_TX_FIFO_SIZE; + _ao_serial_tx_start(); + ao_wakeup(&ao_usart_tx_fifo); + } + if (wake_input) { + ao_wakeup(&ao_usart_rx_fifo); + if (stdin) + ao_wakeup(&ao_stdin_ready); + } +} + +int +_ao_serial0_pollchar(void) +{ + int c; + + if (ao_fifo_empty(ao_usart_rx_fifo)) + c = AO_READ_AGAIN; + else { + uint8_t u; + ao_fifo_remove(ao_usart_rx_fifo,u); + c = u; + } + return c; +} + +char +ao_serial0_getchar(void) +{ + int c; + ao_arch_block_interrupts(); + while ((c = _ao_serial0_pollchar()) == AO_READ_AGAIN) + ao_sleep(&ao_usart_rx_fifo); + ao_arch_release_interrupts(); + return (char) c; +} + +void +ao_serial0_putchar(char c) +{ + ao_arch_block_interrupts(); + while (ao_fifo_full(ao_usart_tx_fifo)) + ao_sleep(&ao_usart_tx_fifo); + ao_fifo_insert(ao_usart_tx_fifo, c); + _ao_serial_tx_start(); + ao_arch_release_interrupts(); +} + +void +ao_serial0_drain(void) +{ + ao_arch_block_interrupts(); + while (!ao_fifo_empty(ao_usart_tx_fifo)) + ao_sleep(&ao_usart_tx_fifo); + ao_arch_release_interrupts(); +} + +#include "ao_serial_lpc.h" + +void +ao_serial0_set_speed(uint8_t speed) +{ + if (speed > AO_SERIAL_SPEED_115200) + return; + + /* Flip to allow access to divisor latches */ + lpc_usart.lcr |= (1 << LPC_USART_LCR_DLAB); + + /* DL LSB */ + lpc_usart.rbr_thr = ao_usart_speeds[speed].dl & 0xff; + + /* DL MSB */ + lpc_usart.ier = (ao_usart_speeds[speed].dl >> 8) & 0xff; + + lpc_usart.fdr = ((ao_usart_speeds[speed].divaddval << LPC_USART_FDR_DIVADDVAL) | + (ao_usart_speeds[speed].mulval << LPC_USART_FDR_MULVAL)); + + /* Turn access to divisor latches back off */ + lpc_usart.lcr &= ~(1 << LPC_USART_LCR_DLAB); +} + +void +ao_serial_init(void) +{ +#if SERIAL_0_18_19 + lpc_ioconf.pio0_18 = ((LPC_IOCONF_FUNC_PIO0_18_RXD << LPC_IOCONF_FUNC) | + (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) | + (0 << LPC_IOCONF_HYS) | + (0 << LPC_IOCONF_INV) | + (0 << LPC_IOCONF_OD)); + lpc_ioconf.pio0_19 = ((LPC_IOCONF_FUNC_PIO0_19_TXD << LPC_IOCONF_FUNC) | + (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) | + (0 << LPC_IOCONF_HYS) | + (0 << LPC_IOCONF_INV) | + (0 << LPC_IOCONF_OD)); +#endif + + /* Turn on the USART */ + lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_USART); + + /* Turn on the USART clock */ + lpc_scb.uartclkdiv = AO_LPC_CLKOUT / AO_LPC_USARTCLK; + + /* Configure USART */ + + /* Enable FIFOs, reset fifo contents, interrupt on 1 received char */ + lpc_usart.iir_fcr = ((1 << LPC_USART_FCR_FIFOEN) | + (1 << LPC_USART_FCR_RXFIFORES) | + (1 << LPC_USART_FCR_TXFIFORES) | + (LPC_USART_FCR_RXTL_1 << LPC_USART_FCR_RXTL)); + + ao_usart_tx_avail = LPC_USART_TX_FIFO_SIZE; + ao_usart_tx_avail_min = LPC_USART_TX_FIFO_SIZE; + + /* 8 n 1 */ + lpc_usart.lcr = ((LPC_USART_LCR_WLS_8 << LPC_USART_LCR_WLS) | + (LPC_USART_LCR_SBS_1 << LPC_USART_LCR_SBS) | + (0 << LPC_USART_LCR_PE) | + (LPC_USART_LCR_PS_ODD << LPC_USART_LCR_PS) | + (0 << LPC_USART_LCR_BC) | + (0 << LPC_USART_LCR_DLAB)); + + /* Disable flow control */ + lpc_usart.mcr = ((0 << LPC_USART_MCR_DTRCTRL) | + (0 << LPC_USART_MCR_RTSCTRL) | + (0 << LPC_USART_MCR_LMS) | + (0 << LPC_USART_MCR_RTSEN) | + (0 << LPC_USART_MCR_CTSEN)); + + /* 16x oversampling */ + lpc_usart.osr = ((0 << LPC_USART_OSR_OSFRAC) | + ((16 - 1) << LPC_USART_OSR_OSINT) | + (0 << LPC_USART_OSR_FDINT)); + + /* Full duplex */ + lpc_usart.hden = ((0 << LPC_USART_HDEN_HDEN)); + + /* Set baud rate */ + ao_serial0_set_speed(AO_SERIAL_SPEED_9600); + + /* Enable interrupts */ + lpc_usart.ier = ((1 << LPC_USART_IER_RBRINTEN) | + (1 << LPC_USART_IER_THREINTEN)); + + lpc_nvic_set_enable(LPC_ISR_USART_POS); + lpc_nvic_set_priority(LPC_ISR_USART_POS, 0); +#if USE_SERIAL_0_STDIN + ao_add_stdio(_ao_serial_pollchar, + ao_serial_putchar, + NULL); +#endif +} diff --git a/src/lpc/ao_spi_lpc.c b/src/lpc/ao_spi_lpc.c new file mode 100644 index 00000000..a889137c --- /dev/null +++ b/src/lpc/ao_spi_lpc.c @@ -0,0 +1,207 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> + +static uint8_t ao_spi_mutex[LPC_NUM_SPI]; + +static struct lpc_ssp * const ao_lpc_ssp[LPC_NUM_SPI] = { &lpc_ssp0, &lpc_ssp1 }; + +static uint8_t spi_dev_null; + +#define tx_busy(lpc_ssp) (lpc_ssp->sr & ((1 << LPC_SSP_SR_BSY) | (1 << LPC_SSP_SR_TNF))) != (1 << LPC_SSP_SR_TNF) +#define rx_busy(lpc_ssp) (lpc_ssp->sr & ((1 << LPC_SSP_SR_BSY) | (1 << LPC_SSP_SR_RNE))) != (1 << LPC_SSP_SR_RNE) + +#define spi_loop(len, put, get) do { \ + while (len--) { \ + /* Wait for space in the fifo */ \ + while (tx_busy(lpc_ssp)) \ + ; \ + \ + /* send a byte */ \ + lpc_ssp->dr = put; \ + \ + /* Wait for byte to appear in the fifo */ \ + while (rx_busy(lpc_ssp)) \ + ; \ + \ + /* recv a byte */ \ + get lpc_ssp->dr; \ + } \ + } while (0) + +void +ao_spi_send(void *block, uint16_t len, uint8_t id) +{ + uint8_t *b = block; + struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id]; + + spi_loop(len, *b++, (void)); +} + +void +ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t id) +{ + struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id]; + + spi_loop(len, value, (void)); +} + +void +ao_spi_recv(void *block, uint16_t len, uint8_t id) +{ + uint8_t *b = block; + struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id]; + + spi_loop(len, 0xff, *b++ =); +} + +void +ao_spi_duplex(void *out, void *in, uint16_t len, uint8_t id) +{ + uint8_t *o = out; + uint8_t *i = in; + struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id]; + + spi_loop(len, *o++, *i++ =); +} + +void +ao_spi_get(uint8_t id, uint32_t speed) +{ + struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id]; + + ao_mutex_get(&ao_spi_mutex[id]); + + /* Set the clock prescale */ + lpc_ssp->cpsr = speed; +} + +void +ao_spi_put(uint8_t id) +{ + ao_mutex_put(&ao_spi_mutex[id]); +} + +static void +ao_spi_channel_init(uint8_t id) +{ + struct lpc_ssp *lpc_ssp = ao_lpc_ssp[id]; + uint8_t d; + + lpc_ssp->cr0 = ((LPC_SSP_CR0_DSS_8 << LPC_SSP_CR0_DSS) | + (LPC_SSP_CR0_FRF_SPI << LPC_SSP_CR0_FRF) | + (0 << LPC_SSP_CR0_CPOL) | + (0 << LPC_SSP_CR0_CPHA) | + (0 << LPC_SSP_CR0_SCR)); + + /* Enable the device */ + lpc_ssp->cr1 = ((0 << LPC_SSP_CR1_LBM) | + (1 << LPC_SSP_CR1_SSE) | + (LPC_SSP_CR1_MS_MASTER << LPC_SSP_CR1_MS) | + (0 << LPC_SSP_CR1_SOD)); + + /* Drain the receive fifo */ + for (d = 0; d < LPC_SSP_FIFOSIZE; d++) + (void) lpc_ssp->dr; +} + +void +ao_spi_init(void) +{ +#if HAS_SPI_0 + /* Configure pins */ +#if SPI_SCK0_P0_6 + lpc_ioconf.pio0_6 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO0_6_SCK0); +#define HAS_SCK0 +#endif +#if SPI_SCK0_P0_10 + lpc_ioconf.pio0_10 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO0_10_SCK0); +#define HAS_SCK0 +#endif +#if SPI_SCK0_P1_29 + lpc_ioconf.pio1_29 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_29_SCK0); +#define HAS_SCK0 +#endif +#ifndef HAS_SCK0 +#error "No pin specified for SCK0" +#endif + lpc_ioconf.pio0_8 = ao_lpc_alternate(LPC_IOCONF_FUNC_MISO0); + lpc_ioconf.pio0_9 = ao_lpc_alternate(LPC_IOCONF_FUNC_MOSI0); + + /* Enable the device */ + lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_SSP0); + + /* Turn on the clock */ + lpc_scb.ssp0clkdiv = 1; + + /* Reset the device */ + lpc_scb.presetctrl &= ~(1 << LPC_SCB_PRESETCTRL_SSP0_RST_N); + lpc_scb.presetctrl |= (1 << LPC_SCB_PRESETCTRL_SSP0_RST_N); + ao_spi_channel_init(0); +#endif + +#if HAS_SPI_1 + +#if SPI_SCK1_P1_15 + lpc_ioconf.pio1_15 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_15_SCK1); +#define HAS_SCK1 +#endif +#if SPI_SCK1_P1_20 + lpc_ioconf.pio1_20 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_20_SCK1); +#define HAS_SCK1 +#endif +#ifndef HAS_SCK1 +#error "No pin specified for SCK1" +#endif + +#if SPI_MISO1_P0_22 + lpc_ioconf.pio0_22 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO0_22_MISO1); +#define HAS_MISO1 +#endif +#if SPI_MISO1_P1_21 + lpc_ioconf.pio1_21 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_21_MISO1); +#define HAS_MISO1 +#endif +#ifndef HAS_MISO1 +#error "No pin specified for MISO1" +#endif + +#if SPI_MOSI1_P0_21 + lpc_ioconf.pio0_21 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO0_21_MOSI1); +#define HAS_MOSI1 +#endif +#if SPI_MOSI1_P1_22 + lpc_ioconf.pio1_22 = ao_lpc_alternate(LPC_IOCONF_FUNC_PIO1_22_MOSI1); +#define HAS_MOSI1 +#endif +#ifndef HAS_MOSI1 +#error "No pin specified for MOSI1" +#endif + + /* Enable the device */ + lpc_scb.sysahbclkctrl |= (1 << LPC_SCB_SYSAHBCLKCTRL_SSP1); + + /* Turn on the clock */ + lpc_scb.ssp1clkdiv = 1; + + /* Reset the device */ + lpc_scb.presetctrl &= ~(1 << LPC_SCB_PRESETCTRL_SSP1_RST_N); + lpc_scb.presetctrl |= (1 << LPC_SCB_PRESETCTRL_SSP1_RST_N); + ao_spi_channel_init(1); +#endif /* HAS_SPI_1 */ +} diff --git a/src/lpc/ao_timer_lpc.c b/src/lpc/ao_timer_lpc.c new file mode 100644 index 00000000..44fb410e --- /dev/null +++ b/src/lpc/ao_timer_lpc.c @@ -0,0 +1,213 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> + +volatile __data AO_TICK_TYPE ao_tick_count; + +uint16_t +ao_time(void) +{ + return ao_tick_count; +} + +#if AO_DATA_ALL +volatile __data uint8_t ao_data_interval = 1; +volatile __data uint8_t ao_data_count; +#endif + +void lpc_systick_isr(void) +{ + if (lpc_systick.csr & (1 << LPC_SYSTICK_CSR_COUNTFLAG)) { + ++ao_tick_count; +#if HAS_TASK_QUEUE + if (ao_task_alarm_tick && (int16_t) (ao_tick_count - ao_task_alarm_tick) >= 0) + ao_task_check_alarm((uint16_t) ao_tick_count); +#endif +#if AO_DATA_ALL + if (++ao_data_count == ao_data_interval) { + ao_data_count = 0; + ao_adc_poll(); +#if (AO_DATA_ALL & ~(AO_DATA_ADC)) + ao_wakeup((void *) &ao_data_count); +#endif + } +#endif + } +} + +#if HAS_ADC +void +ao_timer_set_adc_interval(uint8_t interval) +{ + ao_arch_critical( + ao_data_interval = interval; + ao_data_count = 0; + ); +} +#endif + +#define SYSTICK_RELOAD ((AO_LPC_SYSCLK / 2) / 100 - 1) + +/* Initialize our 100Hz clock */ +void +ao_timer_init(void) +{ + lpc_systick.rvr = SYSTICK_RELOAD; + lpc_systick.cvr = 0; + lpc_systick.csr = ((1 << LPC_SYSTICK_CSR_ENABLE) | + (1 << LPC_SYSTICK_CSR_TICKINT) | + (LPC_SYSTICK_CSR_CLKSOURCE_CPU_OVER_2 << LPC_SYSTICK_CSR_CLKSOURCE)); +} + +#define AO_LPC_M ((AO_LPC_CLKOUT / AO_LPC_CLKIN) - 1) + +#define AO_LPC_FCCO_MIN 156000000 + +static void +ao_clock_delay(void) +{ + uint32_t i; + for (i = 0; i < 200; i++) + ao_arch_nop(); +} + +void +ao_clock_init(void) +{ + uint8_t p; + uint32_t i; + + /* Turn off all perhipherals except for GPIO configuration */ + lpc_scb.sysahbclkctrl = ((1 << LPC_SCB_SYSAHBCLKCTRL_SYS) | + (1 << LPC_SCB_SYSAHBCLKCTRL_ROM) | + (1 << LPC_SCB_SYSAHBCLKCTRL_RAM0) | + (1 << LPC_SCB_SYSAHBCLKCTRL_FLASHARRAY) | + (1 << LPC_SCB_SYSAHBCLKCTRL_GPIO) | + (1 << LPC_SCB_SYSAHBCLKCTRL_IOCON)); + + /* Enable the brown-out detection at the highest voltage to + * make sure the flash part remains happy + */ + + lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_BOD_PD); + lpc_scb.bodctrl = ((LPC_SCB_BOD_BODRSTLEV_2_63 << LPC_SCB_BOD_BODRSTLEV) | + (LPC_SCB_BOD_BODINTVAL_RESERVED << LPC_SCB_BOD_BODINTVAL) | + (1 << LPC_SCB_BOD_BODRSTENA)); + + /* Turn the IRC clock back on */ + lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_IRC_PD); + ao_clock_delay(); + + /* Switch to the IRC clock */ + lpc_scb.mainclksel = LPC_SCB_MAINCLKSEL_SEL_IRC << LPC_SCB_MAINCLKSEL_SEL; + lpc_scb.mainclkuen = (0 << LPC_SCB_MAINCLKUEN_ENA); + lpc_scb.mainclkuen = (1 << LPC_SCB_MAINCLKUEN_ENA); + while (!(lpc_scb.mainclkuen & (1 << LPC_SCB_MAINCLKUEN_ENA))) + ; + + /* Switch USB to the main clock */ + lpc_scb.usbclksel = (LPC_SCB_USBCLKSEL_SEL_MAIN_CLOCK << LPC_SCB_USBCLKSEL_SEL); + lpc_scb.usbclkuen = (0 << LPC_SCB_USBCLKUEN_ENA); + lpc_scb.usbclkuen = (1 << LPC_SCB_USBCLKUEN_ENA); + while (!(lpc_scb.usbclkuen & (1 << LPC_SCB_USBCLKUEN_ENA))) + ; + + /* Find a PLL post divider ratio that gets the FCCO in range */ + for (p = 0; p < 4; p++) + if (AO_LPC_CLKOUT << (1 + p) >= AO_LPC_FCCO_MIN) + break; + + if (p == 4) + ao_panic(AO_PANIC_CRASH); + + /* Power down the PLL before touching the registers */ + lpc_scb.pdruncfg |= (1 << LPC_SCB_PDRUNCFG_SYSPLL_PD); + ao_clock_delay(); + + /* Set PLL divider values */ + lpc_scb.syspllctrl = ((AO_LPC_M << LPC_SCB_SYSPLLCTRL_MSEL) | + (p << LPC_SCB_SYSPLLCTRL_PSEL)); + + /* Turn off the external crystal clock */ + lpc_scb.pdruncfg |= (1 << LPC_SCB_PDRUNCFG_SYSOSC_PD); + ao_clock_delay(); + + /* Configure the crystal clock */ + lpc_scb.sysoscctrl = ((0 << LPC_SCB_SYSOSCCTRL_BYPASS) | /* using a crystal */ + ((AO_LPC_CLKIN > 15000000) << LPC_SCB_SYSOSCCTRL_FREQRANGE));/* set range */ + + /* Turn on the external crystal clock */ + lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_SYSOSC_PD); + ao_clock_delay(); + + /* Select crystal as PLL input */ + + lpc_scb.syspllclksel = (LPC_SCB_SYSPLLCLKSEL_SEL_SYSOSC << LPC_SCB_SYSPLLCLKSEL_SEL); + lpc_scb.syspllclkuen = (1 << LPC_SCB_SYSPLLCLKUEN_ENA); + lpc_scb.syspllclkuen = (0 << LPC_SCB_SYSPLLCLKUEN_ENA); + lpc_scb.syspllclkuen = (1 << LPC_SCB_SYSPLLCLKUEN_ENA); + while (!(lpc_scb.syspllclkuen & (1 << LPC_SCB_SYSPLLCLKUEN_ENA))) + ; + + /* Turn on the PLL */ + lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_SYSPLL_PD); + + /* Wait for it to lock */ + + for (i = 0; i < 20000; i++) + if (lpc_scb.syspllstat & (1 << LPC_SCB_SYSPLLSTAT_LOCK)) + break; + if (i == 20000) + ao_panic(AO_PANIC_CRASH); + + /* Switch to the PLL */ + lpc_scb.mainclksel = LPC_SCB_MAINCLKSEL_SEL_PLL_OUTPUT << LPC_SCB_MAINCLKSEL_SEL; + lpc_scb.mainclkuen = (1 << LPC_SCB_MAINCLKUEN_ENA); + lpc_scb.mainclkuen = (0 << LPC_SCB_MAINCLKUEN_ENA); + lpc_scb.mainclkuen = (1 << LPC_SCB_MAINCLKUEN_ENA); + while (!(lpc_scb.mainclkuen & (1 << LPC_SCB_MAINCLKUEN_ENA))) + ; + + /* Set system clock divider */ + lpc_scb.sysahbclkdiv = AO_LPC_CLKOUT / AO_LPC_SYSCLK; + + /* Shut down perhipheral clocks (enabled as needed) */ + lpc_scb.ssp0clkdiv = 0; + lpc_scb.uartclkdiv = 0; + lpc_scb.ssp1clkdiv = 0; + lpc_scb.usbclkdiv = 0; + lpc_scb.clkoutdiv = 0; + + /* Switch USB PLL source to system osc so we can power down the IRC */ + lpc_scb.usbpllclksel = (LPC_SCB_USBPLLCLKSEL_SEL_SYSOSC << LPC_SCB_USBPLLCLKSEL_SEL); + lpc_scb.usbpllclkuen = (0 << LPC_SCB_USBPLLCLKUEN_ENA); + lpc_scb.usbpllclkuen = (1 << LPC_SCB_USBPLLCLKUEN_ENA); + while (!(lpc_scb.usbpllclkuen & (1 << LPC_SCB_USBPLLCLKUEN_ENA))) + ; + + /* Power down everything we don't need */ + lpc_scb.pdruncfg = ((1 << LPC_SCB_PDRUNCFG_IRCOUT_PD) | + (1 << LPC_SCB_PDRUNCFG_IRC_PD) | + (0 << LPC_SCB_PDRUNCFG_BOD_PD) | + (1 << LPC_SCB_PDRUNCFG_ADC_PD) | + (1 << LPC_SCB_PDRUNCFG_WDTOSC_PD) | + (1 << LPC_SCB_PDRUNCFG_USBPLL_PD) | + (1 << LPC_SCB_PDRUNCFG_USBPAD_PD) | + (1 << 11) | + (7 << 13)); +} diff --git a/src/lpc/ao_usb_lpc.c b/src/lpc/ao_usb_lpc.c new file mode 100644 index 00000000..686dc3a4 --- /dev/null +++ b/src/lpc/ao_usb_lpc.c @@ -0,0 +1,1055 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include "ao_usb.h" +#include "ao_product.h" + +#ifndef USE_USB_STDIO +#define USE_USB_STDIO 1 +#endif + +#if USE_USB_STDIO +#define AO_USB_OUT_SLEEP_ADDR (&ao_stdin_ready) +#else +#define AO_USB_OUT_SLEEP_ADDR (&ao_usb_out_avail) +#endif + +#define USB_DEBUG 0 +#define USB_DEBUG_DATA 0 +#define USB_ECHO 0 + +#if USB_DEBUG +#define debug(format, args...) printf(format, ## args); +#else +#define debug(format, args...) +#endif + +#if USB_DEBUG_DATA +#define debug_data(format, args...) printf(format, ## args); +#else +#define debug_data(format, args...) +#endif + +struct ao_usb_setup { + uint8_t dir_type_recip; + uint8_t request; + uint16_t value; + uint16_t index; + uint16_t length; +} ao_usb_setup; + +static uint8_t ao_usb_ep0_state; + +/* Pending EP0 IN data */ +static const uint8_t *ao_usb_ep0_in_data; /* Remaining data */ +static uint8_t ao_usb_ep0_in_len; /* Remaining amount */ +static uint16_t ao_usb_ep0_in_max; /* Requested amount from host */ + +/* Temp buffer for smaller EP0 in data */ +static uint8_t ao_usb_ep0_in_buf[2]; + +/* Pending EP0 OUT data */ +static uint8_t *ao_usb_ep0_out_data; +static uint8_t ao_usb_ep0_out_len; + +/* + * Objects allocated in special USB memory + */ + +/* USB address of end of allocated storage */ +static uint8_t *ao_usb_sram; + +/* Pointer to ep0 tx/rx buffers in USB memory */ +static uint8_t *ao_usb_ep0_tx_buffer; +static uint8_t *ao_usb_ep0_setup_buffer; +static uint8_t *ao_usb_ep0_rx_buffer; + +/* Pointer to bulk data tx/rx buffers in USB memory */ +static uint8_t *ao_usb_in_tx_buffer; +static uint8_t *ao_usb_out_rx_buffer; + +/* Our data buffers */ +static uint8_t ao_usb_tx_buffer[AO_USB_IN_SIZE]; +static uint8_t ao_usb_tx_count; + +static uint8_t ao_usb_rx_buffer[AO_USB_OUT_SIZE]; +static uint8_t ao_usb_rx_count, ao_usb_rx_pos; + +extern struct lpc_usb_endpoint lpc_usb_endpoint; + +/* Marks when we don't need to send an IN packet. + * This happens only when the last IN packet is not full, + * otherwise the host will expect to keep seeing packets. + * Send a zero-length packet as required + */ +static uint8_t ao_usb_in_flushed; + +/* Marks when we have delivered an IN packet to the hardware + * and it has not been received yet. ao_sleep on this address + * to wait for it to be delivered. + */ +static uint8_t ao_usb_in_pending; + +/* Marks when an OUT packet has been received by the hardware + * but not pulled to the shadow buffer. + */ +static uint8_t ao_usb_out_avail; +static uint8_t ao_usb_running; +static uint8_t ao_usb_configuration; +static uint8_t ueienx_0; + +#define AO_USB_EP0_GOT_RESET 1 +#define AO_USB_EP0_GOT_SETUP 2 +#define AO_USB_EP0_GOT_RX_DATA 4 +#define AO_USB_EP0_GOT_TX_ACK 8 + +static uint8_t ao_usb_ep0_receive; +static uint8_t ao_usb_address; +static uint8_t ao_usb_address_pending; + +static inline uint32_t set_toggle(uint32_t current_value, + uint32_t mask, + uint32_t desired_value) +{ + return (current_value ^ desired_value) & mask; +} + +/* + * Set current device address and mark the + * interface as active + */ +void +ao_usb_set_address(uint8_t address) +{ + debug("ao_usb_set_address %02x\n", address); + lpc_usb.devcmdstat = ((address << LPC_USB_DEVCMDSTAT_DEV_ADDR) | + (1 << LPC_USB_DEVCMDSTAT_DEV_EN) | + (0 << LPC_USB_DEVCMDSTAT_SETUP) | + (0 << LPC_USB_DEVCMDSTAT_PLL_ON) | + (0 << LPC_USB_DEVCMDSTAT_LPM_SUP) | + (0 << LPC_USB_DEVCMDSTAT_INTONNAK_AO) | + (0 << LPC_USB_DEVCMDSTAT_INTONNAK_AI) | + (0 << LPC_USB_DEVCMDSTAT_INTONNAK_CO) | + (0 << LPC_USB_DEVCMDSTAT_INTONNAK_CI) | + (1 << LPC_USB_DEVCMDSTAT_DCON) | + (0 << LPC_USB_DEVCMDSTAT_DSUS) | + (0 << LPC_USB_DEVCMDSTAT_DCON_C) | + (0 << LPC_USB_DEVCMDSTAT_DSUS_C) | + (0 << LPC_USB_DEVCMDSTAT_DRES_C) | + (0 << LPC_USB_DEVCMDSTAT_VBUSDEBOUNCED)); + ao_usb_address_pending = 0; +} + +#define TX_DBG 0 +#define RX_DBG 0 + +#if TX_DBG +#define _tx_dbg0(msg) _dbg(__LINE__,msg,0) +#define _tx_dbg1(msg,value) _dbg(__LINE__,msg,value) +#else +#define _tx_dbg0(msg) +#define _tx_dbg1(msg,value) +#endif + +#if RX_DBG +#define _rx_dbg0(msg) _dbg(__LINE__,msg,0) +#define _rx_dbg1(msg,value) _dbg(__LINE__,msg,value) +#else +#define _rx_dbg0(msg) +#define _rx_dbg1(msg,value) +#endif + +#if TX_DBG || RX_DBG +static void _dbg(int line, char *msg, uint32_t value); +#endif + +/* + * Set just endpoint 0, for use during startup + */ + +static uint8_t * +ao_usb_alloc_sram(uint16_t size) +{ + uint8_t *addr = ao_usb_sram; + + ao_usb_sram += (size + 63) & ~63; + return addr; +} + +static uint16_t +ao_usb_sram_offset(uint8_t *addr) +{ + return (uint16_t) ((intptr_t) addr >> 6); +} + +static void +ao_usb_set_ep(vuint32_t *ep, uint8_t *addr, uint16_t nbytes) +{ + *ep = ((ao_usb_sram_offset(addr) << LPC_USB_EP_OFFSET) | + (nbytes << LPC_USB_EP_NBYTES) | + (0 << LPC_USB_EP_ENDPOINT_ISO) | + (0 << LPC_USB_EP_RATE_FEEDBACK) | + (0 << LPC_USB_EP_TOGGLE_RESET) | + (0 << LPC_USB_EP_STALL) | + (0 << LPC_USB_EP_DISABLED) | + (1 << LPC_USB_EP_ACTIVE)); +} + +static inline uint16_t +ao_usb_ep_count(vuint32_t *ep) +{ + return (*ep >> LPC_USB_EP_NBYTES) & LPC_USB_EP_NBYTES_MASK; +} + +static inline uint8_t +ao_usb_ep_stall(vuint32_t *ep) +{ + return (*ep >> LPC_USB_EP_STALL) & 1; +} + +static inline vuint32_t * +ao_usb_ep0_out(void) +{ + return &lpc_usb_endpoint.ep0_out; +} + +static inline vuint32_t * +ao_usb_ep0_in(void) +{ + return &lpc_usb_endpoint.ep0_in; +} + +static inline vuint32_t * +ao_usb_epn_out(uint8_t n) +{ + return &lpc_usb_endpoint.epn[n-1].out[0]; +} + +static inline vuint32_t * +ao_usb_epn_in(uint8_t n) +{ + return &lpc_usb_endpoint.epn[n-1].in[0]; +} + +static void +ao_usb_set_epn_in(uint8_t n, uint8_t *addr, uint16_t nbytes) +{ + ao_usb_set_ep(ao_usb_epn_in(n), addr, nbytes); +} + +static void +ao_usb_set_epn_out(uint8_t n, uint8_t *addr, uint16_t nbytes) +{ + ao_usb_set_ep(ao_usb_epn_out(n), addr, nbytes); +} + +static inline uint16_t +ao_usb_epn_out_count(uint8_t n) +{ + return ao_usb_ep_count(ao_usb_epn_out(n)); +} + +static inline uint16_t +ao_usb_epn_in_count(uint8_t n) +{ + return ao_usb_ep_count(ao_usb_epn_in(n)); +} + +static uint8_t * +ao_usb_enable_ep(vuint32_t *ep, uint16_t nbytes, uint16_t set_nbytes) +{ + uint8_t *addr = ao_usb_alloc_sram(nbytes); + + ao_usb_set_ep(ep, addr, set_nbytes); + return addr; +} + +static void +ao_usb_disable_ep(vuint32_t *ep) +{ + *ep = ((0 << LPC_USB_EP_OFFSET) | + (0 << LPC_USB_EP_NBYTES) | + (0 << LPC_USB_EP_ENDPOINT_ISO) | + (0 << LPC_USB_EP_RATE_FEEDBACK) | + (0 << LPC_USB_EP_TOGGLE_RESET) | + (0 << LPC_USB_EP_STALL) | + (1 << LPC_USB_EP_DISABLED) | + (0 << LPC_USB_EP_ACTIVE)); +} + +static void +ao_usb_enable_epn(uint8_t n, uint16_t out_bytes, uint8_t **out_addr, uint16_t in_bytes, uint8_t **in_addr) +{ + uint8_t *addr; + + addr = ao_usb_enable_ep(ao_usb_epn_out(n), out_bytes, out_bytes); + if (out_addr) + *out_addr = addr; + ao_usb_disable_ep(&lpc_usb_endpoint.epn[n-1].out[1]); + + addr = ao_usb_enable_ep(ao_usb_epn_in(n), in_bytes, 0); + if (in_addr) + *in_addr = addr; + ao_usb_disable_ep(&lpc_usb_endpoint.epn[n-1].in[1]); +} + +static void +ao_usb_disable_epn(uint8_t n) +{ + ao_usb_disable_ep(ao_usb_epn_out(n)); + ao_usb_disable_ep(&lpc_usb_endpoint.epn[n-1].out[1]); + ao_usb_disable_ep(ao_usb_epn_in(n)); + ao_usb_disable_ep(&lpc_usb_endpoint.epn[n-1].in[1]); +} + +static void +ao_usb_reset(void) +{ + ao_usb_set_address(0); + ao_usb_configuration = 0; +} + +static void +ao_usb_set_ep0(void) +{ + int e; + + /* Everything is single buffered for now */ + lpc_usb.epbufcfg = 0; + lpc_usb.epinuse = 0; + lpc_usb.epskip = 0xffffffff; + + lpc_usb.intstat = 0xc00003ff; + + ao_usb_sram = lpc_usb_sram; + + lpc_usb.epliststart = (uint32_t) (intptr_t) &lpc_usb_endpoint; + lpc_usb.databufstart = ((uint32_t) (intptr_t) ao_usb_sram) & 0xffc00000; + + /* Set up EP 0 - a Control end point with 32 bytes of in and out buffers */ + + ao_usb_ep0_rx_buffer = ao_usb_enable_ep(ao_usb_ep0_out(), AO_USB_CONTROL_SIZE, AO_USB_CONTROL_SIZE); + ao_usb_ep0_setup_buffer = ao_usb_alloc_sram(AO_USB_CONTROL_SIZE); + lpc_usb_endpoint.setup = ao_usb_sram_offset(ao_usb_ep0_setup_buffer); + ao_usb_ep0_tx_buffer = ao_usb_enable_ep(ao_usb_ep0_in(), AO_USB_CONTROL_SIZE, 0); + + /* Clear all of the other endpoints */ + for (e = 1; e <= 4; e++) + ao_usb_disable_epn(e); + ao_usb_reset(); +} + +static void +ao_usb_set_configuration(void) +{ + debug ("ao_usb_set_configuration\n"); + + /* Set up the INT end point */ + ao_usb_enable_epn(AO_USB_INT_EP, 0, NULL, 0, NULL); + + /* Set up the OUT end point */ + ao_usb_enable_epn(AO_USB_OUT_EP, AO_USB_OUT_SIZE, &ao_usb_out_rx_buffer, 0, NULL); + + /* Set up the IN end point */ + ao_usb_enable_epn(AO_USB_IN_EP, 0, NULL, AO_USB_IN_SIZE, &ao_usb_in_tx_buffer); + + ao_usb_running = 1; +} + +/* Send an IN data packet */ +static void +ao_usb_ep0_flush(void) +{ + uint8_t this_len; + + this_len = ao_usb_ep0_in_len; + if (this_len > AO_USB_CONTROL_SIZE) + this_len = AO_USB_CONTROL_SIZE; + + ao_usb_ep0_in_len -= this_len; + ao_usb_ep0_in_max -= this_len; + + if (this_len < AO_USB_CONTROL_SIZE || ao_usb_ep0_in_max == 0) + ao_usb_ep0_state = AO_USB_EP0_IDLE; + + debug_data ("Flush EP0 len %d:", this_len); + memcpy(ao_usb_ep0_tx_buffer, ao_usb_ep0_in_data, this_len); + debug_data ("\n"); + ao_usb_ep0_in_data += this_len; + + /* Mark the endpoint as TX valid to send the packet */ + ao_usb_set_ep(ao_usb_ep0_in(), ao_usb_ep0_tx_buffer, this_len); + debug ("queue tx. 0 now %08x\n", *ao_usb_ep0_in()); +} + +/* Read data from the ep0 OUT fifo */ +static void +ao_usb_ep0_fill(void) +{ + uint16_t len; + uint8_t *rx_buffer; + + /* Pull all of the data out of the packet */ + if (lpc_usb.devcmdstat & (1 << LPC_USB_DEVCMDSTAT_SETUP)) { + rx_buffer = ao_usb_ep0_setup_buffer; + len = 8; + } else { + rx_buffer = ao_usb_ep0_rx_buffer; + len = AO_USB_CONTROL_SIZE - ao_usb_ep_count(ao_usb_ep0_out()); + } + + if (len > ao_usb_ep0_out_len) + len = ao_usb_ep0_out_len; + ao_usb_ep0_out_len -= len; + + debug_data ("Fill EP0 len %d:", len); + memcpy(ao_usb_ep0_out_data, rx_buffer, len); + debug_data ("\n"); + ao_usb_ep0_out_data += len; + + /* ACK the packet */ + ao_usb_set_ep(ao_usb_ep0_out(), ao_usb_ep0_rx_buffer, AO_USB_CONTROL_SIZE); + lpc_usb.devcmdstat |= (1 << LPC_USB_DEVCMDSTAT_SETUP); +} + +static void +ao_usb_ep0_in_reset(void) +{ + ao_usb_ep0_in_data = ao_usb_ep0_in_buf; + ao_usb_ep0_in_len = 0; +} + +static void +ao_usb_ep0_in_queue_byte(uint8_t a) +{ + if (ao_usb_ep0_in_len < sizeof (ao_usb_ep0_in_buf)) + ao_usb_ep0_in_buf[ao_usb_ep0_in_len++] = a; +} + +static void +ao_usb_ep0_in_set(const uint8_t *data, uint8_t len) +{ + ao_usb_ep0_in_data = data; + ao_usb_ep0_in_len = len; +} + +static void +ao_usb_ep0_out_set(uint8_t *data, uint8_t len) +{ + ao_usb_ep0_out_data = data; + ao_usb_ep0_out_len = len; +} + +static void +ao_usb_ep0_in_start(uint16_t max) +{ + ao_usb_ep0_in_max = max; + /* Don't send more than asked for */ + if (ao_usb_ep0_in_len > max) + ao_usb_ep0_in_len = max; + ao_usb_ep0_flush(); +} + +static struct ao_usb_line_coding ao_usb_line_coding = {115200, 0, 0, 8}; + +/* Walk through the list of descriptors and find a match + */ +static void +ao_usb_get_descriptor(uint16_t value) +{ + const uint8_t *descriptor; + uint8_t type = value >> 8; + uint8_t index = value; + + descriptor = ao_usb_descriptors; + while (descriptor[0] != 0) { + if (descriptor[1] == type && index-- == 0) { + uint8_t len; + if (type == AO_USB_DESC_CONFIGURATION) + len = descriptor[2]; + else + len = descriptor[0]; + ao_usb_ep0_in_set(descriptor, len); + break; + } + descriptor += descriptor[0]; + } +} + +static void +ao_usb_ep0_setup(void) +{ + /* Pull the setup packet out of the fifo */ + ao_usb_ep0_out_set((uint8_t *) &ao_usb_setup, 8); + ao_usb_ep0_fill(); + if (ao_usb_ep0_out_len != 0) { + debug ("invalid setup packet length\n"); + return; + } + + if ((ao_usb_setup.dir_type_recip & AO_USB_DIR_IN) || ao_usb_setup.length == 0) + ao_usb_ep0_state = AO_USB_EP0_DATA_IN; + else + ao_usb_ep0_state = AO_USB_EP0_DATA_OUT; + + ao_usb_ep0_in_reset(); + + switch(ao_usb_setup.dir_type_recip & AO_USB_SETUP_TYPE_MASK) { + case AO_USB_TYPE_STANDARD: + debug ("Standard setup packet\n"); + switch(ao_usb_setup.dir_type_recip & AO_USB_SETUP_RECIP_MASK) { + case AO_USB_RECIP_DEVICE: + debug ("Device setup packet\n"); + switch(ao_usb_setup.request) { + case AO_USB_REQ_GET_STATUS: + debug ("get status\n"); + ao_usb_ep0_in_queue_byte(0); + ao_usb_ep0_in_queue_byte(0); + break; + case AO_USB_REQ_SET_ADDRESS: + debug ("set address %d\n", ao_usb_setup.value); + ao_usb_address = ao_usb_setup.value; + ao_usb_address_pending = 1; + break; + case AO_USB_REQ_GET_DESCRIPTOR: + debug ("get descriptor %d\n", ao_usb_setup.value); + ao_usb_get_descriptor(ao_usb_setup.value); + break; + case AO_USB_REQ_GET_CONFIGURATION: + debug ("get configuration %d\n", ao_usb_configuration); + ao_usb_ep0_in_queue_byte(ao_usb_configuration); + break; + case AO_USB_REQ_SET_CONFIGURATION: + ao_usb_configuration = ao_usb_setup.value; + debug ("set configuration %d\n", ao_usb_configuration); + ao_usb_set_configuration(); + break; + } + break; + case AO_USB_RECIP_INTERFACE: + debug ("Interface setup packet\n"); + switch(ao_usb_setup.request) { + case AO_USB_REQ_GET_STATUS: + ao_usb_ep0_in_queue_byte(0); + ao_usb_ep0_in_queue_byte(0); + break; + case AO_USB_REQ_GET_INTERFACE: + ao_usb_ep0_in_queue_byte(0); + break; + case AO_USB_REQ_SET_INTERFACE: + break; + } + break; + case AO_USB_RECIP_ENDPOINT: + debug ("Endpoint setup packet\n"); + switch(ao_usb_setup.request) { + case AO_USB_REQ_GET_STATUS: + ao_usb_ep0_in_queue_byte(0); + ao_usb_ep0_in_queue_byte(0); + break; + } + break; + } + break; + case AO_USB_TYPE_CLASS: + debug ("Class setup packet\n"); + switch (ao_usb_setup.request) { + case AO_USB_SET_LINE_CODING: + debug ("set line coding\n"); + ao_usb_ep0_out_set((uint8_t *) &ao_usb_line_coding, 7); + break; + case AO_USB_GET_LINE_CODING: + debug ("get line coding\n"); + ao_usb_ep0_in_set((const uint8_t *) &ao_usb_line_coding, 7); + break; + case AO_USB_SET_CONTROL_LINE_STATE: + break; + } + break; + } + + /* If we're not waiting to receive data from the host, + * queue an IN response + */ + if (ao_usb_ep0_state == AO_USB_EP0_DATA_IN) + ao_usb_ep0_in_start(ao_usb_setup.length); +} + +static void +ao_usb_ep0_handle(uint8_t receive) +{ + ao_usb_ep0_receive = 0; + + if (receive & AO_USB_EP0_GOT_RESET) { + debug ("\treset\n"); + ao_usb_reset(); + return; + } + if (receive & AO_USB_EP0_GOT_SETUP) { + debug ("\tsetup\n"); + ao_usb_ep0_setup(); + } + if (receive & AO_USB_EP0_GOT_RX_DATA) { + debug ("\tgot rx data\n"); + if (ao_usb_ep0_state == AO_USB_EP0_DATA_OUT) { + ao_usb_ep0_fill(); + if (ao_usb_ep0_out_len == 0) { + ao_usb_ep0_state = AO_USB_EP0_DATA_IN; + ao_usb_ep0_in_start(0); + } + } + } + if (receive & AO_USB_EP0_GOT_TX_ACK) { + debug ("\tgot tx ack\n"); + + /* Wait until the IN packet is received from addr 0 + * before assigning our local address + */ + if (ao_usb_address_pending) { +#if HAS_FLIGHT + /* Go to idle mode if USB is connected + */ + ao_flight_force_idle = 1; +#endif + ao_usb_set_address(ao_usb_address); + } + if (ao_usb_ep0_state == AO_USB_EP0_DATA_IN) + ao_usb_ep0_flush(); + } +} + +static uint16_t control_count; +static uint16_t int_count; +static uint16_t in_count; +static uint16_t out_count; +static uint16_t reset_count; + +void +lpc_usb_irq_isr(void) +{ + uint32_t intstat = lpc_usb.intstat & lpc_usb.inten; + + lpc_usb.intstat = intstat; + /* Handle EP0 OUT packets */ + if (intstat & (1 << LPC_USB_INT_EPOUT(0))) { + if (lpc_usb.devcmdstat & (1 << LPC_USB_DEVCMDSTAT_SETUP)) + ao_usb_ep0_receive |= AO_USB_EP0_GOT_SETUP; + else + ao_usb_ep0_receive |= AO_USB_EP0_GOT_RX_DATA; + + ao_usb_ep0_handle(ao_usb_ep0_receive); + } + + /* Handle EP0 IN packets */ + if (intstat & (1 << LPC_USB_INT_EPIN(0))) { + ao_usb_ep0_receive |= AO_USB_EP0_GOT_TX_ACK; + + ao_usb_ep0_handle(ao_usb_ep0_receive); + } + + + /* Handle OUT packets */ + if (intstat & (1 << LPC_USB_INT_EPOUT(AO_USB_OUT_EP))) { + ++out_count; + _rx_dbg1("RX ISR", *ao_usb_epn_out(AO_USB_OUT_EP)); + ao_usb_out_avail = 1; + _rx_dbg0("out avail set"); + ao_wakeup(AO_USB_OUT_SLEEP_ADDR) + _rx_dbg0("stdin awoken"); + } + + /* Handle IN packets */ + if (intstat & (1 << LPC_USB_INT_EPIN(AO_USB_IN_EP))) { + ++in_count; + _tx_dbg1("TX ISR", *ao_usb_epn_in(AO_USB_IN_EP)); + ao_usb_in_pending = 0; + ao_wakeup(&ao_usb_in_pending); + } + + /* NAK all INT EP IN packets */ + if (intstat & (1 << LPC_USB_INT_EPIN(AO_USB_INT_EP))) { + ; + } + + /* Check for reset */ + if (intstat & (1 << LPC_USB_INT_DEV)) { + if (lpc_usb.devcmdstat & (1 << LPC_USB_DEVCMDSTAT_DRES_C)) + { + lpc_usb.devcmdstat |= (1 << LPC_USB_DEVCMDSTAT_DRES_C); + ao_usb_ep0_receive |= AO_USB_EP0_GOT_RESET; + ao_usb_ep0_handle(ao_usb_ep0_receive); + } + } +} + + + +/* Queue the current IN buffer for transmission */ +static void +_ao_usb_in_send(void) +{ + _tx_dbg0("in_send start"); + debug ("send %d\n", ao_usb_tx_count); + while (ao_usb_in_pending) + ao_sleep(&ao_usb_in_pending); + ao_usb_in_pending = 1; + if (ao_usb_tx_count != AO_USB_IN_SIZE) + ao_usb_in_flushed = 1; + memcpy(ao_usb_in_tx_buffer, ao_usb_tx_buffer, ao_usb_tx_count); + ao_usb_set_ep(ao_usb_epn_in(AO_USB_IN_EP), ao_usb_in_tx_buffer, ao_usb_tx_count); + ao_usb_tx_count = 0; + _tx_dbg0("in_send end"); +} + +/* Wait for a free IN buffer. Interrupts are blocked */ +static void +_ao_usb_in_wait(void) +{ + for (;;) { + /* Check if the current buffer is writable */ + if (ao_usb_tx_count < AO_USB_IN_SIZE) + break; + + _tx_dbg0("in_wait top"); + /* Wait for an IN buffer to be ready */ + while (ao_usb_in_pending) + ao_sleep(&ao_usb_in_pending); + _tx_dbg0("in_wait bottom"); + } +} + +void +ao_usb_flush(void) +{ + if (!ao_usb_running) + return; + + /* Anytime we've sent a character since + * the last time we flushed, we'll need + * to send a packet -- the only other time + * we would send a packet is when that + * packet was full, in which case we now + * want to send an empty packet + */ + ao_arch_block_interrupts(); + while (!ao_usb_in_flushed) { + _tx_dbg0("flush top"); + _ao_usb_in_send(); + _tx_dbg0("flush end"); + } + ao_arch_release_interrupts(); +} + +void +ao_usb_putchar(char c) +{ + if (!ao_usb_running) + return; + + ao_arch_block_interrupts(); + _ao_usb_in_wait(); + + ao_usb_in_flushed = 0; + ao_usb_tx_buffer[ao_usb_tx_count++] = (uint8_t) c; + + /* Send the packet when full */ + if (ao_usb_tx_count == AO_USB_IN_SIZE) { + _tx_dbg0("putchar full"); + _ao_usb_in_send(); + _tx_dbg0("putchar flushed"); + } + ao_arch_release_interrupts(); +} + +static void +_ao_usb_out_recv(void) +{ + _rx_dbg0("out_recv top"); + ao_usb_out_avail = 0; + + ao_usb_rx_count = AO_USB_OUT_SIZE - ao_usb_epn_out_count(AO_USB_OUT_EP); + + _rx_dbg1("out_recv count", ao_usb_rx_count); + debug ("recv %d\n", ao_usb_rx_count); + debug_data("Fill OUT len %d:", ao_usb_rx_count); + memcpy(ao_usb_rx_buffer, ao_usb_out_rx_buffer, ao_usb_rx_count); + debug_data("\n"); + ao_usb_rx_pos = 0; + + /* ACK the packet */ + ao_usb_set_epn_out(AO_USB_OUT_EP, ao_usb_out_rx_buffer, AO_USB_OUT_SIZE); +} + +int +_ao_usb_pollchar(void) +{ + uint8_t c; + + if (!ao_usb_running) + return AO_READ_AGAIN; + + for (;;) { + if (ao_usb_rx_pos != ao_usb_rx_count) + break; + + _rx_dbg0("poll check"); + /* Check to see if a packet has arrived */ + if (!ao_usb_out_avail) { + _rx_dbg0("poll none"); + return AO_READ_AGAIN; + } + _ao_usb_out_recv(); + } + + /* Pull a character out of the fifo */ + c = ao_usb_rx_buffer[ao_usb_rx_pos++]; + return c; +} + +char +ao_usb_getchar(void) +{ + int c; + + ao_arch_block_interrupts(); + while ((c = _ao_usb_pollchar()) == AO_READ_AGAIN) + ao_sleep(AO_USB_OUT_SLEEP_ADDR); + ao_arch_release_interrupts(); + return c; +} + +void +ao_usb_disable(void) +{ + ao_arch_block_interrupts(); + +#if HAS_USB_PULLUP + ao_gpio_set(AO_USB_PULLUP_PORT, AO_USB_PULLUP_PIN, AO_USB_PULLUP, 0); +#endif + /* Disable interrupts */ + lpc_usb.inten = 0; + + lpc_nvic_clear_enable(LPC_ISR_USB_IRQ_POS); + + /* Disable the device */ + lpc_usb.devcmdstat = 0; + + /* Turn off USB clock */ + lpc_scb.usbclkdiv = 0; + + /* Disable USB PHY and PLL */ + lpc_scb.pdruncfg |= ((1 << LPC_SCB_PDRUNCFG_USBPAD_PD) | + (1 << LPC_SCB_PDRUNCFG_USBPLL_PD)); + + /* Disable USB registers and RAM */ + lpc_scb.sysahbclkctrl &= ~((1 << LPC_SCB_SYSAHBCLKCTRL_USB) | + (1 << LPC_SCB_SYSAHBCLKCTRL_USBRAM)); + + ao_arch_release_interrupts(); +} + +void +ao_usb_enable(void) +{ + int t; + + /* Enable USB pins */ +#if HAS_USB_CONNECT + lpc_ioconf.pio0_6 = ((LPC_IOCONF_FUNC_USB_CONNECT << LPC_IOCONF_FUNC) | + (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) | + (0 << LPC_IOCONF_HYS) | + (0 << LPC_IOCONF_INV) | + (0 << LPC_IOCONF_OD) | + 0x80); +#endif +#if HAS_USB_VBUS + lpc_ioconf.pio0_3 = ((LPC_IOCONF_FUNC_USB_VBUS << LPC_IOCONF_FUNC) | + (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) | + (0 << LPC_IOCONF_HYS) | + (0 << LPC_IOCONF_INV) | + (0 << LPC_IOCONF_OD) | + 0x80); +#endif + /* Enable USB registers and RAM */ + lpc_scb.sysahbclkctrl |= ((1 << LPC_SCB_SYSAHBCLKCTRL_USB) | + (1 << LPC_SCB_SYSAHBCLKCTRL_USBRAM)); + + /* Enable USB PHY */ + lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_USBPAD_PD); + + /* Turn on USB PLL */ + lpc_scb.pdruncfg &= ~(1 << LPC_SCB_PDRUNCFG_USBPLL_PD); + + lpc_scb.usbpllclksel = (LPC_SCB_SYSPLLCLKSEL_SEL_SYSOSC << LPC_SCB_SYSPLLCLKSEL_SEL); + lpc_scb.usbpllclkuen = (0 << LPC_SCB_USBPLLCLKUEN_ENA); + lpc_scb.usbpllclkuen = (1 << LPC_SCB_USBPLLCLKUEN_ENA); + while (!(lpc_scb.usbpllclkuen & (1 << LPC_SCB_USBPLLCLKUEN_ENA))) + ; + lpc_scb.usbpllctrl = 0x23; + while (!(lpc_scb.usbpllstat & 1)) + ; + + lpc_scb.usbclksel = 0; + lpc_scb.usbclkuen = (0 << LPC_SCB_USBCLKUEN_ENA); + lpc_scb.usbclkuen = (1 << LPC_SCB_USBCLKUEN_ENA); + while (!(lpc_scb.usbclkuen & (1 << LPC_SCB_USBCLKUEN_ENA))) + ; + + /* Turn on USB clock, use 48MHz clock unchanged */ + lpc_scb.usbclkdiv = 1; + + /* Configure interrupts */ + ao_arch_block_interrupts(); + + /* Route all interrupts to the main isr */ + lpc_usb.introuting = 0; + + /* Configure NVIC */ + + lpc_nvic_set_enable(LPC_ISR_USB_IRQ_POS); + lpc_nvic_set_priority(LPC_ISR_USB_IRQ_POS, 0); + + /* Clear any spurious interrupts */ + lpc_usb.intstat = 0xffffffff; + + debug ("ao_usb_enable\n"); + + /* Enable interrupts */ + lpc_usb.inten = ((1 << LPC_USB_INT_EPOUT(0)) | + (1 << LPC_USB_INT_EPIN(0)) | + (1 << LPC_USB_INT_EPIN(AO_USB_INT_EP)) | + (1 << LPC_USB_INT_EPOUT(AO_USB_OUT_EP)) | + (1 << LPC_USB_INT_EPIN(AO_USB_IN_EP)) | + (1 << LPC_USB_INT_DEV)); + + ao_arch_release_interrupts(); + + lpc_usb.devcmdstat = 0; + for (t = 0; t < 1000; t++) + ao_arch_nop(); + + ao_usb_set_ep0(); + +#if HAS_USB_PULLUP + ao_gpio_set(AO_USB_PULLUP_PORT, AO_USB_PULLUP_PIN, AO_USB_PULLUP, 1); +#endif +} + +#if USB_ECHO +struct ao_task ao_usb_echo_task; + +static void +ao_usb_echo(void) +{ + char c; + + for (;;) { + c = ao_usb_getchar(); + ao_usb_putchar(c); + ao_usb_flush(); + } +} +#endif + +#if USB_DEBUG +static void +ao_usb_irq(void) +{ + printf ("control: %d out: %d in: %d int: %d reset: %d\n", + control_count, out_count, in_count, int_count, reset_count); +} + +__code struct ao_cmds ao_usb_cmds[] = { + { ao_usb_irq, "I\0Show USB interrupt counts" }, + { 0, NULL } +}; +#endif + +void +ao_usb_init(void) +{ +#if HAS_USB_PULLUP + ao_enable_output(AO_USB_PULLUP_PORT, AO_USB_PULLUP_PIN, AO_USB_PULLUP, 0); +#endif + + ao_usb_enable(); + + debug ("ao_usb_init\n"); +#if USB_ECHO + ao_add_task(&ao_usb_echo_task, ao_usb_echo, "usb echo"); +#endif +#if USB_DEBUG + ao_cmd_register(&ao_usb_cmds[0]); +#endif +#if USE_USB_STDIO + ao_add_stdio(_ao_usb_pollchar, ao_usb_putchar, ao_usb_flush); +#endif +} + +#if TX_DBG || RX_DBG + +struct ao_usb_dbg { + int line; + char *msg; + uint32_t value; + uint32_t primask; +#if TX_DBG + uint16_t in_count; + uint32_t in_ep; + uint32_t in_pending; + uint32_t tx_count; + uint32_t in_flushed; +#endif +#if RX_DBG + uint8_t rx_count; + uint8_t rx_pos; + uint8_t out_avail; + uint32_t out_ep; +#endif +}; + +#define NUM_USB_DBG 8 + +static struct ao_usb_dbg dbg[NUM_USB_DBG]; +static int dbg_i; + +static void _dbg(int line, char *msg, uint32_t value) +{ + uint32_t primask; + dbg[dbg_i].line = line; + dbg[dbg_i].msg = msg; + dbg[dbg_i].value = value; + asm("mrs %0,primask" : "=&r" (primask)); + dbg[dbg_i].primask = primask; +#if TX_DBG + dbg[dbg_i].in_count = in_count; + dbg[dbg_i].in_ep = *ao_usb_epn_in(AO_USB_IN_EP); + dbg[dbg_i].in_pending = ao_usb_in_pending; + dbg[dbg_i].tx_count = ao_usb_tx_count; + dbg[dbg_i].in_flushed = ao_usb_in_flushed; +#endif +#if RX_DBG + dbg[dbg_i].rx_count = ao_usb_rx_count; + dbg[dbg_i].rx_pos = ao_usb_rx_pos; + dbg[dbg_i].out_avail = ao_usb_out_avail; + dbg[dbg_i].out_ep = *ao_usb_epn_out(AO_USB_OUT_EP); +#endif + if (++dbg_i == NUM_USB_DBG) + dbg_i = 0; +} +#endif diff --git a/src/lpc/baud_rate b/src/lpc/baud_rate new file mode 100644 index 00000000..2bfbf309 --- /dev/null +++ b/src/lpc/baud_rate @@ -0,0 +1,131 @@ +#!/usr/bin/env nickle + +/* + * Given a main clock frequency, + * compute USART clock freq and a table + * of USART config parameters for our target baud rates + */ + +real main_clock = 0; +real usart_clock = 0; + +real[] baud_rates = { 4800, 9600, 19200, 57600, 115200 }; + +void +compute_baud_rate(real rate) { + int divaddval; + int mulval; + + real dl_est = usart_clock / (16 * rate); + + if (dl_est == floor(dl_est)) { + divaddval = 0; + mulval = 1; + } else { + if (false) { + + /* This is how the docs suggest doing it; this + * generates a rate which is reasonably close + */ + + real fr_est = 1.5; + + /* Compute fractional estimate */ + do { + dl_est = floor(usart_clock / (16 * rate * fr_est) + 0.5); + fr_est = usart_clock / (16 * rate * dl_est); + } while (fr_est <= 1.1 || 1.9 <= fr_est); + + /* Given fractional estimate, compute divaddval/mulvals that work best */ + + real best_dist = 1000; + for (int tmp_divaddval = 1; tmp_divaddval < 15; tmp_divaddval++) { + for (int tmp_mulval = 1; tmp_mulval < 16; tmp_mulval++) { + real fr = 1 + tmp_divaddval / tmp_mulval; + real dist = abs(fr - fr_est); + if (dist < best_dist) { + divaddval = tmp_divaddval; + mulval = tmp_mulval; + best_dist = dist; + } + } + } + } else { + + /* This exhaustively searches for the best match */ + + real my_best_dist = 1e20; + int my_best_dl; + int my_best_divaddval; + int my_best_mulval; + for (int my_dl = 1; my_dl < 1024; my_dl++) { + for (int my_mulval = 1; my_mulval < 16; my_mulval++) { + for (int my_divaddval = 0; my_divaddval < my_mulval; my_divaddval++) { + real my_rate = usart_clock / ((16 * my_dl) * (1 + my_divaddval/my_mulval)); + + real my_dist = abs(rate - my_rate); + + if (my_dist == 0 && my_divaddval == 0) { + my_dist = -1; + } + + if (my_dist < my_best_dist) { + my_best_dl = my_dl; + my_best_divaddval = my_divaddval; + my_best_mulval = my_mulval; + my_best_dist = my_dist; + } + } + } + } + + dl_est = my_best_dl; + divaddval = my_best_divaddval; + mulval = my_best_mulval; + } + } + + int dl = floor (dl_est); + + real actual = usart_clock / ((16 * dl) * (1 + divaddval/mulval)); + + printf("\t[AO_SERIAL_SPEED_%d] = { /* actual = %8.2f */\n", floor(rate), actual); + printf("\t\t.dl = %d,\n", dl); + printf("\t\t.divaddval = %d,\n", divaddval); + printf("\t\t.mulval = %d\n", mulval); + printf("\t},\n"); +} + +void +main() { + if (dim(argv) < 2) { + printf ("usage: %s <main-clock>\n", argv[0]); + exit(1); + } + main_clock = string_to_real(argv[1]); + + for (int div = 0; div < 4; div++) { + if (main_clock / (1 << div) <= 12000000) { + usart_clock = main_clock / (1 << div); + break; + } + } + + if (usart_clock == 0) { + printf ("can't get usart clock in range\n"); + exit(1); + } + + printf ("#define AO_LPC_USARTCLK %d\n\n", floor(usart_clock)); + printf("static const struct {\n"); + printf("\tuint16_t dl;\n"); + printf("\tuint8_t divaddval;\n"); + printf("\tuint8_t mulval;\n"); + printf("} ao_usart_speeds[] = {\n"); + for (int i = 0; i < dim(baud_rates); i++) { + compute_baud_rate(baud_rates[i]); + } + printf ("};\n"); +} + +main(); diff --git a/src/lpc/figure-checksum b/src/lpc/figure-checksum new file mode 100755 index 00000000..0b1de578 --- /dev/null +++ b/src/lpc/figure-checksum @@ -0,0 +1,39 @@ +#!/usr/bin/env nickle + +autoimport Process; + +int byteflip(int x) { + return ((x >> 24) & 0xff) | ((x >> 8) & 0xff00) | ((x << 8) & 0xff0000) | ((x << 24) & 0xff000000); +} + +void main () { + file input = popen(popen_direction.read, true, "objdump", + "objdump", "-j", ".text", + "--start-address=0", + "--stop-address=0x20", + "-s", argv[1]); + int sum = 0; + + void add_in(int addr, int value) { + if (addr < 0x1c) { + sum += value; + } else if (addr == 0x1c) { + printf ("-DCKSUM=0x%08x\n", -sum & 0xffffffff); + exit(0); + } + } + while (!File::end(input)) { + string line = File::fgets(input); + string[] words = String::wordsplit(line, " "); + + if (dim(words) < 5) + continue; + if (words[0] == "0000" || words[0] == "0010") { + int addr = string_to_integer(words[0], 16); + for (int i = 0; i < 4; i++) + add_in(addr + i * 4, byteflip(string_to_integer(words[i+1], 16))); + } + } +} + +main(); diff --git a/src/lpc/lpc.h b/src/lpc/lpc.h new file mode 100644 index 00000000..3300c86f --- /dev/null +++ b/src/lpc/lpc.h @@ -0,0 +1,1249 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _LPC_H_ +#define _LPC_H_ + +#include <stdint.h> + +typedef volatile uint32_t vuint32_t; +typedef volatile uint16_t vuint16_t; +typedef volatile uint8_t vuint8_t; +typedef volatile void * vvoid_t; + +struct lpc_ioconf { + vuint32_t pio0_0; + vuint32_t pio0_1; + vuint32_t pio0_2; + vuint32_t pio0_3; + + vuint32_t pio0_4; + vuint32_t pio0_5; + vuint32_t pio0_6; + vuint32_t pio0_7; + + vuint32_t pio0_8; + vuint32_t pio0_9; + vuint32_t pio0_10; + vuint32_t pio0_11; + + vuint32_t pio0_12; + vuint32_t pio0_13; + vuint32_t pio0_14; + vuint32_t pio0_15; + + vuint32_t pio0_16; + vuint32_t pio0_17; + vuint32_t pio0_18; + vuint32_t pio0_19; + + vuint32_t pio0_20; + vuint32_t pio0_21; + vuint32_t pio0_22; + vuint32_t pio0_23; + + vuint32_t pio1_0; /* 0x60 */ + vuint32_t pio1_1; + vuint32_t pio1_2; + vuint32_t pio1_3; + + vuint32_t pio1_4; + vuint32_t pio1_5; + vuint32_t pio1_6; + vuint32_t pio1_7; + + vuint32_t pio1_8; /* 0x80 */ + vuint32_t pio1_9; + vuint32_t pio1_10; + vuint32_t pio1_11; + + vuint32_t pio1_12; + vuint32_t pio1_13; + vuint32_t pio1_14; + vuint32_t pio1_15; + + vuint32_t pio1_16; /* 0xa0 */ + vuint32_t pio1_17; + vuint32_t pio1_18; + vuint32_t pio1_19; + + vuint32_t pio1_20; + vuint32_t pio1_21; + vuint32_t pio1_22; + vuint32_t pio1_23; + + vuint32_t pio1_24; /* 0xc0 */ + vuint32_t pio1_25; + vuint32_t pio1_26; + vuint32_t pio1_27; + + vuint32_t pio1_28; + vuint32_t pio1_29; + vuint32_t pio1_30; + vuint32_t pio1_31; +}; + +extern struct lpc_ioconf lpc_ioconf; + +#define LPC_IOCONF_FUNC 0 + +/* PIO0_0 */ +#define LPC_IOCONF_FUNC_RESET 0 +#define LPC_IOCONF_FUNC_PIO0_0 1 + +/* PIO0_1 */ +#define LPC_IOCONF_FUNC_PIO0_1 0 +#define LPC_IOCONF_FUNC_CLKOUT 1 +#define LPC_IOCONF_FUNC_CT32B0_MAT2 2 +#define LPC_IOCONF_FUNC_USB_FTOGGLE 3 + +/* PIO0_2 */ +#define LPC_IOCONF_FUNC_PIO0_2 0 +#define LPC_IOCONF_FUNC_SSEL0 1 +#define LPC_IOCONF_FUNC_CT16B0_CAP0 2 + +/* PIO0_3 */ +#define LPC_IOCONF_FUNC_PIO0_3 0 +#define LPC_IOCONF_FUNC_USB_VBUS 1 + +/* PIO0_4 +#define LPC_IOCONF_FUNC_PIO0_4 0 +#define LPC_IOCONF_FUNC_I2C_SCL 1 + +/* PIO0_5 */ +#define LPC_IOCONF_FUNC_PIO0_5 0 +#define LPC_IOCONF_FUNC_I2C_SDA 1 + +/* PIO0_6 */ +#define LPC_IOCONF_FUNC_PIO0_6 0 +#define LPC_IOCONF_FUNC_USB_CONNECT 1 +#define LPC_IOCONF_FUNC_PIO0_6_SCK0 2 + +/* PIO0_7 */ +#define LPC_IOCONF_FUNC_PIO0_7 0 +#define LPC_IOCONF_FUNC_CTS 1 + +/* PIO0_8 */ +#define LPC_IOCONF_FUNC_PIO0_8 0 +#define LPC_IOCONF_FUNC_MISO0 1 +#define LPC_IOCONF_FUNC_CT16B0_MAT0 2 + +/* PIO0_9 */ +#define LPC_IOCONF_FUNC_PIO0_9 0 +#define LPC_IOCONF_FUNC_MOSI0 1 +#define LPC_IOCONF_FUNC_CT16B0_MAT1 2 + +/* PIO0_10 */ +#define LPC_IOCONF_FUNC_SWCLK 0 +#define LPC_IOCONF_FUNC_PIO0_10 1 +#define LPC_IOCONF_FUNC_PIO0_10_SCK0 2 +#define LPC_IOCONF_FUNC_CT16B0_MAT2 3 + +/* PIO0_11 */ +#define LPC_IOCONF_FUNC_TDI 0 +#define LPC_IOCONF_FUNC_PIO0_11 1 +#define LPC_IOCONF_FUNC_AD0 2 +#define LPC_IOCONF_FUNC_CT32B0_MAT3 3 + +/* PIO0_12 */ +#define LPC_IOCONF_FUNC_TMS 0 +#define LPC_IOCONF_FUNC_PIO0_12 1 +#define LPC_IOCONF_FUNC_AD1 2 +#define LPC_IOCONF_FUNC_CT32B1_CAP0 3 + +/* PIO0_13 */ +#define LPC_IOCONF_FUNC_TD0 0 +#define LPC_IOCONF_FUNC_PIO0_13 1 +#define LPC_IOCONF_FUNC_AD2 2 +#define LPC_IOCONF_FUNC_CT32B1_MAT0 3 + +/* PIO0_14 */ +#define LPC_IOCONF_FUNC_TRST 0 +#define LPC_IOCONF_FUNC_PIO0_14 1 +#define LPC_IOCONF_FUNC_AD3 2 +#define LPC_IOCONF_FUNC_PIO0_14_CT32B1_MAT1 3 + +/* PIO0_15 */ +#define LPC_IOCONF_FUNC_SWDIO 0 +#define LPC_IOCONF_FUNC_PIO0_15 1 +#define LPC_IOCONF_FUNC_AD4 2 +#define LPC_IOCONF_FUNC_CT32B1_MAT2 3 + +/* PIO0_16 */ +#define LPC_IOCONF_FUNC_PIO0_16 0 +#define LPC_IOCONF_FUNC_AD5 1 +#define LPC_IOCONF_FUNC_CT32B1_MAT3 2 + +/* PIO0_17 */ +#define LPC_IOCONF_FUNC_PIO0_17 0 +#define LPC_IOCONF_FUNC_RTS 1 +#define LPC_IOCONF_FUNC_CT32B0_CAP0 2 +#define LPC_IOCONF_FUNC_SCLK 3 + +/* PIO0_18 */ +#define LPC_IOCONF_FUNC_PIO0_18 0 +#define LPC_IOCONF_FUNC_PIO0_18_RXD 1 +#define LPC_IOCONF_FUNC_PIO0_18_CT32B0_MAT0 2 + +/* PIO0_19 */ +#define LPC_IOCONF_FUNC_PIO0_19 0 +#define LPC_IOCONF_FUNC_PIO0_19_TXD 1 +#define LPC_IOCONF_FUNC_PIO0_19_CT32B0_MAT1 2 + +/* PIO0_20 */ +#define LPC_IOCONF_FUNC_PIO0_20 0 +#define LPC_IOCONF_FUNC_CT16B1_CAP0 1 + +/* PIO0_21 */ +#define LPC_IOCONF_FUNC_PIO0_21 0 +#define LPC_IOCONF_FUNC_CT16B1_MAT0 1 +#define LPC_IOCONF_FUNC_PIO0_21_MOSI1 2 + +/* PIO0_22 */ +#define LPC_IOCONF_FUNC_PIO0_22 0 +#define LPC_IOCONF_FUNC_AD6 1 +#define LPC_IOCONF_FUNC_CT16B1_MAT1 2 +#define LPC_IOCONF_FUNC_PIO0_22_MISO1 3 + +/* PIO0_23 */ +#define LPC_IOCONF_FUNC_PIO0_23 0 +#define LPC_IOCONF_FUNC_AD7 1 + +/* PIO1_0 */ +#define LPC_IOCONF_FUNC_PIO1_0 0 +#define LPC_IOCONF_FUNC_CT32B1_MAT1 1 + +/* PIO1_1 */ +#define LPC_IOCONF_FUNC_PIO1_1 0 +#define LPC_IOCONF_FUNC_CT32B1_MAT1 1 + +/* PIO1_2 */ +#define LPC_IOCONF_FUNC_PIO1_2 0 +#define LPC_IOCONF_FUNC_PIO1_2_CT32B1_MAT2 1 + +/* PIO1_3*/ +#define LPC_IOCONF_FUNC_PIO1_3 0 +#define LPC_IOCONF_FUNC_PIO1_3_CT32B1_MAT3 1 + +/* PIO1_4 */ +#define LPC_IOCONF_FUNC_PIO1_4 0 +#define LPC_IOCONF_FUNC_PIO1_4_CT32B1_CAP0 1 + +/* PIO1_5 */ +#define LPC_IOCONF_FUNC_PIO1_5 0 +#define LPC_IOCONF_FUNC_CT32B1_CAP1 1 + +/* PIO1_6 */ +#define LPC_IOCONF_FUNC_PIO1_6 0 + +/* PIO1_7 */ +#define LPC_IOCONF_FUNC_PIO1_7 0 + +/* PIO1_8 */ +#define LPC_IOCONF_FUNC_PIO1_8 0 + +/* PIO1_9 */ +#define LPC_IOCONF_FUNC_PIO1_9 0 + +/* PIO1_10 */ +#define LPC_IOCONF_FUNC_PIO1_10 0 + +/* PIO1_11 */ +#define LPC_IOCONF_FUNC_PIO1_11 0 + +/* PIO1_12 */ +#define LPC_IOCONF_FUNC_PIO1_12 0 + +/* PIO1_13 */ +#define LPC_IOCONF_FUNC_PIO1_13 0 +#define LPC_IOCONF_FUNC_DTR 1 +#define LPC_IOCONF_FUNC_CT16B0_MAT0 2 +#define LPC_IOCONF_FUNC_PIO1_13_TXD 3 + +/* PIO1_14 */ +#define LPC_IOCONF_FUNC_PIO1_14 0 +#define LPC_IOCONF_FUNC_DSR 1 +#define LPC_IOCONF_FUNC_CT16B0_MAT1 2 +#define LPC_IOCONF_FUNC_PIO1_13_RXD 3 + +/* PIO1_15 */ +#define LPC_IOCONF_FUNC_PIO1_15 0 +#define LPC_IOCONF_FUNC_DCD 1 +#define LPC_IOCONF_FUNC_PIO1_15_CT16B0_MAT2 2 +#define LPC_IOCONF_FUNC_PIO1_15_SCK1 3 + +/* PIO1_16 */ +#define LPC_IOCONF_FUNC_PIO1_16 0 +#define LPC_IOCONF_FUNC_RI 1 +#define LPC_IOCONF_FUNC_CT16B0_CAP0 2 + +/* PIO1_17 */ +#define LPC_IOCONF_FUNC_PIO1_17 0 +#define LPC_IOCONF_FUNC_CT16B0_CAP1 1 +#define LPC_IOCONF_FUNC_PIO1_17_RXD 2 + +/* PIO1_18 */ +#define LPC_IOCONF_FUNC_PIO1_18 0 +#define LPC_IOCONF_FUNC_CT16B1_CAP1 1 +#define LPC_IOCONF_FUNC_PIO1_18_TXD 2 + +/* PIO1_19 */ +#define LPC_IOCONF_FUNC_PIO1_19 0 +#define LPC_IOCONF_FUNC_DTR 1 +#define LPC_IOCONF_FUNC_SSEL1 2 + +/* PIO1_20 */ +#define LPC_IOCONF_FUNC_PIO1_20 0 +#define LPC_IOCONF_FUNC_DSR 1 +#define LPC_IOCONF_FUNC_PIO1_20_SCK1 2 + +/* PIO1_21 */ +#define LPC_IOCONF_FUNC_PIO1_21 0 +#define LPC_IOCONF_FUNC_DCD 1 +#define LPC_IOCONF_FUNC_PIO1_21_MISO1 2 + +/* PIO1_22 */ +#define LPC_IOCONF_FUNC_PIO1_22 0 +#define LPC_IOCONF_FUNC_RI 1 +#define LPC_IOCONF_FUNC_PIO1_22_MOSI1 2 + +/* PIO1_23 */ +#define LPC_IOCONF_FUNC_PIO1_23 0 +#define LPC_IOCONF_FUNC_PIO1_23_CT16B1_MAT1 1 +#define LPC_IOCONF_FUNC_SSEL1 2 + +/* PIO1_24 */ +#define LPC_IOCONF_FUNC_PIO1_24 0 +#define LPC_IOCONF_FUNC_PIO1_24_CT32B0_MAT0 1 + +/* PIO1_25 */ +#define LPC_IOCONF_FUNC_PIO1_25 0 +#define LPC_IOCONF_FUNC_PIO1_25_CT32B0_MAT1 1 + +/* PIO1_26 */ +#define LPC_IOCONF_FUNC_PIO1_26 0 +#define LPC_IOCONF_FUNC_PIO1_26_CT32B0_MAT2 1 +#define LPC_IOCONF_FUNC_PIO1_26_RXD 2 + +/* PIO1_27 */ +#define LPC_IOCONF_FUNC_PIO1_27 0 +#define LPC_IOCONF_FUNC_PIO1_27_CT32B0_MAT3 1 +#define LPC_IOCONF_FUNC_PIO1_27_TXD 2 + +/* PIO1_28 */ +#define LPC_IOCONF_FUNC_PIO1_28 0 +#define LPC_IOCONF_FUNC_PIO1_28_CT32B0_CAP0 1 +#define LPC_IOCONF_FUNC_PIO1_28_SCLK 2 + +/* PIO1_29 */ +#define LPC_IOCONF_FUNC_PIO1_29 0 +#define LPC_IOCONF_FUNC_PIO1_29_SCK0 1 +#define LPC_IOCONF_FUNC_PIO1_29_CT32B0_CAP1 2 + +/* PIO1_31 */ +#define LPC_IOCONF_FUNC_PIO1_31 0 + +#define LPC_IOCONF_FUNC_MASK 0x7 + +#define ao_lpc_alternate(func) (((func) << LPC_IOCONF_FUNC) | \ + (LPC_IOCONF_MODE_INACTIVE << LPC_IOCONF_MODE) | \ + (0 << LPC_IOCONF_HYS) | \ + (0 << LPC_IOCONF_INV) | \ + (0 << LPC_IOCONF_OD) | \ + 0x80) + +#define LPC_IOCONF_MODE 3 +#define LPC_IOCONF_MODE_INACTIVE 0 +#define LPC_IOCONF_MODE_PULL_DOWN 1 +#define LPC_IOCONF_MODE_PULL_UP 2 +#define LPC_IOCONF_MODE_REPEATER 3 +#define LPC_IOCONF_MODE_MASK 3 + +#define LPC_IOCONF_HYS 5 + +#define LPC_IOCONF_INV 6 +#define LPC_IOCONF_ADMODE 7 +#define LPC_IOCONF_FILTR 8 +#define LPC_IOCONF_OD 10 + +struct lpc_scb { + vuint32_t sysmemremap; /* 0x00 */ + vuint32_t presetctrl; + vuint32_t syspllctrl; + vuint32_t syspllstat; + + vuint32_t usbpllctrl; /* 0x10 */ + vuint32_t usbpllstat; + uint32_t r18; + uint32_t r1c; + + vuint32_t sysoscctrl; /* 0x20 */ + vuint32_t wdtoscctrl; + uint32_t r28; + uint32_t r2c; + + vuint32_t sysrststat; /* 0x30 */ + uint32_t r34; + uint32_t r38; + uint32_t r3c; + + vuint32_t syspllclksel; /* 0x40 */ + vuint32_t syspllclkuen; + vuint32_t usbpllclksel; + vuint32_t usbpllclkuen; + + uint32_t r50[8]; + + vuint32_t mainclksel; /* 0x70 */ + vuint32_t mainclkuen; + vuint32_t sysahbclkdiv; + uint32_t r7c; + + vuint32_t sysahbclkctrl; /* 0x80 */ + uint32_t r84[3]; + + uint32_t r90; /* 0x90 */ + vuint32_t ssp0clkdiv; + vuint32_t uartclkdiv; + vuint32_t ssp1clkdiv; + + uint32_t ra0[8]; + + vuint32_t usbclksel; /* 0xc0 */ + vuint32_t usbclkuen; + vuint32_t usbclkdiv; + uint32_t rcc; + + uint32_t rd0[4]; + + vuint32_t clkoutsel; /* 0xe0 */ + vuint32_t clkoutuen; + vuint32_t clkoutdiv; + uint32_t rec; + + uint32_t rf0[4]; /* 0xf0 */ + + vuint32_t pioporcap0; /* 0x100 */ + vuint32_t pioporcap1; + uint32_t r102[2]; + + uint32_t r110[4]; /* 0x110 */ + uint32_t r120[4]; /* 0x120 */ + uint32_t r130[4]; /* 0x130 */ + uint32_t r140[4]; /* 0x140 */ + + vuint32_t bodctrl; /* 0x150 */ + vuint32_t systckcal; + uint32_t r158[2]; + + uint32_t r160[4]; /* 0x160 */ + + vuint32_t irqlatency; /* 0x170 */ + vuint32_t nmisrc; + vuint32_t pintsel[8]; + + vuint32_t usbclkctrl; /* 0x198 */ + vuint32_t usbclkst; + + uint32_t r1a0[6*4]; /* 0x1a0 */ + + uint32_t r200; /* 0x200 */ + vuint32_t starterp0; + uint32_t r208[2]; + + uint32_t r210; /* 0x210 */ + vuint32_t starterp1; + uint32_t r218[2]; + + uint32_t r220[4]; /* 0x220 */ + + vuint32_t pdsleepcfg; /* 0x230 */ + vuint32_t pdawakecfg; + vuint32_t pdruncfg; + uint32_t r23c; + + uint32_t r240[12 * 4]; /* 0x240 */ + + uint32_t r300[15 * 4]; /* 0x300 */ + + uint32_t r3f0; /* 0x3f0 */ + vuint32_t device_id; +}; + +extern struct lpc_scb lpc_scb; + +#define LPC_SCB_SYSMEMREMAP_MAP 0 +# define LPC_SCB_SYSMEMREMAP_MAP_BOOT_LOADER 0 +# define LPC_SCB_SYSMEMREMAP_MAP_RAM 1 +# define LPC_SCB_SYSMEMREMAP_MAP_FLASH 2 + +#define LPC_SCB_PRESETCTRL_SSP0_RST_N 0 +#define LPC_SCB_PRESETCTRL_I2C_RST_N 1 +#define LPC_SCB_PRESETCTRL_SSP1_RST_N 2 + +#define LPC_SCB_SYSPLLCTRL_MSEL 0 +#define LPC_SCB_SYSPLLCTRL_PSEL 5 +#define LPC_SCB_SYSPLLCTRL_PSEL_1 0 +#define LPC_SCB_SYSPLLCTRL_PSEL_2 1 +#define LPC_SCB_SYSPLLCTRL_PSEL_4 2 +#define LPC_SCB_SYSPLLCTRL_PSEL_8 3 +#define LPC_SCB_SYSPLLCTRL_PSEL_MASK 3 + +#define LPC_SCB_SYSPLLSTAT_LOCK 0 + +#define LPC_SCB_USBPLLCTRL_MSEL 0 +#define LPC_SCB_USBPLLCTRL_PSEL 5 +#define LPC_SCB_USBPLLCTRL_PSEL_1 0 +#define LPC_SCB_USBPLLCTRL_PSEL_2 1 +#define LPC_SCB_USBPLLCTRL_PSEL_4 2 +#define LPC_SCB_USBPLLCTRL_PSEL_8 3 +#define LPC_SCB_USBPLLCTRL_PSEL_MASK 3 + +#define LPC_SCB_USBPLLSTAT_LOCK 0 + +#define LPC_SCB_SYSOSCCTRL_BYPASS 0 +#define LPC_SCB_SYSOSCCTRL_FREQRANGE 1 +#define LPC_SCB_SYSOSCCTRL_FREQRANGE_1_20 0 +#define LPC_SCB_SYSOSCCTRL_FREQRANGE_15_25 1 + +#define LPC_SCB_WDTOSCCTRL_DIVSEL 0 +#define LPC_SCB_WDTOSCCTRL_DIVSEL_MASK 0x1f +#define LPC_SCB_WDTOSCCTRL_FREQSEL 5 +#define LPC_SCB_WDTOSCCTRL_FREQSEL_0_6 1 +#define LPC_SCB_WDTOSCCTRL_FREQSEL_1_05 2 +#define LPC_SCB_WDTOSCCTRL_FREQSEL_1_4 3 +#define LPC_SCB_WDTOSCCTRL_FREQSEL_1_75 4 +#define LPC_SCB_WDTOSCCTRL_FREQSEL_2_1 5 +#define LPC_SCB_WDTOSCCTRL_FREQSEL_2_4 6 +#define LPC_SCB_WDTOSCCTRL_FREQSEL_2_7 7 +#define LPC_SCB_WDTOSCCTRL_FREQSEL_3_0 8 +#define LPC_SCB_WDTOSCCTRL_FREQSEL_3_25 9 +#define LPC_SCB_WDTOSCCTRL_FREQSEL_3_5 0x0a +#define LPC_SCB_WDTOSCCTRL_FREQSEL_3_75 0x0b +#define LPC_SCB_WDTOSCCTRL_FREQSEL_4_0 0x0c +#define LPC_SCB_WDTOSCCTRL_FREQSEL_4_2 0x0d +#define LPC_SCB_WDTOSCCTRL_FREQSEL_4_4 0x0e +#define LPC_SCB_WDTOSCCTRL_FREQSEL_4_6 0x0f +#define LPC_SCB_WDTOSCCTRL_FREQSEL_MASK 0x0f + +#define LPC_SCB_SYSRSTSTAT_POR 0 +#define LPC_SCB_SYSRSTSTAT_EXTRST 1 +#define LPC_SCB_SYSRSTSTAT_WDT 2 +#define LPC_SCB_SYSRSTSTAT_BOD 3 +#define LPC_SCB_SYSRSTSTAT_SYSRST 4 + +#define LPC_SCB_SYSPLLCLKSEL_SEL 0 +#define LPC_SCB_SYSPLLCLKSEL_SEL_IRC 0 +#define LPC_SCB_SYSPLLCLKSEL_SEL_SYSOSC 1 +#define LPC_SCB_SYSPLLCLKSEL_SEL_MASK 3 + +#define LPC_SCB_SYSPLLCLKUEN_ENA 0 + +#define LPC_SCB_USBPLLCLKSEL_SEL 0 +#define LPC_SCB_USBPLLCLKSEL_SEL_IRC 0 +#define LPC_SCB_USBPLLCLKSEL_SEL_SYSOSC 1 +#define LPC_SCB_USBPLLCLKSEL_SEL_MASK 3 + +#define LPC_SCB_USBPLLCLKUEN_ENA 0 + +#define LPC_SCB_MAINCLKSEL_SEL 0 +#define LPC_SCB_MAINCLKSEL_SEL_IRC 0 +#define LPC_SCB_MAINCLKSEL_SEL_PLL_INPUT 1 +#define LPC_SCB_MAINCLKSEL_SEL_WATCHDOG 2 +#define LPC_SCB_MAINCLKSEL_SEL_PLL_OUTPUT 3 +#define LPC_SCB_MAINCLKSEL_SEL_MASK 3 + +#define LPC_SCB_MAINCLKUEN_ENA 0 + +#define LPC_SCB_SYSAHBCLKDIV_DIV 0 + +#define LPC_SCB_SYSAHBCLKCTRL_SYS 0 +#define LPC_SCB_SYSAHBCLKCTRL_ROM 1 +#define LPC_SCB_SYSAHBCLKCTRL_RAM0 2 +#define LPC_SCB_SYSAHBCLKCTRL_FLASHREG 3 +#define LPC_SCB_SYSAHBCLKCTRL_FLASHARRAY 4 +#define LPC_SCB_SYSAHBCLKCTRL_I2C 5 +#define LPC_SCB_SYSAHBCLKCTRL_GPIO 6 +#define LPC_SCB_SYSAHBCLKCTRL_CT16B0 7 +#define LPC_SCB_SYSAHBCLKCTRL_CT16B1 8 +#define LPC_SCB_SYSAHBCLKCTRL_CT32B0 9 +#define LPC_SCB_SYSAHBCLKCTRL_CT32B1 10 +#define LPC_SCB_SYSAHBCLKCTRL_SSP0 11 +#define LPC_SCB_SYSAHBCLKCTRL_USART 12 +#define LPC_SCB_SYSAHBCLKCTRL_ADC 13 +#define LPC_SCB_SYSAHBCLKCTRL_USB 14 +#define LPC_SCB_SYSAHBCLKCTRL_WWDT 15 +#define LPC_SCB_SYSAHBCLKCTRL_IOCON 16 +#define LPC_SCB_SYSAHBCLKCTRL_SSP1 18 +#define LPC_SCB_SYSAHBCLKCTRL_PINT 19 +#define LPC_SCB_SYSAHBCLKCTRL_GROUP0INT 23 +#define LPC_SCB_SYSAHBCLKCTRL_GROUP1INT 24 +#define LPC_SCB_SYSAHBCLKCTRL_RAM1 26 +#define LPC_SCB_SYSAHBCLKCTRL_USBRAM 27 + +#define LPC_SCB_SSP0CLKDIV_ +#define LPC_SCB_UARTCLKDIV_ +#define LPC_SCB_SSP1CLKDIV_ + +#define LPC_SCB_USBCLKSEL_SEL 0 +#define LPC_SCB_USBCLKSEL_SEL_USB_PLL 0 +#define LPC_SCB_USBCLKSEL_SEL_MAIN_CLOCK 1 + +#define LPC_SCB_USBCLKUEN_ENA 0 +#define LPC_SCB_USBCLKDIV_DIV 0 + +#define LPC_SCB_CLKOUTSEL_SEL 0 +#define LPC_SCB_CLKOUTSEL_SEL_IRC 0 +#define LPC_SCB_CLKOUTSEL_SEL_SYSOSC 1 +#define LPC_SCB_CLKOUTSEL_SEL_LF 2 +#define LPC_SCB_CLKOUTSEL_SEL_MAIN_CLOCK 3 + +#define LPC_SCB_CLKOUTUEN_ENA 0 + +#define LPC_SCB_BOD_BODRSTLEV 0 +# define LPC_SCB_BOD_BODRSTLEV_1_46 0 +# define LPC_SCB_BOD_BODRSTLEV_2_06 1 +# define LPC_SCB_BOD_BODRSTLEV_2_35 2 +# define LPC_SCB_BOD_BODRSTLEV_2_63 3 +#define LPC_SCB_BOD_BODINTVAL 2 +# define LPC_SCB_BOD_BODINTVAL_RESERVED 0 +# define LPC_SCB_BOD_BODINTVAL_2_22 1 +# define LPC_SCB_BOD_BODINTVAL_2_52 2 +# define LPC_SCB_BOD_BODINTVAL_2_80 3 +#define LPC_SCB_BOD_BODRSTENA 4 + +#define LPC_SCB_PDRUNCFG_IRCOUT_PD 0 +#define LPC_SCB_PDRUNCFG_IRC_PD 1 +#define LPC_SCB_PDRUNCFG_FLASH_PD 2 +#define LPC_SCB_PDRUNCFG_BOD_PD 3 +#define LPC_SCB_PDRUNCFG_ADC_PD 4 +#define LPC_SCB_PDRUNCFG_SYSOSC_PD 5 +#define LPC_SCB_PDRUNCFG_WDTOSC_PD 6 +#define LPC_SCB_PDRUNCFG_SYSPLL_PD 7 +#define LPC_SCB_PDRUNCFG_USBPLL_PD 8 +#define LPC_SCB_PDRUNCFG_USBPAD_PD 10 + +struct lpc_flash { + uint32_t r0[4]; /* 0x0 */ + + vuint32_t flashcfg; /* 0x10 */ +}; + +extern struct lpc_flash lpc_flash; + +struct lpc_gpio_pin { + vuint32_t isel; /* 0x00 */ + vuint32_t ienr; + vuint32_t sienr; + vuint32_t cienr; + + vuint32_t ienf; /* 0x10 */ + vuint32_t sienf; + vuint32_t cienf; + vuint32_t rise; + + vuint32_t fall; /* 0x20 */ + vuint32_t ist; +}; + +extern struct lpc_gpio_pin lpc_gpio_pin; + +struct lpc_gpio_group0 { +}; + +extern struct lpc_gpio_group0 lpc_gpio_group0; + +struct lpc_gpio_group1 { +}; + +extern struct lpc_gpio_group1 lpc_gpio_group1; + +struct lpc_gpio { + vuint8_t byte[0x40]; /* 0x0000 */ + + uint8_t r0030[0x1000 - 0x40]; + + vuint32_t word[0x40]; /* 0x1000 */ + + uint8_t r1100[0x2000 - 0x1100]; + + vuint32_t dir[2]; /* 0x2000 */ + + uint8_t r2008[0x2080 - 0x2008]; + + vuint32_t mask[2]; /* 0x2080 */ + + uint8_t r2088[0x2100 - 0x2088]; + + vuint32_t pin[2]; /* 0x2100 */ + + uint8_t r2108[0x2200 - 0x2108]; + + vuint32_t set[2]; /* 0x2200 */ + + uint8_t r2208[0x2280 - 0x2208]; + + vuint32_t clr[2]; /* 0x2280 */ + + uint8_t r2288[0x2300 - 0x2288]; + + vuint32_t not[2]; /* 0x2300 */ +}; + +extern struct lpc_gpio lpc_gpio; + +struct lpc_systick { + uint8_t r0000[0x10]; /* 0x0000 */ + + vuint32_t csr; /* 0x0010 */ + vuint32_t rvr; + vuint32_t cvr; + vuint32_t calib; +}; + +extern struct lpc_systick lpc_systick; + +#define LPC_SYSTICK_CSR_ENABLE 0 +#define LPC_SYSTICK_CSR_TICKINT 1 +#define LPC_SYSTICK_CSR_CLKSOURCE 2 +#define LPC_SYSTICK_CSR_CLKSOURCE_CPU_OVER_2 0 +#define LPC_SYSTICK_CSR_CLKSOURCE_CPU 1 +#define LPC_SYSTICK_CSR_COUNTFLAG 16 + +struct lpc_usart { + vuint32_t rbr_thr; /* 0x0000 */ + vuint32_t ier; + vuint32_t iir_fcr; + vuint32_t lcr; + + vuint32_t mcr; /* 0x0010 */ + vuint32_t lsr; + vuint32_t msr; + vuint32_t scr; + + vuint32_t acr; /* 0x0020 */ + vuint32_t icr; + vuint32_t fdr; + vuint32_t osr; + + vuint32_t ter; /* 0x0030 */ + uint32_t r34[3]; + + vuint32_t hden; /* 0x0040 */ + uint32_t r44; + vuint32_t scictrl; + vuint32_t rs485ctrl; + + vuint32_t rs485addrmatch; /* 0x0050 */ + vuint32_t rs485dly; + vuint32_t syncctrl; +}; + +extern struct lpc_usart lpc_usart; + +#define LPC_USART_IER_RBRINTEN 0 +#define LPC_USART_IER_THREINTEN 1 +#define LPC_USART_IER_RSLINTEN 2 +#define LPC_USART_IER_MSINTEN 3 +#define LPC_USART_IER_ABEOINTEN 8 +#define LPC_USART_IER_ABTOINTEN 9 + +#define LPC_USART_IIR_INTSTATUS 0 +#define LPC_USART_IIR_INTID 1 +#define LPC_USART_IIR_INTID_RLS 3 +#define LPC_USART_IIR_INTID_RDA 2 +#define LPC_USART_IIR_INTID_CTI 6 +#define LPC_USART_IIR_INTID_THRE 1 +#define LPC_USART_IIR_INTID_MS 0 +#define LPC_USART_IIR_INTID_MASK 7 +#define LPC_USART_IIR_FIFOEN 6 +#define LPC_USART_IIR_ABEOINT 8 +#define LPC_USART_IIR_ABTOINT 9 + +#define LPC_USART_FCR_FIFOEN 0 +#define LPC_USART_FCR_RXFIFORES 1 +#define LPC_USART_FCR_TXFIFORES 2 +#define LPC_USART_FCR_RXTL 6 +#define LPC_USART_FCR_RXTL_1 0 +#define LPC_USART_FCR_RXTL_4 1 +#define LPC_USART_FCR_RXTL_8 2 +#define LPC_USART_FCR_RXTL_14 3 + +#define LPC_USART_LCR_WLS 0 +#define LPC_USART_LCR_WLS_5 0 +#define LPC_USART_LCR_WLS_6 1 +#define LPC_USART_LCR_WLS_7 2 +#define LPC_USART_LCR_WLS_8 3 +#define LPC_USART_LCR_WLS_MASK 3 +#define LPC_USART_LCR_SBS 2 +#define LPC_USART_LCR_SBS_1 0 +#define LPC_USART_LCR_SBS_2 1 +#define LPC_USART_LCR_SBS_MASK 1 +#define LPC_USART_LCR_PE 3 +#define LPC_USART_LCR_PS 4 +#define LPC_USART_LCR_PS_ODD 0 +#define LPC_USART_LCR_PS_EVEN 1 +#define LPC_USART_LCR_PS_ONE 2 +#define LPC_USART_LCR_PS_ZERO 3 +#define LPC_USART_LCR_PS_MASK 3 +#define LPC_USART_LCR_BC 6 +#define LPC_USART_LCR_DLAB 7 + +#define LPC_USART_MCR_DTRCTRL 0 +#define LPC_USART_MCR_RTSCTRL 1 +#define LPC_USART_MCR_LMS 4 +#define LPC_USART_MCR_RTSEN 6 +#define LPC_USART_MCR_CTSEN 7 + +#define LPC_USART_LSR_RDR 0 +#define LPC_USART_LSR_OE 1 +#define LPC_USART_LSR_PE 2 +#define LPC_USART_LSR_FE 3 +#define LPC_USART_LSR_BI 4 +#define LPC_USART_LSR_THRE 5 +#define LPC_USART_LSR_TEMT 6 +#define LPC_USART_LSR_RXFE 7 +#define LPC_USART_LSR_TXERR 8 + +#define LPC_USART_MSR_DCTS 0 +#define LPC_USART_MSR_DDSR 1 +#define LPC_USART_MSR_TERI 2 +#define LPC_USART_MSR_DDCD 3 +#define LPC_USART_MSR_CTS 4 +#define LPC_USART_MSR_DSR 5 +#define LPC_USART_MSR_RI 6 +#define LPC_USART_MSR_DCD 7 + +#define LPC_USART_ACR_START 0 +#define LPC_USART_ACR_MODE 1 +#define LPC_USART_ACR_AUTORESTART 2 +#define LPC_USART_ACR_ABEOINTCLR 8 +#define LPC_USART_ACR_ABTOINTCLR 9 + +#define LPC_USART_FDR_DIVADDVAL 0 +#define LPC_USART_FDR_MULVAL 4 + +#define LPC_USART_OSR_OSFRAC 1 +#define LPC_USART_OSR_OSINT 4 +#define LPC_USART_OSR_FDINT 8 + +#define LPC_USART_TER_TXEN 7 + +#define LPC_USART_HDEN_HDEN 0 + +struct lpc_usb { + vuint32_t devcmdstat; + vuint32_t info; + vuint32_t epliststart; + vuint32_t databufstart; + vuint32_t lpm; + vuint32_t epskip; + vuint32_t epinuse; + vuint32_t epbufcfg; + vuint32_t intstat; + vuint32_t inten; + vuint32_t intsetstat; + vuint32_t introuting; + uint32_t r30; + vuint32_t eptoggle; +} lpc_usb; + +extern struct lpc_usb lpc_usb; + +#define LPC_USB_DEVCMDSTAT_DEV_ADDR 0 +#define LPC_USB_DEVCMDSTAT_DEV_ADDR_MASK 0x7f +#define LPC_USB_DEVCMDSTAT_DEV_EN 7 +#define LPC_USB_DEVCMDSTAT_SETUP 8 +#define LPC_USB_DEVCMDSTAT_PLL_ON 9 +#define LPC_USB_DEVCMDSTAT_LPM_SUP 11 +#define LPC_USB_DEVCMDSTAT_INTONNAK_AO 12 +#define LPC_USB_DEVCMDSTAT_INTONNAK_AI 13 +#define LPC_USB_DEVCMDSTAT_INTONNAK_CO 14 +#define LPC_USB_DEVCMDSTAT_INTONNAK_CI 15 +#define LPC_USB_DEVCMDSTAT_DCON 16 +#define LPC_USB_DEVCMDSTAT_DSUS 17 +#define LPC_USB_DEVCMDSTAT_LPM_SUS 19 +#define LPC_USB_DEVCMDSTAT_LPM_REWP 20 +#define LPC_USB_DEVCMDSTAT_DCON_C 24 +#define LPC_USB_DEVCMDSTAT_DSUS_C 25 +#define LPC_USB_DEVCMDSTAT_DRES_C 26 +#define LPC_USB_DEVCMDSTAT_VBUSDEBOUNCED 28 + +#define LPC_USB_INFO_FRAME_NR 0 +#define LPC_USB_INFO_FRAME_NR_MASK 0x3ff +#define LPC_USB_INFO_ERR_CODE 11 +#define LPC_USB_INFO_ERR_CODE_NO_ERROR 0 +#define LPC_USB_INFO_ERR_CODE_PID_ENCODING_ERROR 1 +#define LPC_USB_INFO_ERR_CODE_PID_UNKNOWN 2 +#define LPC_USB_INFO_ERR_CODE_PACKET_UNEXPECTED 3 +#define LPC_USB_INFO_ERR_CODE_TOKEN_CRC_ERROR 4 +#define LPC_USB_INFO_ERR_CODE_DATA_CRC_ERROR 5 +#define LPC_USB_INFO_ERR_CODE_TIME_OUT 6 +#define LPC_USB_INFO_ERR_CODE_BABBLE 7 +#define LPC_USB_INFO_ERR_CODE_TRUNCATED_EOP 8 +#define LPC_USB_INFO_ERR_CODE_SENT_RECEIVED_NAK 9 +#define LPC_USB_INFO_ERR_CODE_SENT_STALL 0xa +#define LPC_USB_INFO_ERR_CODE_OVERRUN 0xb +#define LPC_USB_INFO_ERR_CODE_SENT_EMPTY_PACKET 0xc +#define LPC_USB_INFO_ERR_CODE_BITSTUFF_ERROR 0xd +#define LPC_USB_INFO_ERR_CODE_SYNC_ERROR 0xe +#define LPC_USB_INFO_ERR_CODE_WRONG_DATA_TOGGLE 0xf +#define LPC_USB_INFO_ERR_CODE_MASK 0xf + +#define LPC_USB_EPLISTSTART_EP_LIST 0 + +#define LPC_USB_DATABUFSTART_DA_BUF 0 + +#define LPC_USB_LPM_HIRD_HW 0 +#define LPC_USB_LPM_HIRD_HW_MASK 0xf +#define LPC_USB_LPM_HIRD_SW 4 +#define LPC_USB_LPM_HIRD_SW_MASK 0xf +#define LPC_USB_LPM_DATA_PENDING 8 + +#define LPC_USB_EPSKIP_SKIP 0 + +#define LPC_USB_EPINUSE_BUF(ep) (ep) + +#define LPC_USB_EPBUFCFG_BUF_SB(ep) (ep) + +#define LPC_USB_INT_EPOUT(ep) ((ep) << 1) +#define LPC_USB_INT_EPIN(ep) (((ep) << 1) + 1) + +#define LPC_USB_INT_FRAME 30 +#define LPC_USB_INT_DEV 31 + +#define LPC_USB_INTIN_EP_INT_EN(ep) (ep) +#define LPC_USB_INTIN_FRAME_INT_EN 30 +#define LPC_USB_INTIN_DEV_INT_EN 31 + +#define LPC_USB_INTSETSTAT_EP_SET_INT(ep) (ep) +#define LPC_USB_INTSETSTAT_FRAME_SET_INT 30 +#define LPC_USB_INTSETSTAT_DEV_SET_INT 31 + +#define LPC_USB_INTROUTING_ROUTE_INT(ep) (ep) +#define LPC_USB_INTROUTING_INT30 30 +#define LPC_USB_INTROUTING_INT31 31 + +#define LPC_USB_EPTOGGLE_TOGGLE(ep) (ep) + +struct lpc_usb_epn { + vuint32_t out[2]; + vuint32_t in[2]; +}; + +struct lpc_usb_endpoint { + vuint32_t ep0_out; + vuint32_t setup; + vuint32_t ep0_in; + vuint32_t reserved_0c; + struct lpc_usb_epn epn[4]; +}; + +/* Assigned in registers.ld to point at the base + * of USB ram + */ + +extern uint8_t lpc_usb_sram[]; + +#define LPC_USB_EP_ACTIVE 31 +#define LPC_USB_EP_DISABLED 30 +#define LPC_USB_EP_STALL 29 +#define LPC_USB_EP_TOGGLE_RESET 28 +#define LPC_USB_EP_RATE_FEEDBACK 27 +#define LPC_USB_EP_ENDPOINT_ISO 26 +#define LPC_USB_EP_NBYTES 16 +#define LPC_USB_EP_NBYTES_MASK 0x3ff +#define LPC_USB_EP_OFFSET 0 + +#define LPC_ISR_PIN_INT0_POS 0 +#define LPC_ISR_PIN_INT1_POS 1 +#define LPC_ISR_PIN_INT2_POS 2 +#define LPC_ISR_PIN_INT3_POS 3 +#define LPC_ISR_PIN_INT4_POS 4 +#define LPC_ISR_PIN_INT5_POS 5 +#define LPC_ISR_PIN_INT6_POS 6 +#define LPC_ISR_PIN_INT7_POS 7 +#define LPC_ISR_GINT0_POS 8 +#define LPC_ISR_GINT1_POS 9 +#define LPC_ISR_SSP1_POS 14 +#define LPC_ISR_I2C_POS 15 +#define LPC_ISR_CT16B0_POS 16 +#define LPC_ISR_CT16B1_POS 17 +#define LPC_ISR_CT32B0_POS 18 +#define LPC_ISR_CT32B1_POS 19 +#define LPC_ISR_SSP0_POS 20 +#define LPC_ISR_USART_POS 21 +#define LPC_ISR_USB_IRQ_POS 22 +#define LPC_ISR_USB_FIQ_POS 23 +#define LPC_ISR_ADC_POS 24 +#define LPC_ISR_WWDT_POS 25 +#define LPC_ISR_BOD_POS 26 +#define LPC_ISR_FLASH_POS 27 +#define LPC_ISR_USB_WAKEUP_POS 30 + +struct lpc_nvic { + vuint32_t iser; /* 0x000 0xe000e100 Set Enable Register */ + + uint8_t _unused020[0x080 - 0x004]; + + vuint32_t icer; /* 0x080 0xe000e180 Clear Enable Register */ + + uint8_t _unused0a0[0x100 - 0x084]; + + vuint32_t ispr; /* 0x100 0xe000e200 Set Pending Register */ + + uint8_t _unused120[0x180 - 0x104]; + + vuint32_t icpr; /* 0x180 0xe000e280 Clear Pending Register */ + + uint8_t _unused1a0[0x300 - 0x184]; + + vuint32_t ipr[8]; /* 0x300 0xe000e400 Priority Register */ +}; + +extern struct lpc_nvic lpc_nvic; + +static inline void +lpc_nvic_set_enable(int irq) { + lpc_nvic.iser = (1 << irq); +} + +static inline void +lpc_nvic_clear_enable(int irq) { + lpc_nvic.icer = (1 << irq); +} + +static inline int +lpc_nvic_enabled(int irq) { + return (lpc_nvic.iser >> irq) & 1; +} + + +static inline void +lpc_nvic_set_pending(int irq) { + lpc_nvic.ispr = (1 << irq); +} + +static inline void +lpc_nvic_clear_pending(int irq) { + lpc_nvic.icpr = (1 << irq); +} + +static inline int +lpc_nvic_pending(int irq) { + return (lpc_nvic.ispr >> irq) & 1; +} + +#define IRQ_PRIO_REG(irq) ((irq) >> 2) +#define IRQ_PRIO_BIT(irq) (((irq) & 3) << 3) +#define IRQ_PRIO_MASK(irq) (0xff << IRQ_PRIO_BIT(irq)) + +static inline void +lpc_nvic_set_priority(int irq, uint8_t prio) { + int n = IRQ_PRIO_REG(irq); + uint32_t v; + + v = lpc_nvic.ipr[n]; + v &= ~IRQ_PRIO_MASK(irq); + v |= (prio) << IRQ_PRIO_BIT(irq); + lpc_nvic.ipr[n] = v; +} + +static inline uint8_t +lpc_nvic_get_priority(int irq) { + return (lpc_nvic.ipr[IRQ_PRIO_REG(irq)] >> IRQ_PRIO_BIT(irq)) & IRQ_PRIO_MASK(0); +} + +struct arm_scb { + vuint32_t cpuid; + vuint32_t icsr; + uint32_t reserved08; + vuint32_t aircr; + + vuint32_t scr; + vuint32_t ccr; + uint32_t reserved18; + vuint32_t shpr2; + + vuint32_t shpr3; +}; + +extern struct arm_scb arm_scb; + +struct lpc_ssp { + vuint32_t cr0; /* 0x00 */ + vuint32_t cr1; + vuint32_t dr; + vuint32_t sr; + + vuint32_t cpsr; /* 0x10 */ + vuint32_t imsc; + vuint32_t ris; + vuint32_t mis; + + vuint32_t icr; /* 0x20 */ +}; + +extern struct lpc_ssp lpc_ssp0, lpc_ssp1; + +#define LPC_NUM_SPI 2 + +#define LPC_SSP_FIFOSIZE 8 + +#define LPC_SSP_CR0_DSS 0 +#define LPC_SSP_CR0_DSS_4 0x3 +#define LPC_SSP_CR0_DSS_5 0x4 +#define LPC_SSP_CR0_DSS_6 0x5 +#define LPC_SSP_CR0_DSS_7 0x6 +#define LPC_SSP_CR0_DSS_8 0x7 +#define LPC_SSP_CR0_DSS_9 0x8 +#define LPC_SSP_CR0_DSS_10 0x9 +#define LPC_SSP_CR0_DSS_11 0xa +#define LPC_SSP_CR0_DSS_12 0xb +#define LPC_SSP_CR0_DSS_13 0xc +#define LPC_SSP_CR0_DSS_14 0xd +#define LPC_SSP_CR0_DSS_15 0xe +#define LPC_SSP_CR0_DSS_16 0xf +#define LPC_SSP_CR0_FRF 4 +#define LPC_SSP_CR0_FRF_SPI 0 +#define LPC_SSP_CR0_FRF_TI 1 +#define LPC_SSP_CR0_FRF_MICROWIRE 2 +#define LPC_SSP_CR0_CPOL 6 +#define LPC_SSP_CR0_CPOL_LOW 0 +#define LPC_SSP_CR0_CPOL_HIGH 1 +#define LPC_SSP_CR0_CPHA 7 +#define LPC_SSP_CR0_CPHA_FIRST 0 +#define LPC_SSP_CR0_CPHA_SECOND 1 +#define LPC_SSP_CR0_SCR 8 + +#define LPC_SSP_CR1_LBM 0 +#define LPC_SSP_CR1_SSE 1 +#define LPC_SSP_CR1_MS 2 +#define LPC_SSP_CR1_MS_MASTER 0 +#define LPC_SSP_CR1_MS_SLAVE 1 +#define LPC_SSP_CR1_SOD 3 + +#define LPC_SSP_SR_TFE 0 +#define LPC_SSP_SR_TNF 1 +#define LPC_SSP_SR_RNE 2 +#define LPC_SSP_SR_RFF 3 +#define LPC_SSP_SR_BSY 4 + +#define LPC_SSP_IMSC_RORIM 0 +#define LPC_SSP_IMSC_RTIM 1 +#define LPC_SSP_IMSC_RXIM 2 +#define LPC_SSP_IMSC_TXIM 3 + +#define LPC_SSP_RIS_RORRIS 0 +#define LPC_SSP_RIS_RTRIS 1 +#define LPC_SSP_RIS_RXRIS 2 +#define LPC_SSP_RIS_TXRIS 3 + +#define LPC_SSP_MIS_RORMIS 0 +#define LPC_SSP_MIS_RTMIS 1 +#define LPC_SSP_MIS_RXMIS 2 +#define LPC_SSP_MIS_TXMIS 3 + +#define LPC_SSP_ICR_RORIC 0 +#define LPC_SSP_ICR_RTIC 1 + +struct lpc_adc { + vuint32_t cr; /* 0x00 */ + vuint32_t gdr; + uint32_t r08; + vuint32_t inten; + + vuint32_t dr[8]; /* 0x10 */ + + vuint32_t stat; /* 0x30 */ +}; + +extern struct lpc_adc lpc_adc; + +#define LPC_ADC_CR_SEL 0 +#define LPC_ADC_CR_CLKDIV 8 +#define LPC_ADC_CR_BURST 16 +#define LPC_ADC_CR_CLKS 17 +#define LPC_ADC_CR_CLKS_11 0 +#define LPC_ADC_CR_CLKS_10 1 +#define LPC_ADC_CR_CLKS_9 2 +#define LPC_ADC_CR_CLKS_8 3 +#define LPC_ADC_CR_CLKS_7 4 +#define LPC_ADC_CR_CLKS_6 5 +#define LPC_ADC_CR_CLKS_5 6 +#define LPC_ADC_CR_CLKS_4 7 +#define LPC_ADC_CR_START 24 +#define LPC_ADC_CR_START_NONE 0 +#define LPC_ADC_CR_START_NOW 1 + +#define LPC_ADC_GDR_CHN 24 +#define LPC_ADC_GDR_OVERRUN 30 +#define LPC_ADC_GDR_DONE 31 + +#define LPC_ADC_INTEN_ADINTEN 0 +#define LPC_ADC_INTEN_ADGINTEN 8 + +#define LPC_ADC_STAT_DONE 0 +#define LPC_ADC_STAT_OVERRUN 8 +#define LPC_ADC_STAT_ADINT 16 + +struct lpc_ct32b { + vuint32_t ir; /* 0x00 */ + vuint32_t tcr; + vuint32_t tc; + vuint32_t pr; + + vuint32_t pc; /* 0x10 */ + vuint32_t mcr; + vuint32_t mr[4]; /* 0x18 */ + vuint32_t ccr; /* 0x28 */ + vuint32_t cr0; + + vuint32_t cr1_0; /* 0x30 (only for ct32b0 */ + vuint32_t cr1_1; /* 0x34 (only for ct32b1 */ + uint32_t r38; + vuint32_t emr; + + uint32_t r40[12]; + + vuint32_t ctcr; /* 0x70 */ + vuint32_t pwmc; +}; + +extern struct lpc_ct32b lpc_ct32b0, lpc_ct32b1; + +#define LPC_CT32B_TCR_CEN 0 +#define LPC_CT32B_TCR_CRST 1 + +#define LPC_CT32B_MCR_MR0R 1 + +#define LPC_CT32B_PWMC_PWMEN0 0 +#define LPC_CT32B_PWMC_PWMEN1 1 +#define LPC_CT32B_PWMC_PWMEN2 2 +#define LPC_CT32B_PWMC_PWMEN3 3 + +#define LPC_CT32B_EMR_EMC0 4 +#define LPC_CT32B_EMR_EMC1 6 +#define LPC_CT32B_EMR_EMC2 8 +#define LPC_CT32B_EMR_EMC3 10 + +#define LPC_CT32B_EMR_EMC_NOTHING 0 +#define LPC_CT32B_EMR_EMC_CLEAR 1 +#define LPC_CT32B_EMR_EMC_SET 2 +#define LPC_CT32B_EMR_EMC_TOGGLE 3 + +#endif /* _LPC_H_ */ diff --git a/src/lpc/registers.ld b/src/lpc/registers.ld new file mode 100644 index 00000000..a523c39c --- /dev/null +++ b/src/lpc/registers.ld @@ -0,0 +1,19 @@ +lpc_usb_sram = 0x20004000; +lpc_usb_endpoint = 0x20004700; +lpc_usart = 0x40008000; +lpc_ct32b0 = 0x40014000; +lpc_ct32b1 = 0x40018000; +lpc_adc = 0x4001c000; +lpc_flash = 0x4003c000; +lpc_ssp0 = 0x40040000; +lpc_ioconf = 0x40044000; +lpc_scb = 0x40048000; +lpc_gpio_pin = 0x4004c000; +lpc_ssp1 = 0x40058000; +lpc_gpio_group0 = 0x4005c000; +lpc_gpio_group1 = 0x40060000; +lpc_usb = 0x40080000; +lpc_gpio = 0x50000000; +lpc_systick = 0xe000e000; +lpc_nvic = 0xe000e100; +arm_scb = 0xe000ed00; diff --git a/src/lpcxpresso/.gitignore b/src/lpcxpresso/.gitignore new file mode 100644 index 00000000..892c3acc --- /dev/null +++ b/src/lpcxpresso/.gitignore @@ -0,0 +1,3 @@ +ao_product.h +ao_serial_lpc.h +*.elf diff --git a/src/lpcxpresso/Makefile b/src/lpcxpresso/Makefile new file mode 100644 index 00000000..374c052f --- /dev/null +++ b/src/lpcxpresso/Makefile @@ -0,0 +1,66 @@ +# +# AltOS build +# +# + +include ../lpc/Makefile.defs + +INC = \ + ao.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_pins.h \ + ao_product.h \ + lpc.h + +# +# Common AltOS sources +# +ALTOS_SRC = \ + ao_interrupt.c \ + ao_romconfig.c \ + ao_product.c \ + ao_panic.c \ + ao_led_lpc.c \ + ao_task.c \ + ao_cmd.c \ + ao_timer_lpc.c \ + ao_serial_lpc.c \ + ao_usb_lpc.c \ + ao_stdio.c + +PRODUCT=LpcDemo-v0.0 +PRODUCT_DEF=-DLPC_DEMO +IDPRODUCT=0x000a + +CFLAGS = $(PRODUCT_DEF) $(LPC_CFLAGS) -g -Os + +PROG=lpc-demo.elf + +SRC=$(ALTOS_SRC) ao_demo.c +OBJ=$(SRC:.c=.o) + +all: $(PROG) + +LDFLAGS=-L../lpc -Wl,-Taltos.ld + +$(PROG): Makefile $(OBJ) + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) + +ao_product.h: ao-make-product.5c ../Version + $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ + +$(OBJ): $(INC) + +load: $(PROG) + lpc-load $(PROG) + +distclean: clean + +clean: + rm -f *.o $(PROG) + rm -f ao_product.h + +install: + +uninstall: diff --git a/src/lpcxpresso/ao_demo.c b/src/lpcxpresso/ao_demo.c new file mode 100644 index 00000000..0c931611 --- /dev/null +++ b/src/lpcxpresso/ao_demo.c @@ -0,0 +1,47 @@ +/* + * Copyright © 2011 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_usb.h> + +int +main(void) +{ + int i; + ao_led_init(LEDS_AVAILABLE); + ao_led_on(AO_LED_RED); + ao_clock_init(); + ao_timer_init(); + + ao_serial_init(); + ao_usb_init(); + ao_cmd_init(); + ao_task_init(); + + ao_start_scheduler(); + + for (;;) { + ao_led_off(AO_LED_RED); + for (;;) + if (ao_tick_count & 1) + break; + ao_led_on(AO_LED_RED); + for (;;) + if (!(ao_tick_count & 1)) + break; + } +} diff --git a/src/lpcxpresso/ao_pins.h b/src/lpcxpresso/ao_pins.h new file mode 100644 index 00000000..c0074ce2 --- /dev/null +++ b/src/lpcxpresso/ao_pins.h @@ -0,0 +1,51 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#define HAS_BEEP 0 +#define HAS_LED 1 + +/* Crystal on the board */ +#define AO_LPC_CLKIN 12000000 + +/* Main clock frequency. 48MHz for USB so we don't use the USB PLL */ +#define AO_LPC_CLKOUT 48000000 + +/* System clock frequency */ +#define AO_LPC_SYSCLK 24000000 + +#define LED_PORT 0 +#define LED_PIN_RED 7 + +#define AO_LED_RED (1 << LED_PIN_RED) + +#define LEDS_AVAILABLE AO_LED_RED + +#define HAS_USB 1 + +#define HAS_USB_CONNECT 1 +#define HAS_USB_VBUS 1 + +#define PACKET_HAS_SLAVE 0 + +/* USART */ + +#define HAS_SERIAL 1 +#define USE_SERIAL_0_STDIN 1 +#define SERIAL_0_18_19 1 +#define SERIAL_0_14_15 0 +#define SERIAL_0_17_18 0 +#define SERIAL_0_26_27 0 diff --git a/src/math/ef_acos.c b/src/math/ef_acos.c new file mode 100644 index 00000000..f73f97de --- /dev/null +++ b/src/math/ef_acos.c @@ -0,0 +1,84 @@ +/* ef_acos.c -- float version of e_acos.c. + * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com. + */ + +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +#include "fdlibm.h" + +#ifdef __STDC__ +static const float +#else +static float +#endif +one = 1.0000000000e+00, /* 0x3F800000 */ +pi = 3.1415925026e+00, /* 0x40490fda */ +pio2_hi = 1.5707962513e+00, /* 0x3fc90fda */ +pio2_lo = 7.5497894159e-08, /* 0x33a22168 */ +pS0 = 1.6666667163e-01, /* 0x3e2aaaab */ +pS1 = -3.2556581497e-01, /* 0xbea6b090 */ +pS2 = 2.0121252537e-01, /* 0x3e4e0aa8 */ +pS3 = -4.0055535734e-02, /* 0xbd241146 */ +pS4 = 7.9153501429e-04, /* 0x3a4f7f04 */ +pS5 = 3.4793309169e-05, /* 0x3811ef08 */ +qS1 = -2.4033949375e+00, /* 0xc019d139 */ +qS2 = 2.0209457874e+00, /* 0x4001572d */ +qS3 = -6.8828397989e-01, /* 0xbf303361 */ +qS4 = 7.7038154006e-02; /* 0x3d9dc62e */ + +#ifdef __STDC__ + float __ieee754_acosf(float x) +#else + float __ieee754_acosf(x) + float x; +#endif +{ + float z,p,q,r,w,s,c,df; + __int32_t hx,ix; + GET_FLOAT_WORD(hx,x); + ix = hx&0x7fffffff; + if(ix==0x3f800000) { /* |x|==1 */ + if(hx>0) return 0.0; /* acos(1) = 0 */ + else return pi+(float)2.0*pio2_lo; /* acos(-1)= pi */ + } else if(ix>0x3f800000) { /* |x| >= 1 */ + return (x-x)/(x-x); /* acos(|x|>1) is NaN */ + } + if(ix<0x3f000000) { /* |x| < 0.5 */ + if(ix<=0x23000000) return pio2_hi+pio2_lo;/*if|x|<2**-57*/ + z = x*x; + p = z*(pS0+z*(pS1+z*(pS2+z*(pS3+z*(pS4+z*pS5))))); + q = one+z*(qS1+z*(qS2+z*(qS3+z*qS4))); + r = p/q; + return pio2_hi - (x - (pio2_lo-x*r)); + } else if (hx<0) { /* x < -0.5 */ + z = (one+x)*(float)0.5; + p = z*(pS0+z*(pS1+z*(pS2+z*(pS3+z*(pS4+z*pS5))))); + q = one+z*(qS1+z*(qS2+z*(qS3+z*qS4))); + s = __ieee754_sqrtf(z); + r = p/q; + w = r*s-pio2_lo; + return pi - (float)2.0*(s+w); + } else { /* x > 0.5 */ + __int32_t idf; + z = (one-x)*(float)0.5; + s = __ieee754_sqrtf(z); + df = s; + GET_FLOAT_WORD(idf,df); + SET_FLOAT_WORD(df,idf&0xfffff000); + c = (z-df*df)/(s+df); + p = z*(pS0+z*(pS1+z*(pS2+z*(pS3+z*(pS4+z*pS5))))); + q = one+z*(qS1+z*(qS2+z*(qS3+z*qS4))); + r = p/q; + w = r*s+c; + return (float)2.0*(df+w); + } +} diff --git a/src/math/ef_rem_pio2.c b/src/math/ef_rem_pio2.c new file mode 100644 index 00000000..f1191d09 --- /dev/null +++ b/src/math/ef_rem_pio2.c @@ -0,0 +1,193 @@ +/* ef_rem_pio2.c -- float version of e_rem_pio2.c + * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com. + */ + +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + * + */ + +/* __ieee754_rem_pio2f(x,y) + * + * return the remainder of x rem pi/2 in y[0]+y[1] + * use __kernel_rem_pio2f() + */ + +#include "fdlibm.h" + +/* + * Table of constants for 2/pi, 396 Hex digits (476 decimal) of 2/pi + */ +#ifdef __STDC__ +static const __int32_t two_over_pi[] = { +#else +static __int32_t two_over_pi[] = { +#endif +0xA2, 0xF9, 0x83, 0x6E, 0x4E, 0x44, 0x15, 0x29, 0xFC, +0x27, 0x57, 0xD1, 0xF5, 0x34, 0xDD, 0xC0, 0xDB, 0x62, +0x95, 0x99, 0x3C, 0x43, 0x90, 0x41, 0xFE, 0x51, 0x63, +0xAB, 0xDE, 0xBB, 0xC5, 0x61, 0xB7, 0x24, 0x6E, 0x3A, +0x42, 0x4D, 0xD2, 0xE0, 0x06, 0x49, 0x2E, 0xEA, 0x09, +0xD1, 0x92, 0x1C, 0xFE, 0x1D, 0xEB, 0x1C, 0xB1, 0x29, +0xA7, 0x3E, 0xE8, 0x82, 0x35, 0xF5, 0x2E, 0xBB, 0x44, +0x84, 0xE9, 0x9C, 0x70, 0x26, 0xB4, 0x5F, 0x7E, 0x41, +0x39, 0x91, 0xD6, 0x39, 0x83, 0x53, 0x39, 0xF4, 0x9C, +0x84, 0x5F, 0x8B, 0xBD, 0xF9, 0x28, 0x3B, 0x1F, 0xF8, +0x97, 0xFF, 0xDE, 0x05, 0x98, 0x0F, 0xEF, 0x2F, 0x11, +0x8B, 0x5A, 0x0A, 0x6D, 0x1F, 0x6D, 0x36, 0x7E, 0xCF, +0x27, 0xCB, 0x09, 0xB7, 0x4F, 0x46, 0x3F, 0x66, 0x9E, +0x5F, 0xEA, 0x2D, 0x75, 0x27, 0xBA, 0xC7, 0xEB, 0xE5, +0xF1, 0x7B, 0x3D, 0x07, 0x39, 0xF7, 0x8A, 0x52, 0x92, +0xEA, 0x6B, 0xFB, 0x5F, 0xB1, 0x1F, 0x8D, 0x5D, 0x08, +0x56, 0x03, 0x30, 0x46, 0xFC, 0x7B, 0x6B, 0xAB, 0xF0, +0xCF, 0xBC, 0x20, 0x9A, 0xF4, 0x36, 0x1D, 0xA9, 0xE3, +0x91, 0x61, 0x5E, 0xE6, 0x1B, 0x08, 0x65, 0x99, 0x85, +0x5F, 0x14, 0xA0, 0x68, 0x40, 0x8D, 0xFF, 0xD8, 0x80, +0x4D, 0x73, 0x27, 0x31, 0x06, 0x06, 0x15, 0x56, 0xCA, +0x73, 0xA8, 0xC9, 0x60, 0xE2, 0x7B, 0xC0, 0x8C, 0x6B, +}; + +/* This array is like the one in e_rem_pio2.c, but the numbers are + single precision and the last 8 bits are forced to 0. */ +#ifdef __STDC__ +static const __int32_t npio2_hw[] = { +#else +static __int32_t npio2_hw[] = { +#endif +0x3fc90f00, 0x40490f00, 0x4096cb00, 0x40c90f00, 0x40fb5300, 0x4116cb00, +0x412fed00, 0x41490f00, 0x41623100, 0x417b5300, 0x418a3a00, 0x4196cb00, +0x41a35c00, 0x41afed00, 0x41bc7e00, 0x41c90f00, 0x41d5a000, 0x41e23100, +0x41eec200, 0x41fb5300, 0x4203f200, 0x420a3a00, 0x42108300, 0x4216cb00, +0x421d1400, 0x42235c00, 0x4229a500, 0x422fed00, 0x42363600, 0x423c7e00, +0x4242c700, 0x42490f00 +}; + +/* + * invpio2: 24 bits of 2/pi + * pio2_1: first 17 bit of pi/2 + * pio2_1t: pi/2 - pio2_1 + * pio2_2: second 17 bit of pi/2 + * pio2_2t: pi/2 - (pio2_1+pio2_2) + * pio2_3: third 17 bit of pi/2 + * pio2_3t: pi/2 - (pio2_1+pio2_2+pio2_3) + */ + +#ifdef __STDC__ +static const float +#else +static float +#endif +zero = 0.0000000000e+00, /* 0x00000000 */ +half = 5.0000000000e-01, /* 0x3f000000 */ +two8 = 2.5600000000e+02, /* 0x43800000 */ +invpio2 = 6.3661980629e-01, /* 0x3f22f984 */ +pio2_1 = 1.5707855225e+00, /* 0x3fc90f80 */ +pio2_1t = 1.0804334124e-05, /* 0x37354443 */ +pio2_2 = 1.0804273188e-05, /* 0x37354400 */ +pio2_2t = 6.0770999344e-11, /* 0x2e85a308 */ +pio2_3 = 6.0770943833e-11, /* 0x2e85a300 */ +pio2_3t = 6.1232342629e-17; /* 0x248d3132 */ + +#ifdef __STDC__ + __int32_t __ieee754_rem_pio2f(float x, float *y) +#else + __int32_t __ieee754_rem_pio2f(x,y) + float x,y[]; +#endif +{ + float z,w,t,r,fn; + float tx[3]; + __int32_t i,j,n,ix,hx; + int e0,nx; + + GET_FLOAT_WORD(hx,x); + ix = hx&0x7fffffff; + if(ix<=0x3f490fd8) /* |x| ~<= pi/4 , no need for reduction */ + {y[0] = x; y[1] = 0; return 0;} + if(ix<0x4016cbe4) { /* |x| < 3pi/4, special case with n=+-1 */ + if(hx>0) { + z = x - pio2_1; + if((ix&0xfffffff0)!=0x3fc90fd0) { /* 24+24 bit pi OK */ + y[0] = z - pio2_1t; + y[1] = (z-y[0])-pio2_1t; + } else { /* near pi/2, use 24+24+24 bit pi */ + z -= pio2_2; + y[0] = z - pio2_2t; + y[1] = (z-y[0])-pio2_2t; + } + return 1; + } else { /* negative x */ + z = x + pio2_1; + if((ix&0xfffffff0)!=0x3fc90fd0) { /* 24+24 bit pi OK */ + y[0] = z + pio2_1t; + y[1] = (z-y[0])+pio2_1t; + } else { /* near pi/2, use 24+24+24 bit pi */ + z += pio2_2; + y[0] = z + pio2_2t; + y[1] = (z-y[0])+pio2_2t; + } + return -1; + } + } + if(ix<=0x43490f80) { /* |x| ~<= 2^7*(pi/2), medium size */ + t = fabsf(x); + n = (__int32_t) (t*invpio2+half); + fn = (float)n; + r = t-fn*pio2_1; + w = fn*pio2_1t; /* 1st round good to 40 bit */ + if(n<32&&(ix&0xffffff00)!=npio2_hw[n-1]) { + y[0] = r-w; /* quick check no cancellation */ + } else { + __uint32_t high; + j = ix>>23; + y[0] = r-w; + GET_FLOAT_WORD(high,y[0]); + i = j-((high>>23)&0xff); + if(i>8) { /* 2nd iteration needed, good to 57 */ + t = r; + w = fn*pio2_2; + r = t-w; + w = fn*pio2_2t-((t-r)-w); + y[0] = r-w; + GET_FLOAT_WORD(high,y[0]); + i = j-((high>>23)&0xff); + if(i>25) { /* 3rd iteration need, 74 bits acc */ + t = r; /* will cover all possible cases */ + w = fn*pio2_3; + r = t-w; + w = fn*pio2_3t-((t-r)-w); + y[0] = r-w; + } + } + } + y[1] = (r-y[0])-w; + if(hx<0) {y[0] = -y[0]; y[1] = -y[1]; return -n;} + else return n; + } + /* + * all other (large) arguments + */ + if(!FLT_UWORD_IS_FINITE(ix)) { + y[0]=y[1]=x-x; return 0; + } + /* set z = scalbn(|x|,ilogb(x)-7) */ + e0 = (int)((ix>>23)-134); /* e0 = ilogb(z)-7; */ + SET_FLOAT_WORD(z, ix - ((__int32_t)e0<<23)); + for(i=0;i<2;i++) { + tx[i] = (float)((__int32_t)(z)); + z = (z-tx[i])*two8; + } + tx[2] = z; + nx = 3; + while(tx[nx-1]==zero) nx--; /* skip zero term */ + n = __kernel_rem_pio2f(tx,y,e0,nx,2,two_over_pi); + if(hx<0) {y[0] = -y[0]; y[1] = -y[1]; return -n;} + return n; +} diff --git a/src/math/ef_sqrt.c b/src/math/ef_sqrt.c new file mode 100644 index 00000000..80e7f360 --- /dev/null +++ b/src/math/ef_sqrt.c @@ -0,0 +1,89 @@ +/* ef_sqrtf.c -- float version of e_sqrt.c. + * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com. + */ + +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +#include "fdlibm.h" + +#ifdef __STDC__ +static const float one = 1.0, tiny=1.0e-30; +#else +static float one = 1.0, tiny=1.0e-30; +#endif + +#ifdef __STDC__ + float __ieee754_sqrtf(float x) +#else + float __ieee754_sqrtf(x) + float x; +#endif +{ + float z; + __uint32_t r,hx; + __int32_t ix,s,q,m,t,i; + + GET_FLOAT_WORD(ix,x); + hx = ix&0x7fffffff; + + /* take care of Inf and NaN */ + if(!FLT_UWORD_IS_FINITE(hx)) + return x*x+x; /* sqrt(NaN)=NaN, sqrt(+inf)=+inf + sqrt(-inf)=sNaN */ + /* take care of zero and -ves */ + if(FLT_UWORD_IS_ZERO(hx)) return x;/* sqrt(+-0) = +-0 */ + if(ix<0) return (x-x)/(x-x); /* sqrt(-ve) = sNaN */ + + /* normalize x */ + m = (ix>>23); + if(FLT_UWORD_IS_SUBNORMAL(hx)) { /* subnormal x */ + for(i=0;(ix&0x00800000L)==0;i++) ix<<=1; + m -= i-1; + } + m -= 127; /* unbias exponent */ + ix = (ix&0x007fffffL)|0x00800000L; + if(m&1) /* odd m, double x to make it even */ + ix += ix; + m >>= 1; /* m = [m/2] */ + + /* generate sqrt(x) bit by bit */ + ix += ix; + q = s = 0; /* q = sqrt(x) */ + r = 0x01000000L; /* r = moving bit from right to left */ + + while(r!=0) { + t = s+r; + if(t<=ix) { + s = t+r; + ix -= t; + q += r; + } + ix += ix; + r>>=1; + } + + /* use floating add to find out rounding direction */ + if(ix!=0) { + z = one-tiny; /* trigger inexact flag */ + if (z>=one) { + z = one+tiny; + if (z>one) + q += 2; + else + q += (q&1); + } + } + ix = (q>>1)+0x3f000000L; + ix += (m <<23); + SET_FLOAT_WORD(z,ix); + return z; +} diff --git a/src/math/fdlibm.h b/src/math/fdlibm.h new file mode 100644 index 00000000..821619ad --- /dev/null +++ b/src/math/fdlibm.h @@ -0,0 +1,413 @@ + +/* @(#)fdlibm.h 5.1 93/09/24 */ +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +/* AltOS local */ +#include <math.h> +#include <stdint.h> +#define __int32_t int32_t +#define __uint32_t uint32_t + +#define __ieee754_acosf acosf +#define __ieee754_sqrtf sqrtf + +/* REDHAT LOCAL: Include files. */ +#include <math.h> +/* #include <sys/types.h> */ +#include <machine/ieeefp.h> + +/* REDHAT LOCAL: Default to XOPEN_MODE. */ +#define _XOPEN_MODE + +/* Most routines need to check whether a float is finite, infinite, or not a + number, and many need to know whether the result of an operation will + overflow. These conditions depend on whether the largest exponent is + used for NaNs & infinities, or whether it's used for finite numbers. The + macros below wrap up that kind of information: + + FLT_UWORD_IS_FINITE(X) + True if a positive float with bitmask X is finite. + + FLT_UWORD_IS_NAN(X) + True if a positive float with bitmask X is not a number. + + FLT_UWORD_IS_INFINITE(X) + True if a positive float with bitmask X is +infinity. + + FLT_UWORD_MAX + The bitmask of FLT_MAX. + + FLT_UWORD_HALF_MAX + The bitmask of FLT_MAX/2. + + FLT_UWORD_EXP_MAX + The bitmask of the largest finite exponent (129 if the largest + exponent is used for finite numbers, 128 otherwise). + + FLT_UWORD_LOG_MAX + The bitmask of log(FLT_MAX), rounded down. This value is the largest + input that can be passed to exp() without producing overflow. + + FLT_UWORD_LOG_2MAX + The bitmask of log(2*FLT_MAX), rounded down. This value is the + largest input than can be passed to cosh() without producing + overflow. + + FLT_LARGEST_EXP + The largest biased exponent that can be used for finite numbers + (255 if the largest exponent is used for finite numbers, 254 + otherwise) */ + +#ifdef _FLT_LARGEST_EXPONENT_IS_NORMAL +#define FLT_UWORD_IS_FINITE(x) 1 +#define FLT_UWORD_IS_NAN(x) 0 +#define FLT_UWORD_IS_INFINITE(x) 0 +#define FLT_UWORD_MAX 0x7fffffff +#define FLT_UWORD_EXP_MAX 0x43010000 +#define FLT_UWORD_LOG_MAX 0x42b2d4fc +#define FLT_UWORD_LOG_2MAX 0x42b437e0 +#define HUGE ((float)0X1.FFFFFEP128) +#else +#define FLT_UWORD_IS_FINITE(x) ((x)<0x7f800000L) +#define FLT_UWORD_IS_NAN(x) ((x)>0x7f800000L) +#define FLT_UWORD_IS_INFINITE(x) ((x)==0x7f800000L) +#define FLT_UWORD_MAX 0x7f7fffffL +#define FLT_UWORD_EXP_MAX 0x43000000 +#define FLT_UWORD_LOG_MAX 0x42b17217 +#define FLT_UWORD_LOG_2MAX 0x42b2d4fc +#define HUGE ((float)3.40282346638528860e+38) +#endif +#define FLT_UWORD_HALF_MAX (FLT_UWORD_MAX-(1L<<23)) +#define FLT_LARGEST_EXP (FLT_UWORD_MAX>>23) + +/* Many routines check for zero and subnormal numbers. Such things depend + on whether the target supports denormals or not: + + FLT_UWORD_IS_ZERO(X) + True if a positive float with bitmask X is +0. Without denormals, + any float with a zero exponent is a +0 representation. With + denormals, the only +0 representation is a 0 bitmask. + + FLT_UWORD_IS_SUBNORMAL(X) + True if a non-zero positive float with bitmask X is subnormal. + (Routines should check for zeros first.) + + FLT_UWORD_MIN + The bitmask of the smallest float above +0. Call this number + REAL_FLT_MIN... + + FLT_UWORD_EXP_MIN + The bitmask of the float representation of REAL_FLT_MIN's exponent. + + FLT_UWORD_LOG_MIN + The bitmask of |log(REAL_FLT_MIN)|, rounding down. + + FLT_SMALLEST_EXP + REAL_FLT_MIN's exponent - EXP_BIAS (1 if denormals are not supported, + -22 if they are). +*/ + +#ifdef _FLT_NO_DENORMALS +#define FLT_UWORD_IS_ZERO(x) ((x)<0x00800000L) +#define FLT_UWORD_IS_SUBNORMAL(x) 0 +#define FLT_UWORD_MIN 0x00800000 +#define FLT_UWORD_EXP_MIN 0x42fc0000 +#define FLT_UWORD_LOG_MIN 0x42aeac50 +#define FLT_SMALLEST_EXP 1 +#else +#define FLT_UWORD_IS_ZERO(x) ((x)==0) +#define FLT_UWORD_IS_SUBNORMAL(x) ((x)<0x00800000L) +#define FLT_UWORD_MIN 0x00000001 +#define FLT_UWORD_EXP_MIN 0x43160000 +#define FLT_UWORD_LOG_MIN 0x42cff1b5 +#define FLT_SMALLEST_EXP -22 +#endif + +#ifdef __STDC__ +#undef __P +#define __P(p) p +#else +#define __P(p) () +#endif + +/* + * set X_TLOSS = pi*2**52, which is possibly defined in <values.h> + * (one may replace the following line by "#include <values.h>") + */ + +#define X_TLOSS 1.41484755040568800000e+16 + +/* Functions that are not documented, and are not in <math.h>. */ + +#ifdef _SCALB_INT +extern double scalb __P((double, int)); +#else +extern double scalb __P((double, double)); +#endif +extern double significand __P((double)); + +/* ieee style elementary functions */ +extern double __ieee754_sqrt __P((double)); +extern double __ieee754_acos __P((double)); +extern double __ieee754_acosh __P((double)); +extern double __ieee754_log __P((double)); +extern double __ieee754_atanh __P((double)); +extern double __ieee754_asin __P((double)); +extern double __ieee754_atan2 __P((double,double)); +extern double __ieee754_exp __P((double)); +extern double __ieee754_cosh __P((double)); +extern double __ieee754_fmod __P((double,double)); +extern double __ieee754_pow __P((double,double)); +extern double __ieee754_lgamma_r __P((double,int *)); +extern double __ieee754_gamma_r __P((double,int *)); +extern double __ieee754_log10 __P((double)); +extern double __ieee754_sinh __P((double)); +extern double __ieee754_hypot __P((double,double)); +extern double __ieee754_j0 __P((double)); +extern double __ieee754_j1 __P((double)); +extern double __ieee754_y0 __P((double)); +extern double __ieee754_y1 __P((double)); +extern double __ieee754_jn __P((int,double)); +extern double __ieee754_yn __P((int,double)); +extern double __ieee754_remainder __P((double,double)); +extern __int32_t __ieee754_rem_pio2 __P((double,double*)); +#ifdef _SCALB_INT +extern double __ieee754_scalb __P((double,int)); +#else +extern double __ieee754_scalb __P((double,double)); +#endif + +/* fdlibm kernel function */ +extern double __kernel_standard __P((double,double,int)); +extern double __kernel_sin __P((double,double,int)); +extern double __kernel_cos __P((double,double)); +extern double __kernel_tan __P((double,double,int)); +extern int __kernel_rem_pio2 __P((double*,double*,int,int,int,const __int32_t*)); + +/* Undocumented float functions. */ +#ifdef _SCALB_INT +extern float scalbf __P((float, int)); +#else +extern float scalbf __P((float, float)); +#endif +extern float significandf __P((float)); + +/* ieee style elementary float functions */ +extern float __ieee754_sqrtf __P((float)); +extern float __ieee754_acosf __P((float)); +extern float __ieee754_acoshf __P((float)); +extern float __ieee754_logf __P((float)); +extern float __ieee754_atanhf __P((float)); +extern float __ieee754_asinf __P((float)); +extern float __ieee754_atan2f __P((float,float)); +extern float __ieee754_expf __P((float)); +extern float __ieee754_coshf __P((float)); +extern float __ieee754_fmodf __P((float,float)); +extern float __ieee754_powf __P((float,float)); +extern float __ieee754_lgammaf_r __P((float,int *)); +extern float __ieee754_gammaf_r __P((float,int *)); +extern float __ieee754_log10f __P((float)); +extern float __ieee754_sinhf __P((float)); +extern float __ieee754_hypotf __P((float,float)); +extern float __ieee754_j0f __P((float)); +extern float __ieee754_j1f __P((float)); +extern float __ieee754_y0f __P((float)); +extern float __ieee754_y1f __P((float)); +extern float __ieee754_jnf __P((int,float)); +extern float __ieee754_ynf __P((int,float)); +extern float __ieee754_remainderf __P((float,float)); +extern __int32_t __ieee754_rem_pio2f __P((float,float*)); +#ifdef _SCALB_INT +extern float __ieee754_scalbf __P((float,int)); +#else +extern float __ieee754_scalbf __P((float,float)); +#endif + +/* float versions of fdlibm kernel functions */ +extern float __kernel_sinf __P((float,float,int)); +extern float __kernel_cosf __P((float,float)); +extern float __kernel_tanf __P((float,float,int)); +extern int __kernel_rem_pio2f __P((float*,float*,int,int,int,const __int32_t*)); + +/* The original code used statements like + n0 = ((*(int*)&one)>>29)^1; * index of high word * + ix0 = *(n0+(int*)&x); * high word of x * + ix1 = *((1-n0)+(int*)&x); * low word of x * + to dig two 32 bit words out of the 64 bit IEEE floating point + value. That is non-ANSI, and, moreover, the gcc instruction + scheduler gets it wrong. We instead use the following macros. + Unlike the original code, we determine the endianness at compile + time, not at run time; I don't see much benefit to selecting + endianness at run time. */ + +#ifndef __IEEE_BIG_ENDIAN +#ifndef __IEEE_LITTLE_ENDIAN + #error Must define endianness +#endif +#endif + +/* A union which permits us to convert between a double and two 32 bit + ints. */ + +#ifdef __IEEE_BIG_ENDIAN + +typedef union +{ + double value; + struct + { + __uint32_t msw; + __uint32_t lsw; + } parts; +} ieee_double_shape_type; + +#endif + +#ifdef __IEEE_LITTLE_ENDIAN + +typedef union +{ + double value; + struct + { + __uint32_t lsw; + __uint32_t msw; + } parts; +} ieee_double_shape_type; + +#endif + +/* Get two 32 bit ints from a double. */ + +#define EXTRACT_WORDS(ix0,ix1,d) \ +do { \ + ieee_double_shape_type ew_u; \ + ew_u.value = (d); \ + (ix0) = ew_u.parts.msw; \ + (ix1) = ew_u.parts.lsw; \ +} while (0) + +/* Get the more significant 32 bit int from a double. */ + +#define GET_HIGH_WORD(i,d) \ +do { \ + ieee_double_shape_type gh_u; \ + gh_u.value = (d); \ + (i) = gh_u.parts.msw; \ +} while (0) + +/* Get the less significant 32 bit int from a double. */ + +#define GET_LOW_WORD(i,d) \ +do { \ + ieee_double_shape_type gl_u; \ + gl_u.value = (d); \ + (i) = gl_u.parts.lsw; \ +} while (0) + +/* Set a double from two 32 bit ints. */ + +#define INSERT_WORDS(d,ix0,ix1) \ +do { \ + ieee_double_shape_type iw_u; \ + iw_u.parts.msw = (ix0); \ + iw_u.parts.lsw = (ix1); \ + (d) = iw_u.value; \ +} while (0) + +/* Set the more significant 32 bits of a double from an int. */ + +#define SET_HIGH_WORD(d,v) \ +do { \ + ieee_double_shape_type sh_u; \ + sh_u.value = (d); \ + sh_u.parts.msw = (v); \ + (d) = sh_u.value; \ +} while (0) + +/* Set the less significant 32 bits of a double from an int. */ + +#define SET_LOW_WORD(d,v) \ +do { \ + ieee_double_shape_type sl_u; \ + sl_u.value = (d); \ + sl_u.parts.lsw = (v); \ + (d) = sl_u.value; \ +} while (0) + +/* A union which permits us to convert between a float and a 32 bit + int. */ + +typedef union +{ + float value; + __uint32_t word; +} ieee_float_shape_type; + +/* Get a 32 bit int from a float. */ + +#define GET_FLOAT_WORD(i,d) \ +do { \ + ieee_float_shape_type gf_u; \ + gf_u.value = (d); \ + (i) = gf_u.word; \ +} while (0) + +/* Set a float from a 32 bit int. */ + +#define SET_FLOAT_WORD(d,i) \ +do { \ + ieee_float_shape_type sf_u; \ + sf_u.word = (i); \ + (d) = sf_u.value; \ +} while (0) + +/* Macros to avoid undefined behaviour that can arise if the amount + of a shift is exactly equal to the size of the shifted operand. */ + +#define SAFE_LEFT_SHIFT(op,amt) \ + (((amt) < 8 * sizeof(op)) ? ((op) << (amt)) : 0) + +#define SAFE_RIGHT_SHIFT(op,amt) \ + (((amt) < 8 * sizeof(op)) ? ((op) >> (amt)) : 0) + +#ifdef _COMPLEX_H + +/* + * Quoting from ISO/IEC 9899:TC2: + * + * 6.2.5.13 Types + * Each complex type has the same representation and alignment requirements as + * an array type containing exactly two elements of the corresponding real type; + * the first element is equal to the real part, and the second element to the + * imaginary part, of the complex number. + */ +typedef union { + float complex z; + float parts[2]; +} float_complex; + +typedef union { + double complex z; + double parts[2]; +} double_complex; + +typedef union { + long double complex z; + long double parts[2]; +} long_double_complex; + +#define REAL_PART(z) ((z).parts[0]) +#define IMAG_PART(z) ((z).parts[1]) + +#endif /* _COMPLEX_H */ + diff --git a/src/math/ieeefp.h b/src/math/ieeefp.h new file mode 100644 index 00000000..0b06fb78 --- /dev/null +++ b/src/math/ieeefp.h @@ -0,0 +1,256 @@ +#ifndef _IEEE_FP_H_ +#define _IEEE_FP_H_ + +#include "_ansi.h" + +#include <machine/ieeefp.h> + +_BEGIN_STD_C + +/* FIXME FIXME FIXME: + Neither of __ieee_{float,double}_shape_tape seem to be used anywhere + except in libm/test. If that is the case, please delete these from here. + If that is not the case, please insert documentation here describing why + they're needed. */ + +#ifdef __IEEE_BIG_ENDIAN + +typedef union +{ + double value; + struct + { + unsigned int sign : 1; + unsigned int exponent: 11; + unsigned int fraction0:4; + unsigned int fraction1:16; + unsigned int fraction2:16; + unsigned int fraction3:16; + + } number; + struct + { + unsigned int sign : 1; + unsigned int exponent: 11; + unsigned int quiet:1; + unsigned int function0:3; + unsigned int function1:16; + unsigned int function2:16; + unsigned int function3:16; + } nan; + struct + { + unsigned long msw; + unsigned long lsw; + } parts; + long aslong[2]; +} __ieee_double_shape_type; + +#endif + +#ifdef __IEEE_LITTLE_ENDIAN + +typedef union +{ + double value; + struct + { +#ifdef __SMALL_BITFIELDS + unsigned int fraction3:16; + unsigned int fraction2:16; + unsigned int fraction1:16; + unsigned int fraction0: 4; +#else + unsigned int fraction1:32; + unsigned int fraction0:20; +#endif + unsigned int exponent :11; + unsigned int sign : 1; + } number; + struct + { +#ifdef __SMALL_BITFIELDS + unsigned int function3:16; + unsigned int function2:16; + unsigned int function1:16; + unsigned int function0:3; +#else + unsigned int function1:32; + unsigned int function0:19; +#endif + unsigned int quiet:1; + unsigned int exponent: 11; + unsigned int sign : 1; + } nan; + struct + { + unsigned long lsw; + unsigned long msw; + } parts; + + long aslong[2]; + +} __ieee_double_shape_type; + +#endif + +#ifdef __IEEE_BIG_ENDIAN + +typedef union +{ + float value; + struct + { + unsigned int sign : 1; + unsigned int exponent: 8; + unsigned int fraction0: 7; + unsigned int fraction1: 16; + } number; + struct + { + unsigned int sign:1; + unsigned int exponent:8; + unsigned int quiet:1; + unsigned int function0:6; + unsigned int function1:16; + } nan; + long p1; + +} __ieee_float_shape_type; + +#endif + +#ifdef __IEEE_LITTLE_ENDIAN + +typedef union +{ + float value; + struct + { + unsigned int fraction0: 7; + unsigned int fraction1: 16; + unsigned int exponent: 8; + unsigned int sign : 1; + } number; + struct + { + unsigned int function1:16; + unsigned int function0:6; + unsigned int quiet:1; + unsigned int exponent:8; + unsigned int sign:1; + } nan; + long p1; + +} __ieee_float_shape_type; + +#endif + + + + + +/* FLOATING ROUNDING */ + +typedef int fp_rnd; +#define FP_RN 0 /* Round to nearest */ +#define FP_RM 1 /* Round down */ +#define FP_RP 2 /* Round up */ +#define FP_RZ 3 /* Round to zero (trunate) */ + +fp_rnd _EXFUN(fpgetround,(void)); +fp_rnd _EXFUN(fpsetround, (fp_rnd)); + +/* EXCEPTIONS */ + +typedef int fp_except; +#define FP_X_INV 0x10 /* Invalid operation */ +#define FP_X_DX 0x80 /* Divide by zero */ +#define FP_X_OFL 0x04 /* Overflow exception */ +#define FP_X_UFL 0x02 /* Underflow exception */ +#define FP_X_IMP 0x01 /* imprecise exception */ + +fp_except _EXFUN(fpgetmask,(void)); +fp_except _EXFUN(fpsetmask,(fp_except)); +fp_except _EXFUN(fpgetsticky,(void)); +fp_except _EXFUN(fpsetsticky, (fp_except)); + +/* INTEGER ROUNDING */ + +typedef int fp_rdi; +#define FP_RDI_TOZ 0 /* Round to Zero */ +#define FP_RDI_RD 1 /* Follow float mode */ + +fp_rdi _EXFUN(fpgetroundtoi,(void)); +fp_rdi _EXFUN(fpsetroundtoi,(fp_rdi)); + +#undef isnan +#undef isinf + +int _EXFUN(isnan, (double)); +int _EXFUN(isinf, (double)); +int _EXFUN(finite, (double)); + + + +int _EXFUN(isnanf, (float)); +int _EXFUN(isinff, (float)); +int _EXFUN(finitef, (float)); + +#define __IEEE_DBL_EXPBIAS 1023 +#define __IEEE_FLT_EXPBIAS 127 + +#define __IEEE_DBL_EXPLEN 11 +#define __IEEE_FLT_EXPLEN 8 + + +#define __IEEE_DBL_FRACLEN (64 - (__IEEE_DBL_EXPLEN + 1)) +#define __IEEE_FLT_FRACLEN (32 - (__IEEE_FLT_EXPLEN + 1)) + +#define __IEEE_DBL_MAXPOWTWO ((double)(1L << 32 - 2) * (1L << (32-11) - 32 + 1)) +#define __IEEE_FLT_MAXPOWTWO ((float)(1L << (32-8) - 1)) + +#define __IEEE_DBL_NAN_EXP 0x7ff +#define __IEEE_FLT_NAN_EXP 0xff + +#ifndef __ieeefp_isnanf +#define __ieeefp_isnanf(x) (((*(long *)&(x) & 0x7f800000L)==0x7f800000L) && \ + ((*(long *)&(x) & 0x007fffffL)!=0000000000L)) +#endif +#define isnanf(x) __ieeefp_isnanf(x) + +#ifndef __ieeefp_isinff +#define __ieeefp_isinff(x) (((*(long *)&(x) & 0x7f800000L)==0x7f800000L) && \ + ((*(long *)&(x) & 0x007fffffL)==0000000000L)) +#endif +#define isinff(x) __ieeefp_isinff(x) + +#ifndef __ieeefp_finitef +#define __ieeefp_finitef(x) (((*(long *)&(x) & 0x7f800000L)!=0x7f800000L)) +#endif +#define finitef(x) __ieeefp_finitef(x) + +#ifdef _DOUBLE_IS_32BITS +#undef __IEEE_DBL_EXPBIAS +#define __IEEE_DBL_EXPBIAS __IEEE_FLT_EXPBIAS + +#undef __IEEE_DBL_EXPLEN +#define __IEEE_DBL_EXPLEN __IEEE_FLT_EXPLEN + +#undef __IEEE_DBL_FRACLEN +#define __IEEE_DBL_FRACLEN __IEEE_FLT_FRACLEN + +#undef __IEEE_DBL_MAXPOWTWO +#define __IEEE_DBL_MAXPOWTWO __IEEE_FLT_MAXPOWTWO + +#undef __IEEE_DBL_NAN_EXP +#define __IEEE_DBL_NAN_EXP __IEEE_FLT_NAN_EXP + +#undef __ieee_double_shape_type +#define __ieee_double_shape_type __ieee_float_shape_type + +#endif /* _DOUBLE_IS_32BITS */ + +_END_STD_C + +#endif /* _IEEE_FP_H_ */ diff --git a/src/math/kf_cos.c b/src/math/kf_cos.c new file mode 100644 index 00000000..4f71af23 --- /dev/null +++ b/src/math/kf_cos.c @@ -0,0 +1,59 @@ +/* kf_cos.c -- float version of k_cos.c + * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com. + */ + +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +#include "fdlibm.h" + +#ifdef __STDC__ +static const float +#else +static float +#endif +one = 1.0000000000e+00, /* 0x3f800000 */ +C1 = 4.1666667908e-02, /* 0x3d2aaaab */ +C2 = -1.3888889225e-03, /* 0xbab60b61 */ +C3 = 2.4801587642e-05, /* 0x37d00d01 */ +C4 = -2.7557314297e-07, /* 0xb493f27c */ +C5 = 2.0875723372e-09, /* 0x310f74f6 */ +C6 = -1.1359647598e-11; /* 0xad47d74e */ + +#ifdef __STDC__ + float __kernel_cosf(float x, float y) +#else + float __kernel_cosf(x, y) + float x,y; +#endif +{ + float a,hz,z,r,qx; + __int32_t ix; + GET_FLOAT_WORD(ix,x); + ix &= 0x7fffffff; /* ix = |x|'s high word*/ + if(ix<0x32000000) { /* if x < 2**27 */ + if(((int)x)==0) return one; /* generate inexact */ + } + z = x*x; + r = z*(C1+z*(C2+z*(C3+z*(C4+z*(C5+z*C6))))); + if(ix < 0x3e99999a) /* if |x| < 0.3 */ + return one - ((float)0.5*z - (z*r - x*y)); + else { + if(ix > 0x3f480000) { /* x > 0.78125 */ + qx = (float)0.28125; + } else { + SET_FLOAT_WORD(qx,ix-0x01000000); /* x/4 */ + } + hz = (float)0.5*z-qx; + a = one-qx; + return a - (hz - (z*r-x*y)); + } +} diff --git a/src/math/kf_rem_pio2.c b/src/math/kf_rem_pio2.c new file mode 100644 index 00000000..261c4812 --- /dev/null +++ b/src/math/kf_rem_pio2.c @@ -0,0 +1,208 @@ +/* kf_rem_pio2.c -- float version of k_rem_pio2.c + * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com. + */ + +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +#include "fdlibm.h" + +/* In the float version, the input parameter x contains 8 bit + integers, not 24 bit integers. 113 bit precision is not supported. */ + +#ifdef __STDC__ +static const int init_jk[] = {4,7,9}; /* initial value for jk */ +#else +static int init_jk[] = {4,7,9}; +#endif + +#ifdef __STDC__ +static const float PIo2[] = { +#else +static float PIo2[] = { +#endif + 1.5703125000e+00, /* 0x3fc90000 */ + 4.5776367188e-04, /* 0x39f00000 */ + 2.5987625122e-05, /* 0x37da0000 */ + 7.5437128544e-08, /* 0x33a20000 */ + 6.0026650317e-11, /* 0x2e840000 */ + 7.3896444519e-13, /* 0x2b500000 */ + 5.3845816694e-15, /* 0x27c20000 */ + 5.6378512969e-18, /* 0x22d00000 */ + 8.3009228831e-20, /* 0x1fc40000 */ + 3.2756352257e-22, /* 0x1bc60000 */ + 6.3331015649e-25, /* 0x17440000 */ +}; + +#ifdef __STDC__ +static const float +#else +static float +#endif +zero = 0.0, +one = 1.0, +two8 = 2.5600000000e+02, /* 0x43800000 */ +twon8 = 3.9062500000e-03; /* 0x3b800000 */ + +#ifdef __STDC__ + int __kernel_rem_pio2f(float *x, float *y, int e0, int nx, int prec, const __int32_t *ipio2) +#else + int __kernel_rem_pio2f(x,y,e0,nx,prec,ipio2) + float x[], y[]; int e0,nx,prec; __int32_t ipio2[]; +#endif +{ + __int32_t jz,jx,jv,jp,jk,carry,n,iq[20],i,j,k,m,q0,ih; + float z,fw,f[20],fq[20],q[20]; + + /* initialize jk*/ + jk = init_jk[prec]; + jp = jk; + + /* determine jx,jv,q0, note that 3>q0 */ + jx = nx-1; + jv = (e0-3)/8; if(jv<0) jv=0; + q0 = e0-8*(jv+1); + + /* set up f[0] to f[jx+jk] where f[jx+jk] = ipio2[jv+jk] */ + j = jv-jx; m = jx+jk; + for(i=0;i<=m;i++,j++) f[i] = (j<0)? zero : (float) ipio2[j]; + + /* compute q[0],q[1],...q[jk] */ + for (i=0;i<=jk;i++) { + for(j=0,fw=0.0;j<=jx;j++) fw += x[j]*f[jx+i-j]; q[i] = fw; + } + + jz = jk; +recompute: + /* distill q[] into iq[] reversingly */ + for(i=0,j=jz,z=q[jz];j>0;i++,j--) { + fw = (float)((__int32_t)(twon8* z)); + iq[i] = (__int32_t)(z-two8*fw); + z = q[j-1]+fw; + } + + /* compute n */ + z = scalbnf(z,(int)q0); /* actual value of z */ + z -= (float)8.0*floorf(z*(float)0.125); /* trim off integer >= 8 */ + n = (__int32_t) z; + z -= (float)n; + ih = 0; + if(q0>0) { /* need iq[jz-1] to determine n */ + i = (iq[jz-1]>>(8-q0)); n += i; + iq[jz-1] -= i<<(8-q0); + ih = iq[jz-1]>>(7-q0); + } + else if(q0==0) ih = iq[jz-1]>>8; + else if(z>=(float)0.5) ih=2; + + if(ih>0) { /* q > 0.5 */ + n += 1; carry = 0; + for(i=0;i<jz ;i++) { /* compute 1-q */ + j = iq[i]; + if(carry==0) { + if(j!=0) { + carry = 1; iq[i] = 0x100- j; + } + } else iq[i] = 0xff - j; + } + if(q0>0) { /* rare case: chance is 1 in 12 */ + switch(q0) { + case 1: + iq[jz-1] &= 0x7f; break; + case 2: + iq[jz-1] &= 0x3f; break; + } + } + if(ih==2) { + z = one - z; + if(carry!=0) z -= scalbnf(one,(int)q0); + } + } + + /* check if recomputation is needed */ + if(z==zero) { + j = 0; + for (i=jz-1;i>=jk;i--) j |= iq[i]; + if(j==0) { /* need recomputation */ + for(k=1;iq[jk-k]==0;k++); /* k = no. of terms needed */ + + for(i=jz+1;i<=jz+k;i++) { /* add q[jz+1] to q[jz+k] */ + f[jx+i] = (float) ipio2[jv+i]; + for(j=0,fw=0.0;j<=jx;j++) fw += x[j]*f[jx+i-j]; + q[i] = fw; + } + jz += k; + goto recompute; + } + } + + /* chop off zero terms */ + if(z==(float)0.0) { + jz -= 1; q0 -= 8; + while(iq[jz]==0) { jz--; q0-=8;} + } else { /* break z into 8-bit if necessary */ + z = scalbnf(z,-(int)q0); + if(z>=two8) { + fw = (float)((__int32_t)(twon8*z)); + iq[jz] = (__int32_t)(z-two8*fw); + jz += 1; q0 += 8; + iq[jz] = (__int32_t) fw; + } else iq[jz] = (__int32_t) z ; + } + + /* convert integer "bit" chunk to floating-point value */ + fw = scalbnf(one,(int)q0); + for(i=jz;i>=0;i--) { + q[i] = fw*(float)iq[i]; fw*=twon8; + } + + /* compute PIo2[0,...,jp]*q[jz,...,0] */ + for(i=jz;i>=0;i--) { + for(fw=0.0,k=0;k<=jp&&k<=jz-i;k++) fw += PIo2[k]*q[i+k]; + fq[jz-i] = fw; + } + + /* compress fq[] into y[] */ + switch(prec) { + case 0: + fw = 0.0; + for (i=jz;i>=0;i--) fw += fq[i]; + y[0] = (ih==0)? fw: -fw; + break; + case 1: + case 2: + fw = 0.0; + for (i=jz;i>=0;i--) fw += fq[i]; + y[0] = (ih==0)? fw: -fw; + fw = fq[0]-fw; + for (i=1;i<=jz;i++) fw += fq[i]; + y[1] = (ih==0)? fw: -fw; + break; + case 3: /* painful */ + for (i=jz;i>0;i--) { + fw = fq[i-1]+fq[i]; + fq[i] += fq[i-1]-fw; + fq[i-1] = fw; + } + for (i=jz;i>1;i--) { + fw = fq[i-1]+fq[i]; + fq[i] += fq[i-1]-fw; + fq[i-1] = fw; + } + for (fw=0.0,i=jz;i>=2;i--) fw += fq[i]; + if(ih==0) { + y[0] = fq[0]; y[1] = fq[1]; y[2] = fw; + } else { + y[0] = -fq[0]; y[1] = -fq[1]; y[2] = -fw; + } + } + return n&7; +} diff --git a/src/math/kf_sin.c b/src/math/kf_sin.c new file mode 100644 index 00000000..e81fa0bd --- /dev/null +++ b/src/math/kf_sin.c @@ -0,0 +1,49 @@ +/* kf_sin.c -- float version of k_sin.c + * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com. + */ + +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +#include "fdlibm.h" + +#ifdef __STDC__ +static const float +#else +static float +#endif +half = 5.0000000000e-01,/* 0x3f000000 */ +S1 = -1.6666667163e-01, /* 0xbe2aaaab */ +S2 = 8.3333337680e-03, /* 0x3c088889 */ +S3 = -1.9841270114e-04, /* 0xb9500d01 */ +S4 = 2.7557314297e-06, /* 0x3638ef1b */ +S5 = -2.5050759689e-08, /* 0xb2d72f34 */ +S6 = 1.5896910177e-10; /* 0x2f2ec9d3 */ + +#ifdef __STDC__ + float __kernel_sinf(float x, float y, int iy) +#else + float __kernel_sinf(x, y, iy) + float x,y; int iy; /* iy=0 if y is zero */ +#endif +{ + float z,r,v; + __int32_t ix; + GET_FLOAT_WORD(ix,x); + ix &= 0x7fffffff; /* high word of x */ + if(ix<0x32000000) /* |x| < 2**-27 */ + {if((int)x==0) return x;} /* generate inexact */ + z = x*x; + v = z*x; + r = S2+z*(S3+z*(S4+z*(S5+z*S6))); + if(iy==0) return x+v*(S1+z*r); + else return x-((z*(half*y-v*r)-y)-v*S1); +} diff --git a/src/math/machine/ieeefp.h b/src/math/machine/ieeefp.h new file mode 100644 index 00000000..fffa3804 --- /dev/null +++ b/src/math/machine/ieeefp.h @@ -0,0 +1,382 @@ +#ifndef __IEEE_BIG_ENDIAN +#ifndef __IEEE_LITTLE_ENDIAN + +/* This file can define macros to choose variations of the IEEE float + format: + + _FLT_LARGEST_EXPONENT_IS_NORMAL + + Defined if the float format uses the largest exponent for finite + numbers rather than NaN and infinity representations. Such a + format cannot represent NaNs or infinities at all, but it's FLT_MAX + is twice the IEEE value. + + _FLT_NO_DENORMALS + + Defined if the float format does not support IEEE denormals. Every + float with a zero exponent is taken to be a zero representation. + + ??? At the moment, there are no equivalent macros above for doubles and + the macros are not fully supported by --enable-newlib-hw-fp. + + __IEEE_BIG_ENDIAN + + Defined if the float format is big endian. This is mutually exclusive + with __IEEE_LITTLE_ENDIAN. + + __IEEE_LITTLE_ENDIAN + + Defined if the float format is little endian. This is mutually exclusive + with __IEEE_BIG_ENDIAN. + + Note that one of __IEEE_BIG_ENDIAN or __IEEE_LITTLE_ENDIAN must be specified for a + platform or error will occur. + + __IEEE_BYTES_LITTLE_ENDIAN + + This flag is used in conjunction with __IEEE_BIG_ENDIAN to describe a situation + whereby multiple words of an IEEE floating point are in big endian order, but the + words themselves are little endian with respect to the bytes. + + _DOUBLE_IS_32BITS + + This is used on platforms that support double by using the 32-bit IEEE + float type. + + _FLOAT_ARG + + This represents what type a float arg is passed as. It is used when the type is + not promoted to double. + +*/ + +#if (defined(__arm__) || defined(__thumb__)) && !defined(__MAVERICK__) +/* ARM traditionally used big-endian words; and within those words the + byte ordering was big or little endian depending upon the target. + Modern floating-point formats are naturally ordered; in this case + __VFP_FP__ will be defined, even if soft-float. */ +#ifdef __VFP_FP__ +# ifdef __ARMEL__ +# define __IEEE_LITTLE_ENDIAN +# else +# define __IEEE_BIG_ENDIAN +# endif +#else +# define __IEEE_BIG_ENDIAN +# ifdef __ARMEL__ +# define __IEEE_BYTES_LITTLE_ENDIAN +# endif +#endif +#endif + +#ifdef __hppa__ +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __SPU__ +#define __IEEE_BIG_ENDIAN + +#define isfinite(__y) \ + (__extension__ ({int __cy; \ + (sizeof (__y) == sizeof (float)) ? (1) : \ + (__cy = fpclassify(__y)) != FP_INFINITE && __cy != FP_NAN;})) + +#define isinf(__x) ((sizeof (__x) == sizeof (float)) ? (0) : __isinfd(__x)) +#define isnan(__x) ((sizeof (__x) == sizeof (float)) ? (0) : __isnand(__x)) + +/* + * Macros for use in ieeefp.h. We can't just define the real ones here + * (like those above) as we have name space issues when this is *not* + * included via generic the ieeefp.h. + */ +#define __ieeefp_isnanf(x) 0 +#define __ieeefp_isinff(x) 0 +#define __ieeefp_finitef(x) 1 +#endif + +#ifdef __sparc__ +#ifdef __LITTLE_ENDIAN_DATA__ +#define __IEEE_LITTLE_ENDIAN +#else +#define __IEEE_BIG_ENDIAN +#endif +#endif + +#if defined(__m68k__) || defined(__mc68000__) +#define __IEEE_BIG_ENDIAN +#endif + +#if defined(__mc68hc11__) || defined(__mc68hc12__) || defined(__mc68hc1x__) +#define __IEEE_BIG_ENDIAN +#ifdef __HAVE_SHORT_DOUBLE__ +# define _DOUBLE_IS_32BITS +#endif +#endif + +#if defined (__H8300__) || defined (__H8300H__) || defined (__H8300S__) || defined (__H8500__) || defined (__H8300SX__) +#define __IEEE_BIG_ENDIAN +#define _FLOAT_ARG float +#define _DOUBLE_IS_32BITS +#endif + +#if defined (__xc16x__) || defined (__xc16xL__) || defined (__xc16xS__) +#define __IEEE_LITTLE_ENDIAN +#define _FLOAT_ARG float +#define _DOUBLE_IS_32BITS +#endif + + +#ifdef __sh__ +#ifdef __LITTLE_ENDIAN__ +#define __IEEE_LITTLE_ENDIAN +#else +#define __IEEE_BIG_ENDIAN +#endif +#if defined(__SH2E__) || defined(__SH3E__) || defined(__SH4_SINGLE_ONLY__) || defined(__SH2A_SINGLE_ONLY__) +#define _DOUBLE_IS_32BITS +#endif +#endif + +#ifdef _AM29K +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef _WIN32 +#define __IEEE_LITTLE_ENDIAN +#endif + +#ifdef __i386__ +#define __IEEE_LITTLE_ENDIAN +#endif + +#ifdef __i960__ +#define __IEEE_LITTLE_ENDIAN +#endif + +#ifdef __lm32__ +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __M32R__ +#define __IEEE_BIG_ENDIAN +#endif + +#if defined(_C4x) || defined(_C3x) +#define __IEEE_BIG_ENDIAN +#define _DOUBLE_IS_32BITS +#endif + +#ifdef __TMS320C6X__ +#ifdef _BIG_ENDIAN +#define __IEEE_BIG_ENDIAN +#else +#define __IEEE_LITTLE_ENDIAN +#endif +#endif + +#ifdef __TIC80__ +#define __IEEE_LITTLE_ENDIAN +#endif + +#ifdef __MIPSEL__ +#define __IEEE_LITTLE_ENDIAN +#endif +#ifdef __MIPSEB__ +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __MMIX__ +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __D30V__ +#define __IEEE_BIG_ENDIAN +#endif + +/* necv70 was __IEEE_LITTLE_ENDIAN. */ + +#ifdef __W65__ +#define __IEEE_LITTLE_ENDIAN +#define _DOUBLE_IS_32BITS +#endif + +#if defined(__Z8001__) || defined(__Z8002__) +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __m88k__ +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __mn10300__ +#define __IEEE_LITTLE_ENDIAN +#endif + +#ifdef __mn10200__ +#define __IEEE_LITTLE_ENDIAN +#define _DOUBLE_IS_32BITS +#endif + +#ifdef __v800 +#define __IEEE_LITTLE_ENDIAN +#endif + +#ifdef __v850 +#define __IEEE_LITTLE_ENDIAN +#endif + +#ifdef __D10V__ +#define __IEEE_BIG_ENDIAN +#if __DOUBLE__ == 32 +#define _DOUBLE_IS_32BITS +#endif +#endif + +#ifdef __PPC__ +#if (defined(_BIG_ENDIAN) && _BIG_ENDIAN) || (defined(_AIX) && _AIX) +#define __IEEE_BIG_ENDIAN +#else +#if (defined(_LITTLE_ENDIAN) && _LITTLE_ENDIAN) || (defined(__sun__) && __sun__) || (defined(_WIN32) && _WIN32) +#define __IEEE_LITTLE_ENDIAN +#endif +#endif +#endif + +#ifdef __xstormy16__ +#define __IEEE_LITTLE_ENDIAN +#endif + +#ifdef __arc__ +#ifdef __big_endian__ +#define __IEEE_BIG_ENDIAN +#else +#define __IEEE_LITTLE_ENDIAN +#endif +#endif + +#ifdef __CRX__ +#define __IEEE_LITTLE_ENDIAN +#endif + +#ifdef __fr30__ +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __mcore__ +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __mt__ +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __frv__ +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __moxie__ +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __ia64__ +#ifdef __BIG_ENDIAN__ +#define __IEEE_BIG_ENDIAN +#else +#define __IEEE_LITTLE_ENDIAN +#endif +#endif + +#ifdef __AVR__ +#define __IEEE_LITTLE_ENDIAN +#define _DOUBLE_IS_32BITS +#endif + +#if defined(__or32__) || defined(__or1k__) || defined(__or16__) +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __IP2K__ +#define __IEEE_BIG_ENDIAN +#define __SMALL_BITFIELDS +#define _DOUBLE_IS_32BITS +#endif + +#ifdef __iq2000__ +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __MAVERICK__ +#ifdef __ARMEL__ +# define __IEEE_LITTLE_ENDIAN +#else /* must be __ARMEB__ */ +# define __IEEE_BIG_ENDIAN +#endif /* __ARMEL__ */ +#endif /* __MAVERICK__ */ + +#ifdef __m32c__ +#define __IEEE_LITTLE_ENDIAN +#define __SMALL_BITFIELDS +#endif + +#ifdef __CRIS__ +#define __IEEE_LITTLE_ENDIAN +#endif + +#ifdef __BFIN__ +#define __IEEE_LITTLE_ENDIAN +#endif + +#ifdef __x86_64__ +#define __IEEE_LITTLE_ENDIAN +#endif + +#ifdef __mep__ +#ifdef __LITTLE_ENDIAN__ +#define __IEEE_LITTLE_ENDIAN +#else +#define __IEEE_BIG_ENDIAN +#endif +#endif + +#ifdef __MICROBLAZE__ +#define __IEEE_BIG_ENDIAN +#endif + +#ifdef __RL78__ +#define __IEEE_LITTLE_ENDIAN +#define __SMALL_BITFIELDS /* 16 Bit INT */ +#define _DOUBLE_IS_32BITS +#endif + +#ifdef __RX__ + +#ifdef __RX_BIG_ENDIAN__ +#define __IEEE_BIG_ENDIAN +#else +#define __IEEE_LITTLE_ENDIAN +#endif + +#ifndef __RX_64BIT_DOUBLES__ +#define _DOUBLE_IS_32BITS +#endif + +#ifdef __RX_16BIT_INTS__ +#define __SMALL_BITFIELDS +#endif + +#endif + +#if (defined(__CR16__) || defined(__CR16C__) ||defined(__CR16CP__)) +#define __IEEE_LITTLE_ENDIAN +#define __SMALL_BITFIELDS /* 16 Bit INT */ +#endif + +#ifndef __IEEE_BIG_ENDIAN +#ifndef __IEEE_LITTLE_ENDIAN +#error Endianess not declared!! +#endif /* not __IEEE_LITTLE_ENDIAN */ +#endif /* not __IEEE_BIG_ENDIAN */ + +#endif /* not __IEEE_LITTLE_ENDIAN */ +#endif /* not __IEEE_BIG_ENDIAN */ + diff --git a/src/math/math.h b/src/math/math.h new file mode 100644 index 00000000..fd543bc2 --- /dev/null +++ b/src/math/math.h @@ -0,0 +1,37 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _MATH_H_ +#define _MATH_H_ + +float acosf(float x); + +float cosf(float x); + +float sinf(float x); + +float sqrtf(float x); + +float fabsf(float x); + +float floorf(float x); + +float scalbnf(float x, int n); + +float copysignf(float x, float y); + +#endif diff --git a/src/math/sf_copysign.c b/src/math/sf_copysign.c new file mode 100644 index 00000000..f547c82e --- /dev/null +++ b/src/math/sf_copysign.c @@ -0,0 +1,50 @@ +/* sf_copysign.c -- float version of s_copysign.c. + * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com. + */ + +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +/* + * copysignf(float x, float y) + * copysignf(x,y) returns a value with the magnitude of x and + * with the sign bit of y. + */ + +#include "fdlibm.h" + +#ifdef __STDC__ + float copysignf(float x, float y) +#else + float copysignf(x,y) + float x,y; +#endif +{ + __uint32_t ix,iy; + GET_FLOAT_WORD(ix,x); + GET_FLOAT_WORD(iy,y); + SET_FLOAT_WORD(x,(ix&0x7fffffff)|(iy&0x80000000)); + return x; +} + +#ifdef _DOUBLE_IS_32BITS + +#ifdef __STDC__ + double copysign(double x, double y) +#else + double copysign(x,y) + double x,y; +#endif +{ + return (double) copysignf((float) x, (float) y); +} + +#endif /* defined(_DOUBLE_IS_32BITS) */ diff --git a/src/math/sf_cos.c b/src/math/sf_cos.c new file mode 100644 index 00000000..4c0a9a53 --- /dev/null +++ b/src/math/sf_cos.c @@ -0,0 +1,68 @@ +/* sf_cos.c -- float version of s_cos.c. + * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com. + */ + +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +#include "fdlibm.h" + +#ifdef __STDC__ +static const float one=1.0; +#else +static float one=1.0; +#endif + +#ifdef __STDC__ + float cosf(float x) +#else + float cosf(x) + float x; +#endif +{ + float y[2],z=0.0; + __int32_t n,ix; + + GET_FLOAT_WORD(ix,x); + + /* |x| ~< pi/4 */ + ix &= 0x7fffffff; + if(ix <= 0x3f490fd8) return __kernel_cosf(x,z); + + /* cos(Inf or NaN) is NaN */ + else if (!FLT_UWORD_IS_FINITE(ix)) return x-x; + + /* argument reduction needed */ + else { + n = __ieee754_rem_pio2f(x,y); + switch(n&3) { + case 0: return __kernel_cosf(y[0],y[1]); + case 1: return -__kernel_sinf(y[0],y[1],1); + case 2: return -__kernel_cosf(y[0],y[1]); + default: + return __kernel_sinf(y[0],y[1],1); + } + } +} + +#ifdef _DOUBLE_IS_32BITS + +#ifdef __STDC__ + double cos(double x) +#else + double cos(x) + double x; +#endif +{ + return (double) cosf((float) x); +} + +#endif /* defined(_DOUBLE_IS_32BITS) */ diff --git a/src/math/sf_fabs.c b/src/math/sf_fabs.c new file mode 100644 index 00000000..2aaed326 --- /dev/null +++ b/src/math/sf_fabs.c @@ -0,0 +1,47 @@ +/* sf_fabs.c -- float version of s_fabs.c. + * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com. + */ + +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +/* + * fabsf(x) returns the absolute value of x. + */ + +#include "fdlibm.h" + +#ifdef __STDC__ + float fabsf(float x) +#else + float fabsf(x) + float x; +#endif +{ + __uint32_t ix; + GET_FLOAT_WORD(ix,x); + SET_FLOAT_WORD(x,ix&0x7fffffff); + return x; +} + +#ifdef _DOUBLE_IS_32BITS + +#ifdef __STDC__ + double fabs(double x) +#else + double fabs(x) + double x; +#endif +{ + return (double) fabsf((float) x); +} + +#endif /* defined(_DOUBLE_IS_32BITS) */ diff --git a/src/math/sf_floor.c b/src/math/sf_floor.c new file mode 100644 index 00000000..9264d81e --- /dev/null +++ b/src/math/sf_floor.c @@ -0,0 +1,80 @@ +/* sf_floor.c -- float version of s_floor.c. + * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com. + */ + +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +/* + * floorf(x) + * Return x rounded toward -inf to integral value + * Method: + * Bit twiddling. + * Exception: + * Inexact flag raised if x not equal to floorf(x). + */ + +#include "fdlibm.h" + +#ifdef __STDC__ +static const float huge = 1.0e30; +#else +static float huge = 1.0e30; +#endif + +#ifdef __STDC__ + float floorf(float x) +#else + float floorf(x) + float x; +#endif +{ + __int32_t i0,j0; + __uint32_t i,ix; + GET_FLOAT_WORD(i0,x); + ix = (i0&0x7fffffff); + j0 = (ix>>23)-0x7f; + if(j0<23) { + if(j0<0) { /* raise inexact if x != 0 */ + if(huge+x>(float)0.0) {/* return 0*sign(x) if |x|<1 */ + if(i0>=0) {i0=0;} + else if(!FLT_UWORD_IS_ZERO(ix)) + { i0=0xbf800000;} + } + } else { + i = (0x007fffff)>>j0; + if((i0&i)==0) return x; /* x is integral */ + if(huge+x>(float)0.0) { /* raise inexact flag */ + if(i0<0) i0 += (0x00800000)>>j0; + i0 &= (~i); + } + } + } else { + if(!FLT_UWORD_IS_FINITE(ix)) return x+x; /* inf or NaN */ + else return x; /* x is integral */ + } + SET_FLOAT_WORD(x,i0); + return x; +} + +#ifdef _DOUBLE_IS_32BITS + +#ifdef __STDC__ + double floor(double x) +#else + double floor(x) + double x; +#endif +{ + return (double) floorf((float) x); +} + +#endif /* defined(_DOUBLE_IS_32BITS) */ diff --git a/src/math/sf_scalbn.c b/src/math/sf_scalbn.c new file mode 100644 index 00000000..70006001 --- /dev/null +++ b/src/math/sf_scalbn.c @@ -0,0 +1,86 @@ +/* sf_scalbn.c -- float version of s_scalbn.c. + * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com. + */ + +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +#include "fdlibm.h" +#include <limits.h> +#include <float.h> + +#if INT_MAX > 50000 +#define OVERFLOW_INT 50000 +#else +#define OVERFLOW_INT 30000 +#endif + +#ifdef __STDC__ +static const float +#else +static float +#endif +two25 = 3.355443200e+07, /* 0x4c000000 */ +twom25 = 2.9802322388e-08, /* 0x33000000 */ +huge = 1.0e+30, +tiny = 1.0e-30; + +#ifdef __STDC__ + float scalbnf (float x, int n) +#else + float scalbnf (x,n) + float x; int n; +#endif +{ + __int32_t k,ix; + __uint32_t hx; + + GET_FLOAT_WORD(ix,x); + hx = ix&0x7fffffff; + k = hx>>23; /* extract exponent */ + if (FLT_UWORD_IS_ZERO(hx)) + return x; + if (!FLT_UWORD_IS_FINITE(hx)) + return x+x; /* NaN or Inf */ + if (FLT_UWORD_IS_SUBNORMAL(hx)) { + x *= two25; + GET_FLOAT_WORD(ix,x); + k = ((ix&0x7f800000)>>23) - 25; + if (n< -50000) return tiny*x; /*underflow*/ + } + k = k+n; + if (k > FLT_LARGEST_EXP) return huge*copysignf(huge,x); /* overflow */ + if (k > 0) /* normal result */ + {SET_FLOAT_WORD(x,(ix&0x807fffff)|(k<<23)); return x;} + if (k < FLT_SMALLEST_EXP) { + if (n > OVERFLOW_INT) /* in case integer overflow in n+k */ + return huge*copysignf(huge,x); /*overflow*/ + else return tiny*copysignf(tiny,x); /*underflow*/ + } + k += 25; /* subnormal result */ + SET_FLOAT_WORD(x,(ix&0x807fffff)|(k<<23)); + return x*twom25; +} + +#ifdef _DOUBLE_IS_32BITS + +#ifdef __STDC__ + double scalbn(double x, int n) +#else + double scalbn(x,n) + double x; + int n; +#endif +{ + return (double) scalbnf((float) x, n); +} + +#endif /* defined(_DOUBLE_IS_32BITS) */ diff --git a/src/math/sf_sin.c b/src/math/sf_sin.c new file mode 100644 index 00000000..da81845d --- /dev/null +++ b/src/math/sf_sin.c @@ -0,0 +1,62 @@ +/* sf_sin.c -- float version of s_sin.c. + * Conversion to float by Ian Lance Taylor, Cygnus Support, ian@cygnus.com. + */ + +/* + * ==================================================== + * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved. + * + * Developed at SunPro, a Sun Microsystems, Inc. business. + * Permission to use, copy, modify, and distribute this + * software is freely granted, provided that this notice + * is preserved. + * ==================================================== + */ + +#include "fdlibm.h" + +#ifdef __STDC__ + float sinf(float x) +#else + float sinf(x) + float x; +#endif +{ + float y[2],z=0.0; + __int32_t n,ix; + + GET_FLOAT_WORD(ix,x); + + /* |x| ~< pi/4 */ + ix &= 0x7fffffff; + if(ix <= 0x3f490fd8) return __kernel_sinf(x,z,0); + + /* sin(Inf or NaN) is NaN */ + else if (!FLT_UWORD_IS_FINITE(ix)) return x-x; + + /* argument reduction needed */ + else { + n = __ieee754_rem_pio2f(x,y); + switch(n&3) { + case 0: return __kernel_sinf(y[0],y[1],1); + case 1: return __kernel_cosf(y[0],y[1]); + case 2: return -__kernel_sinf(y[0],y[1],1); + default: + return -__kernel_cosf(y[0],y[1]); + } + } +} + +#ifdef _DOUBLE_IS_32BITS + +#ifdef __STDC__ + double sin(double x) +#else + double sin(x) + double x; +#endif +{ + return (double) sinf((float) x); +} + +#endif /* defined(_DOUBLE_IS_32BITS) */ diff --git a/src/megadongle-v0.1/Makefile b/src/megadongle-v0.1/Makefile index 7f12963f..6035c348 100644 --- a/src/megadongle-v0.1/Makefile +++ b/src/megadongle-v0.1/Makefile @@ -62,14 +62,15 @@ CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STA PROGNAME=megadongle-v0.1 PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx SRC=$(ALTOS_SRC) ao_megadongle.c OBJ=$(SRC:.c=.o) -all: $(PROG) +all: $(PROG) $(HEX) $(PROG): Makefile $(OBJ) altos.ld - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) $(OBJ): $(INC) @@ -79,7 +80,7 @@ ao_product.h: ao-make-product.5c ../Version distclean: clean clean: - rm -f *.o $(PROGNAME)-*.elf + rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx rm -f ao_product.h install: diff --git a/src/micropeak/Makefile b/src/micropeak/Makefile index 315b93f6..dcc32874 100644 --- a/src/micropeak/Makefile +++ b/src/micropeak/Makefile @@ -2,23 +2,19 @@ # Tiny AltOS build # # -vpath % ../attiny:../drivers:../core:.. +vpath % ../attiny:../drivers:../core:../product:.. vpath ao-make-product.5c ../util vpath make-altitude-pa ../util +include ../avr/Makefile.defs + MCU=attiny85 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 -ifndef VERSION -include ../Version -endif +#LDFLAGS=-L$(LDSCRIPTS) -Tavr25.x ALTOS_SRC = \ ao_micropeak.c \ @@ -28,7 +24,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 \ @@ -51,7 +47,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/nanopeak-v0.1/.gitignore b/src/nanopeak-v0.1/.gitignore new file mode 100644 index 00000000..27cd0a7c --- /dev/null +++ b/src/nanopeak-v0.1/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +nanopeak-* diff --git a/src/nanopeak-v0.1/Makefile b/src/nanopeak-v0.1/Makefile new file mode 100644 index 00000000..04eea902 --- /dev/null +++ b/src/nanopeak-v0.1/Makefile @@ -0,0 +1,108 @@ +# +# Tiny AltOS build +# +# +vpath % ../attiny:../drivers:../core:../product:.. +vpath ao-make-product.5c ../util +vpath make-altitude-pa ../util + +include ../avr/Makefile.defs + +MCU=attiny85 +DUDECPUTYPE=t85 +#PROGRAMMER=stk500v2 -P usb +LOADSLOW=-i 32 -B 32 +LOADARG=-p $(DUDECPUTYPE) -c $(PROGRAMMER) -e -U flash:w: + +#LDFLAGS=-L$(LDSCRIPTS) -Tavr25.x + +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_report_micro.c \ + ao_notask.c \ + ao_eeprom_tiny.c \ + ao_panic.c \ + ao_log_micro.c \ + ao_async.c \ + ao_microflight.c \ + ao_microkalman.c + +INC=\ + ao.h \ + ao_pins.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_exti.h \ + ao_ms5607.h \ + ao_log_micro.h \ + ao_micropeak.h \ + altitude-pa.h + +IDPRODUCT=0 +PRODUCT=NanoPeak-v0.1 +PRODUCT_DEF=-DNANOPEAK +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 + +PROG=nanopeak-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 + +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) > $@ + +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/nanopeak-v0.1/ao_pins.h b/src/nanopeak-v0.1/ao_pins.h new file mode 100644 index 00000000..bd4a06d1 --- /dev/null +++ b/src/nanopeak-v0.1/ao_pins.h @@ -0,0 +1,65 @@ +/* + * Copyright © 2011 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ +#include <avr/pgmspace.h> + +#define AO_LED_ORANGE (1<<3) +#define AO_LED_SERIAL 3 +#define AO_LED_PANIC AO_LED_ORANGE +#define AO_LED_REPORT AO_LED_ORANGE +#define LEDS_AVAILABLE (AO_LED_ORANGE) +#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 0 +#define HAS_EEPROM 0 +#define HAS_BEEP 0 +#define AVR_CLOCK 250000UL + +/* SPI */ +#define SPI_PORT PORTB +#define SPI_PIN PINB +#define SPI_DIR DDRB +#define AO_MS5607_CS_PORT PORTB +#define AO_MS5607_CS_PIN 4 + +/* MS5607 */ +#define AO_MS5607_SPI_INDEX 0 +#define AO_MS5607_MISO_PORT PORTB +#define AO_MS5607_MISO_PIN 0 +#define AO_MS5607_BARO_OVERSAMPLE 4096 +#define AO_MS5607_TEMP_OVERSAMPLE 1024 + +/* 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 +typedef int32_t alt_t; +#define FETCH_ALT(o) ((alt_t) pgm_read_dword(&altitude_table[o])) + +#define AO_ALT_VALUE(x) ((x) * (alt_t) 10) + +#endif /* _AO_PINS_H_ */ diff --git a/src/product/Makefile.teledongle b/src/product/Makefile.teledongle index 3101b777..da9bcba0 100644 --- a/src/product/Makefile.teledongle +++ b/src/product/Makefile.teledongle @@ -80,9 +80,9 @@ endif # Otherwise, print the full command line. quiet ?= $($1) -all: ../$(PROG) +all: $(PROG) -../$(PROG): $(REL) Makefile +$(PROG): $(REL) Makefile $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ diff --git a/src/product/Makefile.telelaunch b/src/product/Makefile.telelaunch index 1e55989c..a5e2eb7f 100644 --- a/src/product/Makefile.telelaunch +++ b/src/product/Makefile.telelaunch @@ -82,9 +82,9 @@ endif # Otherwise, print the full command line. quiet ?= $($1) -all: ../$(PROG) +all: $(PROG) -../$(PROG): $(REL) Makefile +$(PROG): $(REL) Makefile $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ diff --git a/src/product/Makefile.telemetrum b/src/product/Makefile.telemetrum index 5e3eed7f..c740a483 100644 --- a/src/product/Makefile.telemetrum +++ b/src/product/Makefile.telemetrum @@ -94,10 +94,10 @@ endif # Otherwise, print the full command line. quiet ?= $($1) -all: ../$(PROG) +all: $(PROG) -../$(PROG): $(REL) Makefile - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. +$(PROG): $(REL) Makefile + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ ao_product.h: ao-make-product.5c ../Version diff --git a/src/product/Makefile.telemini b/src/product/Makefile.telemini index ef8906ba..0884079e 100644 --- a/src/product/Makefile.telemini +++ b/src/product/Makefile.telemini @@ -83,10 +83,10 @@ endif # Otherwise, print the full command line. quiet ?= $($1) -all: ../$(PROG) +all: $(PROG) -../$(PROG): $(REL) Makefile - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. +$(PROG): $(REL) Makefile + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ ao_product.h: ao-make-product.5c ../Version diff --git a/src/product/Makefile.telenano b/src/product/Makefile.telenano index 67410ae0..c31989ee 100644 --- a/src/product/Makefile.telenano +++ b/src/product/Makefile.telenano @@ -82,9 +82,9 @@ endif # Otherwise, print the full command line. quiet ?= $($1) -all: ../$(PROG) +all: $(PROG) -../$(PROG): $(REL) Makefile +$(PROG): $(REL) Makefile $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ diff --git a/src/product/ao_flash_pins.h b/src/product/ao_flash_pins.h index b774df6d..dd67d820 100644 --- a/src/product/ao_flash_pins.h +++ b/src/product/ao_flash_pins.h @@ -35,6 +35,7 @@ #define HAS_VERSION 0 #define AO_BOOT_CHAIN 1 -#define AO_BOOT_PIN 1 + +#define IS_FLASH_LOADER 1 #endif /* _AO_FLASH_PINS_H_ */ diff --git a/src/product/ao_flash_task.c b/src/product/ao_flash_task.c index fdc4d0aa..4cfbf75f 100644 --- a/src/product/ao_flash_task.c +++ b/src/product/ao_flash_task.c @@ -73,7 +73,7 @@ static void ao_block_erase(void) { uint32_t addr = ao_get_hex32(); - uint32_t *p = (uint32_t *) addr; + void *p = (void *) addr; ao_flash_erase_page(p); } @@ -82,11 +82,8 @@ static void ao_block_write(void) { uint32_t addr = ao_get_hex32(); - uint32_t *p = (uint32_t *) addr; - union { - uint8_t data8[256]; - uint32_t data32[64]; - } u; + void *p = (void *) addr; + uint8_t data[256]; uint16_t i; if (addr < (uint32_t) AO_BOOT_APPLICATION_BASE) { @@ -94,8 +91,8 @@ ao_block_write(void) return; } for (i = 0; i < 256; i++) - u.data8[i] = ao_usb_getchar(); - ao_flash_page(p, u.data32); + data[i] = ao_usb_getchar(); + ao_flash_page(p, (void *) data); } static void diff --git a/src/micropeak/ao_micropeak.c b/src/product/ao_micropeak.c index 10f0d192..10f0d192 100644 --- a/src/micropeak/ao_micropeak.c +++ b/src/product/ao_micropeak.c diff --git a/src/micropeak/ao_micropeak.h b/src/product/ao_micropeak.h index 382b98d9..0cefca6f 100644 --- a/src/micropeak/ao_micropeak.h +++ b/src/product/ao_micropeak.h @@ -20,15 +20,15 @@ #define SAMPLE_SLEEP AO_MS_TO_TICKS(96) -/* 16 sample, or about two seconds worth */ -#define GROUND_AVG_SHIFT 4 +/* 64 sample, or about six seconds worth */ +#define GROUND_AVG_SHIFT 6 #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 */ +#define BOOST_DETECT 360 /* 30m at sea level, 36m 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) +#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 */ diff --git a/src/product/ao_terraui.c b/src/product/ao_terraui.c index 1866eb0c..8fd97033 100644 --- a/src/product/ao_terraui.c +++ b/src/product/ao_terraui.c @@ -629,7 +629,7 @@ ao_terragps(void) for (;;) { while (ao_gps_tick == gps_tick) - ao_sleep(&ao_gps_data); + ao_sleep(&ao_gps_new); gps_tick = ao_gps_tick; ao_gps_progress = (ao_gps_progress + 1) & 3; } diff --git a/src/spiradio-v0.1/Makefile b/src/spiradio-v0.1/Makefile index a207d34f..e644bc49 100644 --- a/src/spiradio-v0.1/Makefile +++ b/src/spiradio-v0.1/Makefile @@ -73,10 +73,10 @@ endif # Otherwise, print the full command line. quiet ?= $($1) -all: ../$(PROG) +all: $(PROG) -../$(PROG): $(REL) Makefile - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. +$(PROG): $(REL) Makefile + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ ao_product.h: ao-make-product.5c ../Version diff --git a/src/stm-bringup/Makefile b/src/stm-bringup/Makefile index 1bc5aaad..797df2d6 100644 --- a/src/stm-bringup/Makefile +++ b/src/stm-bringup/Makefile @@ -4,15 +4,15 @@ vpath ao-make-product.5c ../util ifndef VERSION include ../Version endif +TOPDIR=.. +include $(TOPDIR)/Makedefs CC=arm-none-eabi-gcc OBJCOPY=arm-none-eabi-objcopy -PDCLIB=/opt/cortex -C_LIB=$(PDCLIB)/lib/pdclib-cortex-m3.a -C_INC=-I$(PDCLIB)/include +C_LIB=$(PDCLIB_LIBS_M3) -DEF_CFLAGS=-g -std=gnu99 -Os -mlittle-endian -mthumb -ffreestanding -nostdlib -I. -I../../src/stm $(C_INC) +DEF_CFLAGS=-g -std=gnu99 -Os -mlittle-endian -mthumb -ffreestanding -nostdlib -I. -I../stm $(PDCLIB_INCLUDES) # to run from SRAM LD_FLAGS_RAM=-L../stm -Wl,-Taltos-ram.ld @@ -28,10 +28,10 @@ all: bringup-ram.elf bringup.elf %.bin: %.elf $(OBJCOPY) -O binary $^ $@ -bringup.elf: $(OBJ) $(C_LIB) bringup.ld +bringup.elf: $(OBJ) bringup.ld $(CC) $(CFLAGS) $(LD_FLAGS) -o $@ $(OBJ) $(C_LIB) -lgcc -bringup-ram.elf: $(OBJ) $(C_LIB) altos-ram.ld +bringup-ram.elf: $(OBJ) altos-ram.ld $(CC) $(CFLAGS) $(LD_FLAGS_RAM) -o $@ $(OBJ) $(C_LIB) -lgcc clean: diff --git a/src/stm-demo/Makefile b/src/stm-demo/Makefile index d1f825db..98fcd9e5 100644 --- a/src/stm-demo/Makefile +++ b/src/stm-demo/Makefile @@ -36,10 +36,7 @@ ALTOS_SRC = \ ao_data.c \ ao_i2c_stm.c \ ao_usb_stm.c \ - ao_exti_stm.c \ - ao_event.c \ - ao_quadrature.c \ - ao_button.c + ao_exti_stm.c PRODUCT=StmDemo-v0.0 PRODUCT_DEF=-DSTM_DEMO @@ -59,7 +56,7 @@ all: $(ELF) $(IHX) LDFLAGS=-L../stm -Wl,-Taltos.ld $(ELF): Makefile $(OBJ) - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $@ $(OBJ) $(SAT_CLIB) -lgcc + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $@ $(OBJ) $(LIBS) ao_product.h: ao-make-product.5c ../Version $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ @@ -69,7 +66,7 @@ $(OBJ): $(INC) distclean: clean clean: - rm -f *.o $(PROG) + rm -f *.o *.elf *.ihx rm -f ao_product.h install: diff --git a/src/stm-demo/ao_demo.c b/src/stm-demo/ao_demo.c index 5677cdf4..58cf651b 100644 --- a/src/stm-demo/ao_demo.c +++ b/src/stm-demo/ao_demo.c @@ -153,6 +153,7 @@ ao_temp (void) printf ("temp: %d\n", temp); } +#if 0 static void ao_event(void) { @@ -168,6 +169,7 @@ ao_event(void) } } +#endif __code struct ao_cmds ao_demo_cmds[] = { { ao_dma_test, "D\0DMA test" }, @@ -175,7 +177,7 @@ __code struct ao_cmds ao_demo_cmds[] = { { ao_spi_read, "R\0SPI read" }, { ao_i2c_write, "i\0I2C write" }, { ao_temp, "t\0Show temp" }, - { ao_event, "e\0Monitor event queue" }, +/* { ao_event, "e\0Monitor event queue" }, */ { 0, NULL } }; diff --git a/src/stm-flash/Makefile b/src/stm-flash/Makefile index 1ea35581..5c0699e1 100644 --- a/src/stm-flash/Makefile +++ b/src/stm-flash/Makefile @@ -41,7 +41,7 @@ all: $(PROG) LDFLAGS=-L../stm -Wl,-Taltos-loader.ld $(PROG): Makefile $(OBJ) - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) ao_product.h: ao-make-product.5c ../Version $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ diff --git a/src/stm/Makefile-flash.defs b/src/stm/Makefile-flash.defs index 016bb7e7..3890eff1 100644 --- a/src/stm/Makefile-flash.defs +++ b/src/stm/Makefile-flash.defs @@ -6,16 +6,15 @@ vpath ao-make-product.5c $(TOPDIR)/util .elf.ihx: objcopy -O ihex $*.elf $@ -CC=arm-none-eabi-gcc -SAT=/opt/cortex -SAT_CLIB=$(SAT)/lib/pdclib-cortex-m3.a -SAT_CFLAGS=-I$(SAT)/include - ifndef VERSION include $(TOPDIR)/Version endif +include $(TOPDIR)/Makedefs + +CC=$(ARM_CC) +LIBS=$(PDCLIB_LIBS_M3) -lgcc -AO_CFLAGS=-I. -I$(TOPDIR)/stm -I$(TOPDIR)/core -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) +AO_CFLAGS=-I. -I$(TOPDIR)/stm -I$(TOPDIR)/core -I$(TOPDIR)/drivers -I$(TOPDIR)/product -I$(TOPDIR) $(PDCLIB_INCLUDES) STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS) $(SAT_CFLAGS) LDFLAGS=-L$(TOPDIR)/stm -Wl,-Taltos-loader.ld @@ -62,7 +61,7 @@ SRC = \ OBJ=$(SRC:.c=.o) -PRODUCT=AltosFlash-$(VERSION) +PRODUCT=AltosFlash PRODUCT_DEF=-DALTOS_FLASH IDPRODUCT=0x000a @@ -72,7 +71,7 @@ PROGNAME=altos-flash PROG=$(HARDWARE)-$(PROGNAME)-$(VERSION).elf $(PROG): Makefile $(OBJ) altos-loader.ld - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) ao_product.h: ao-make-product.5c $(TOPDIR)/Version $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ @@ -84,7 +83,7 @@ all: $(PROG) distclean: clean clean: - rm -f *.o $(PROG) + rm -f *.o $(HARDWARE)-$(PROGNAME)-*.elf rm -f ao_product.h install: diff --git a/src/stm/Makefile.defs b/src/stm/Makefile.defs index c8bb7d70..9adcfeb3 100644 --- a/src/stm/Makefile.defs +++ b/src/stm/Makefile.defs @@ -1,4 +1,4 @@ -vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:.. +vpath % ../stm:../product:../drivers:../core:../util:../kalman:../aes:../math:.. vpath make-altitude ../util vpath make-kalman ../util vpath kalman.5c ../kalman @@ -10,23 +10,27 @@ vpath ao-make-product.5c ../util .SUFFIXES: .elf .ihx .elf.ihx: - objcopy -O ihex $*.elf $@ + $(ELFTOHEX) --output=$@ $*.elf -CC=arm-none-eabi-gcc -SAT=/opt/cortex -SAT_CLIB=$(SAT)/lib/pdclib-cortex-m3.a -SAT_CFLAGS=-I$(SAT)/include +ifndef TOPDIR +TOPDIR=.. +endif ifndef VERSION -include ../Version +include $(TOPDIR)/Version endif +include $(TOPDIR)/Makedefs + +CC=$(ARM_CC) +LIBS=$(PDCLIB_LIBS_M3) -lgcc -AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I.. -STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS) $(SAT_CFLAGS) +AO_CFLAGS=-I. -I../stm -I../core -I../drivers -I../math -I.. $(PDCLIB_INCLUDES) +STM_CFLAGS=-std=gnu99 -mlittle-endian -mcpu=cortex-m3 -mthumb -ffreestanding -nostdlib $(AO_CFLAGS) LDFLAGS=-L../stm -Wl,-Taltos.ld NICKLE=nickle +ELFTOHEX=$(TOPDIR)/../ao-tools/ao-elftohex/ao-elftohex V=0 # The user has explicitly enabled quiet compilation. diff --git a/src/stm/altos-loader.ld b/src/stm/altos-loader.ld index 2be964f2..0753f5f7 100644 --- a/src/stm/altos-loader.ld +++ b/src/stm/altos-loader.ld @@ -37,11 +37,11 @@ SECTIONS { ao_romconfig.o(.romconfig*) ao_product.o(.romconfig*) - *(.text) /* Executable code */ + *(.text*) /* Executable code */ *(.ARM.exidx* .gnu.linkonce.armexidx.*) *(.rodata*) /* Constants */ - __text_end__ = .; } > rom + __text_end__ = .; /* Boot data which must live at the start of ram so that * the application and bootloader share the same addresses. @@ -66,7 +66,7 @@ SECTIONS { .textram BLOCK(8): { __data_start__ = .; __text_ram_start__ = .; - *(.text.ram) + *(.ramtext) __text_ram_end = .; } >ram AT>rom diff --git a/src/stm/ao_adc_stm.c b/src/stm/ao_adc_stm.c index 48fc4262..53f19b40 100644 --- a/src/stm/ao_adc_stm.c +++ b/src/stm/ao_adc_stm.c @@ -113,11 +113,15 @@ ao_adc_dump(void) __reentrant uint8_t i; ao_data_get(&packet); +#ifdef AO_ADC_DUMP + AO_ADC_DUMP(&packet); +#else printf("tick: %5u", packet.tick); d = (int16_t *) (&packet.adc); for (i = 0; i < AO_NUM_ADC; i++) printf (" %2d: %5d", i, d[i]); printf("\n"); +#endif } __code struct ao_cmds ao_adc_cmds[] = { diff --git a/src/stm/ao_arch.h b/src/stm/ao_arch.h index adc288c3..42fe727a 100644 --- a/src/stm/ao_arch.h +++ b/src/stm/ao_arch.h @@ -34,6 +34,8 @@ #define AO_TICK_SIGNED int16_t #endif +#define AO_PORT_TYPE uint16_t + /* Various definitions to make GCC look more like SDCC */ #define ao_arch_naked_declare __attribute__((naked)) diff --git a/src/stm/ao_arch_funcs.h b/src/stm/ao_arch_funcs.h index 1e78cabc..b461cd3f 100644 --- a/src/stm/ao_arch_funcs.h +++ b/src/stm/ao_arch_funcs.h @@ -87,13 +87,16 @@ extern uint16_t ao_spi_speed[STM_NUM_SPI]; void ao_spi_init(void); +#define ao_spi_set_cs(reg,mask) ((reg)->bsrr = ((uint32_t) (mask)) << 16) +#define ao_spi_clr_cs(reg,mask) ((reg)->bsrr = (mask)) + #define ao_spi_get_mask(reg,mask,bus, speed) do { \ ao_spi_get(bus, speed); \ - (reg)->bsrr = ((uint32_t) mask) << 16; \ + ao_spi_set_cs(reg,mask); \ } while (0) #define ao_spi_put_mask(reg,mask,bus) do { \ - (reg)->bsrr = mask; \ + ao_spi_clr_cs(reg,mask); \ ao_spi_put(bus); \ } while (0) @@ -326,7 +329,7 @@ static inline void ao_arch_restore_stack(void) { /* Restore APSR */ asm("pop {r0}"); - asm("msr apsr,r0"); + asm("msr apsr_nczvq,r0"); /* Restore general registers */ asm("pop {r0-r12,lr}\n"); @@ -335,6 +338,11 @@ static inline void ao_arch_restore_stack(void) { asm("bx lr"); } +#ifndef HAS_SAMPLE_PROFILE +#define HAS_SAMPLE_PROFILE 0 +#endif + +#if !HAS_SAMPLE_PROFILE #define HAS_ARCH_START_SCHEDULER 1 static inline void ao_arch_start_scheduler(void) { @@ -346,16 +354,19 @@ static inline void ao_arch_start_scheduler(void) { asm("mrs %0,control" : "=&r" (control)); control |= (1 << 1); asm("msr control,%0" : : "r" (control)); + asm("isb"); } +#endif #define ao_arch_isr_stack() #endif -#define ao_arch_wait_interrupt() do { \ - asm(".global ao_idle_loc\n\twfi\nao_idle_loc:"); \ - ao_arch_release_interrupts(); \ - ao_arch_block_interrupts(); \ +#define ao_arch_wait_interrupt() do { \ + asm("\twfi\n"); \ + ao_arch_release_interrupts(); \ + asm(".global ao_idle_loc\nao_idle_loc:"); \ + ao_arch_block_interrupts(); \ } while (0) #define ao_arch_critical(b) do { \ diff --git a/src/stm/ao_beep_stm.c b/src/stm/ao_beep_stm.c index 4761fbfc..a95d869b 100644 --- a/src/stm/ao_beep_stm.c +++ b/src/stm/ao_beep_stm.c @@ -17,6 +17,10 @@ #include "ao.h" +#ifndef BEEPER_CHANNEL +#define BEEPER_CHANNEL 1 +#endif + void ao_beep(uint8_t beep) { @@ -56,6 +60,7 @@ ao_beep(uint8_t beep) * is enabled and active high. */ +#if BEEPER_CHANNEL == 1 stm_tim3.ccmr1 = ((0 << STM_TIM234_CCMR1_OC2CE) | (STM_TIM234_CCMR1_OC2M_FROZEN << STM_TIM234_CCMR1_OC2M) | (0 << STM_TIM234_CCMR1_OC2PE) | @@ -68,7 +73,6 @@ ao_beep(uint8_t beep) (0 << STM_TIM234_CCMR1_OC1FE) | (STM_TIM234_CCMR1_CC1S_OUTPUT << STM_TIM234_CCMR1_CC1S)); - stm_tim3.ccer = ((0 << STM_TIM234_CCER_CC4NP) | (0 << STM_TIM234_CCER_CC4P) | (0 << STM_TIM234_CCER_CC4E) | @@ -81,6 +85,33 @@ ao_beep(uint8_t beep) (0 << STM_TIM234_CCER_CC1NP) | (0 << STM_TIM234_CCER_CC1P) | (1 << STM_TIM234_CCER_CC1E)); +#endif +#if BEEPER_CHANNEL == 4 + stm_tim3.ccmr2 = ((0 << STM_TIM234_CCMR2_OC4CE) | + (STM_TIM234_CCMR2_OC4M_TOGGLE << STM_TIM234_CCMR2_OC4M) | + (0 << STM_TIM234_CCMR2_OC4PE) | + (0 << STM_TIM234_CCMR2_OC4FE) | + (STM_TIM234_CCMR2_CC4S_OUTPUT << STM_TIM234_CCMR2_CC4S) | + + (0 << STM_TIM234_CCMR2_OC3CE) | + (STM_TIM234_CCMR2_OC3M_FROZEN << STM_TIM234_CCMR2_OC3M) | + (0 << STM_TIM234_CCMR2_OC3PE) | + (0 << STM_TIM234_CCMR2_OC3FE) | + (STM_TIM234_CCMR2_CC3S_OUTPUT << STM_TIM234_CCMR2_CC3S)); + + stm_tim3.ccer = ((0 << STM_TIM234_CCER_CC4NP) | + (0 << STM_TIM234_CCER_CC4P) | + (1 << STM_TIM234_CCER_CC4E) | + (0 << STM_TIM234_CCER_CC3NP) | + (0 << STM_TIM234_CCER_CC3P) | + (0 << STM_TIM234_CCER_CC3E) | + (0 << STM_TIM234_CCER_CC2NP) | + (0 << STM_TIM234_CCER_CC2P) | + (0 << STM_TIM234_CCER_CC2E) | + (0 << STM_TIM234_CCER_CC1NP) | + (0 << STM_TIM234_CCER_CC1P) | + (0 << STM_TIM234_CCER_CC1E)); +#endif /* 5. Enable the counter by setting the CEN bit in the TIMx_CR1 register. */ @@ -110,13 +141,22 @@ ao_beep_for(uint8_t beep, uint16_t ticks) __reentrant void ao_beep_init(void) { - /* Our beeper is on PC6, which is hooked to TIM3_CH1, - * which is on PC6 - */ +#if BEEPER_CHANNEL == 1 + /* Our beeper is on PC6, which is hooked to TIM3_CH1. + */ stm_rcc.ahbenr |= (1 << STM_RCC_AHBENR_GPIOCEN); stm_afr_set(&stm_gpioc, 6, STM_AFR_AF2); +#endif +#if BEEPER_CHANNEL == 4 + + /* Our beeper is on PB1, which is hooked to TIM3_CH4. + */ + stm_rcc.ahbenr |= (1 << STM_RCC_AHBENR_GPIOBEN); + + stm_afr_set(&stm_gpiob, 1, STM_AFR_AF2); +#endif /* Leave the timer off until requested */ diff --git a/src/stm/ao_eeprom_stm.c b/src/stm/ao_eeprom_stm.c index 58783f1a..4207a860 100644 --- a/src/stm/ao_eeprom_stm.c +++ b/src/stm/ao_eeprom_stm.c @@ -16,19 +16,10 @@ */ #include <ao.h> -#include <ao_storage.h> +#include <ao_eeprom.h> /* Total bytes of available storage */ -ao_pos_t ao_storage_total = 4096; - -/* Block size - device is erased in these units. */ -ao_pos_t ao_storage_block = 1024; - -/* Byte offset of config block. Will be ao_storage_block bytes long */ -ao_pos_t ao_storage_config = 0; - -/* Storage unit size - device reads and writes must be within blocks of this size. */ -uint16_t ao_storage_unit = 1024; +const ao_pos_t ao_eeprom_total = 4096; /* Location of eeprom in address space */ #define stm_eeprom ((uint8_t *) 0x08080000) @@ -42,16 +33,6 @@ uint16_t ao_storage_unit = 1024; * the same contents, or append to an existing page easily enough */ -/* - * Erase the specified sector - */ -uint8_t -ao_storage_erase(ao_pos_t pos) __reentrant -{ - /* Not necessary */ - return 1; -} - static void ao_intflash_unlock(void) { @@ -131,16 +112,16 @@ ao_intflash_read(uint16_t pos) } /* - * Write to flash + * Write to eeprom */ uint8_t -ao_storage_device_write(ao_pos_t pos32, __xdata void *v, uint16_t len) __reentrant +ao_eeprom_write(ao_pos_t pos32, __xdata void *v, uint16_t len) { uint16_t pos = pos32; __xdata uint8_t *d = v; - if (pos >= ao_storage_total || pos + len > ao_storage_total) + if (pos >= ao_eeprom_total || pos + len > ao_eeprom_total) return 0; ao_intflash_unlock(); @@ -166,38 +147,26 @@ ao_storage_device_write(ao_pos_t pos32, __xdata void *v, uint16_t len) __reentra } /* - * Read from flash + * Read from eeprom */ uint8_t -ao_storage_device_read(ao_pos_t pos, __xdata void *v, uint16_t len) __reentrant +ao_eeprom_read(ao_pos_t pos, __xdata void *v, uint16_t len) { uint8_t *d = v; - if (pos >= ao_storage_total || pos + len > ao_storage_total) + if (pos >= ao_eeprom_total || pos + len > ao_eeprom_total) return 0; while (len--) *d++ = ao_intflash_read(pos++); return 1; } -void -ao_storage_flush(void) __reentrant -{ -} - -void -ao_storage_setup(void) -{ -} - -void -ao_storage_device_info(void) __reentrant -{ - uint8_t i; - printf ("Using internal flash\n"); -} +/* + * Initialize eeprom + */ void -ao_storage_device_init(void) +ao_eeprom_init(void) { + /* Nothing to do here */ } diff --git a/src/stm/ao_exti.h b/src/stm/ao_exti.h index 35b56b57..ebea224d 100644 --- a/src/stm/ao_exti.h +++ b/src/stm/ao_exti.h @@ -25,6 +25,7 @@ #define AO_EXTI_PRIORITY_LOW 16 #define AO_EXTI_PRIORITY_MED 0 #define AO_EXTI_PRIORITY_HIGH 32 +#define AO_EXTI_PIN_NOCONFIGURE 64 void ao_exti_setup(struct stm_gpio *gpio, uint8_t pin, uint8_t mode, void (*callback)()); diff --git a/src/stm/ao_exti_stm.c b/src/stm/ao_exti_stm.c index 1361d0d4..c1dcdf85 100644 --- a/src/stm/ao_exti_stm.c +++ b/src/stm/ao_exti_stm.c @@ -70,21 +70,23 @@ ao_exti_setup (struct stm_gpio *gpio, uint8_t pin, uint8_t mode, void (*callback /* configure gpio to interrupt routing */ stm_exticr_set(gpio, pin); - /* configure pin as input, setting selected pull-up/down mode */ - stm_moder_set(gpio, pin, STM_MODER_INPUT); - switch (mode & (AO_EXTI_MODE_PULL_UP|AO_EXTI_MODE_PULL_DOWN)) { - case 0: - default: - pupdr = STM_PUPDR_NONE; - break; - case AO_EXTI_MODE_PULL_UP: - pupdr = STM_PUPDR_PULL_UP; - break; - case AO_EXTI_MODE_PULL_DOWN: - pupdr = STM_PUPDR_PULL_DOWN; - break; + if (!(mode & AO_EXTI_PIN_NOCONFIGURE)) { + /* configure pin as input, setting selected pull-up/down mode */ + stm_moder_set(gpio, pin, STM_MODER_INPUT); + switch (mode & (AO_EXTI_MODE_PULL_UP|AO_EXTI_MODE_PULL_DOWN)) { + case 0: + default: + pupdr = STM_PUPDR_NONE; + break; + case AO_EXTI_MODE_PULL_UP: + pupdr = STM_PUPDR_PULL_UP; + break; + case AO_EXTI_MODE_PULL_DOWN: + pupdr = STM_PUPDR_PULL_DOWN; + break; + } + stm_pupdr_set(gpio, pin, pupdr); } - stm_pupdr_set(gpio, pin, pupdr); /* Set interrupt mask and rising/falling mode */ stm_exti.imr &= ~mask; diff --git a/src/stm/ao_fast_timer.c b/src/stm/ao_fast_timer.c new file mode 100644 index 00000000..d61b40c9 --- /dev/null +++ b/src/stm/ao_fast_timer.c @@ -0,0 +1,120 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ +#include <ao.h> +#include <ao_fast_timer.h> + +static void (*ao_fast_timer_callback[AO_FAST_TIMER_MAX])(void); +static uint8_t ao_fast_timer_count; +static uint8_t ao_fast_timer_users; + +static void +ao_fast_timer_enable(void) +{ + stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | + (0 << STM_TIM67_CR1_OPM) | + (1 << STM_TIM67_CR1_URS) | + (0 << STM_TIM67_CR1_UDIS) | + (1 << STM_TIM67_CR1_CEN)); +} + +static void +ao_fast_timer_disable(void) +{ + stm_tim6.cr1 = ((0 << STM_TIM67_CR1_ARPE) | + (0 << STM_TIM67_CR1_OPM) | + (1 << STM_TIM67_CR1_URS) | + (0 << STM_TIM67_CR1_UDIS) | + (0 << STM_TIM67_CR1_CEN)); +} + +void +ao_fast_timer_on(void (*callback)(void)) +{ + ao_fast_timer_callback[ao_fast_timer_count] = callback; + if (!ao_fast_timer_count++) + ao_fast_timer_enable(); +} + +void +ao_fast_timer_off(void (*callback)(void)) +{ + uint8_t n; + + for (n = 0; n < ao_fast_timer_count; n++) + if (ao_fast_timer_callback[n] == callback) { + for (; n < ao_fast_timer_count-1; n++) { + ao_fast_timer_callback[n] = ao_fast_timer_callback[n+1]; + } + if (!--ao_fast_timer_count) + ao_fast_timer_disable(); + break; + } +} + +void stm_tim6_isr(void) +{ + uint8_t i; + if (stm_tim6.sr & (1 << STM_TIM67_SR_UIF)) { + stm_tim6.sr = 0; + + for (i = 0; i < ao_fast_timer_count; i++) + (*ao_fast_timer_callback[i])(); + } +} + +/* + * According to the STM clock-configuration, timers run + * twice as fast as the APB1 clock *if* the APB1 prescaler + * is greater than 1. + */ + +#if AO_APB1_PRESCALER > 1 +#define TIMER_23467_SCALER 2 +#else +#define TIMER_23467_SCALER 1 +#endif + +#define TIMER_10kHz ((AO_PCLK1 * TIMER_23467_SCALER) / 10000) + +void +ao_fast_timer_init(void) +{ + if (!ao_fast_timer_users) { + stm_nvic_set_enable(STM_ISR_TIM6_POS); + stm_nvic_set_priority(STM_ISR_TIM6_POS, AO_STM_NVIC_CLOCK_PRIORITY); + + /* Turn on timer 6 */ + stm_rcc.apb1enr |= (1 << STM_RCC_APB1ENR_TIM6EN); + + stm_tim6.psc = TIMER_10kHz; + stm_tim6.arr = 9; + stm_tim6.cnt = 0; + + /* Enable update interrupt */ + stm_tim6.dier = (1 << STM_TIM67_DIER_UIE); + + /* Poke timer to reload values */ + stm_tim6.egr |= (1 << STM_TIM67_EGR_UG); + + stm_tim6.cr2 = (STM_TIM67_CR2_MMS_RESET << STM_TIM67_CR2_MMS); + ao_fast_timer_disable(); + } + if (ao_fast_timer_users == AO_FAST_TIMER_MAX) + ao_panic(AO_PANIC_FAST_TIMER); + ao_fast_timer_users++; +} + diff --git a/src/stm/ao_fast_timer.h b/src/stm/ao_fast_timer.h new file mode 100644 index 00000000..90fb3930 --- /dev/null +++ b/src/stm/ao_fast_timer.h @@ -0,0 +1,34 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_FAST_TIMER_H_ +#define _AO_FAST_TIMER_H_ + +void +ao_fast_timer_init(void); + +#ifndef AO_FAST_TIMER_MAX +#define AO_FAST_TIMER_MAX 2 +#endif + +void +ao_fast_timer_on(void (*callback)(void)); + +void +ao_fast_timer_off(void (*callback)(void)); + +#endif /* _AO_FAST_TIMER_H_ */ diff --git a/src/stm/ao_flash_stm.c b/src/stm/ao_flash_stm.c index d7a85582..38b1c2d8 100644 --- a/src/stm/ao_flash_stm.c +++ b/src/stm/ao_flash_stm.c @@ -69,7 +69,7 @@ ao_flash_wait_bsy(void) ; } -static void __attribute__ ((section(".text.ram"),noinline)) +static void __attribute__ ((section(".ramtext"),noinline)) _ao_flash_erase_page(uint32_t *page) { stm_flash.pecr |= (1 << STM_FLASH_PECR_ERASE) | (1 << STM_FLASH_PECR_PROG); @@ -91,7 +91,7 @@ ao_flash_erase_page(uint32_t *page) ao_flash_lock(); } -static void __attribute__ ((section(".text.ram"), noinline)) +static void __attribute__ ((section(".ramtext"), noinline)) _ao_flash_half_page(uint32_t *dst, uint32_t *src) { uint8_t i; diff --git a/src/stm/ao_timer.c b/src/stm/ao_timer.c index daf2f400..34f9edb9 100644 --- a/src/stm/ao_timer.c +++ b/src/stm/ao_timer.c @@ -67,20 +67,6 @@ ao_timer_set_adc_interval(uint8_t interval) } #endif -/* - * According to the STM clock-configuration, timers run - * twice as fast as the APB1 clock *if* the APB1 prescaler - * is greater than 1. - */ - -#if AO_APB1_PRESCALER > 1 -#define TIMER_23467_SCALER 2 -#else -#define TIMER_23467_SCALER 1 -#endif - -#define TIMER_10kHz ((AO_PCLK1 * TIMER_23467_SCALER) / 10000) - #define SYSTICK_RELOAD (AO_SYSTICK / 100 - 1) void @@ -104,7 +90,15 @@ ao_clock_init(void) /* Switch to MSI while messing about */ stm_rcc.cr |= (1 << STM_RCC_CR_MSION); while (!(stm_rcc.cr & (1 << STM_RCC_CR_MSIRDY))) - asm("nop"); + ao_arch_nop(); + + stm_rcc.cfgr = (stm_rcc.cfgr & ~(STM_RCC_CFGR_SW_MASK << STM_RCC_CFGR_SW)) | + (STM_RCC_CFGR_SW_MSI << STM_RCC_CFGR_SW); + + /* wait for system to switch to MSI */ + while ((stm_rcc.cfgr & (STM_RCC_CFGR_SWS_MASK << STM_RCC_CFGR_SWS)) != + (STM_RCC_CFGR_SWS_MSI << STM_RCC_CFGR_SWS)) + ao_arch_nop(); /* reset SW, HPRE, PPRE1, PPRE2, MCOSEL and MCOPRE */ stm_rcc.cfgr &= (uint32_t)0x88FFC00C; @@ -155,7 +149,6 @@ ao_clock_init(void) stm_flash.acr |= (1 << STM_FLASH_ACR_PRFEN); /* Enable 1 wait state so the CPU can run at 32MHz */ - /* (haven't managed to run the CPU at 32MHz yet, it's at 16MHz) */ stm_flash.acr |= (1 << STM_FLASH_ACR_LATENCY); /* Enable power interface clock */ diff --git a/src/stm/ao_usb_stm.c b/src/stm/ao_usb_stm.c index 11dde92e..28a9f9f3 100644 --- a/src/stm/ao_usb_stm.c +++ b/src/stm/ao_usb_stm.c @@ -572,7 +572,7 @@ ao_usb_ep0_out_set(uint8_t *data, uint8_t len) } static void -ao_usb_ep0_in_start(uint8_t max) +ao_usb_ep0_in_start(uint16_t max) { /* Don't send more than asked for */ if (ao_usb_ep0_in_len > max) @@ -965,7 +965,7 @@ ao_usb_disable(void) stm_usb.cntr = (1 << STM_USB_CNTR_PDWN) | (1 << STM_USB_CNTR_FRES); /* Disable the interface */ - stm_rcc.apb1enr &+ ~(1 << STM_RCC_APB1ENR_USBEN); + stm_rcc.apb1enr &= ~(1 << STM_RCC_APB1ENR_USBEN); ao_arch_release_interrupts(); } diff --git a/src/stm/stm32l.h b/src/stm/stm32l.h index 63bde0f8..ff3f5336 100644 --- a/src/stm/stm32l.h +++ b/src/stm/stm32l.h @@ -171,6 +171,11 @@ stm_gpio_get(struct stm_gpio *gpio, int pin) { return (gpio->idr >> pin) & 1; } +static inline uint16_t +stm_gpio_get_all(struct stm_gpio *gpio) { + return gpio->idr; +} + extern struct stm_gpio stm_gpioa; extern struct stm_gpio stm_gpiob; extern struct stm_gpio stm_gpioc; @@ -1734,7 +1739,7 @@ extern struct stm_tim234 stm_tim2, stm_tim3, stm_tim4; #define STM_TIM234_CCMR1_CC1S_INPUT_TRC 3 #define STM_TIM234_CCMR1_CC1S_MASK 3 -#define STM_TIM234_CCMR2_OC2CE 15 +#define STM_TIM234_CCMR2_OC4CE 15 #define STM_TIM234_CCMR2_OC4M 12 #define STM_TIM234_CCMR2_OC4M_FROZEN 0 #define STM_TIM234_CCMR2_OC4M_SET_HIGH_ON_MATCH 1 diff --git a/src/teleballoon-v1.1/Makefile b/src/teleballoon-v1.1/Makefile index 2eea996e..6ff076a9 100644 --- a/src/teleballoon-v1.1/Makefile +++ b/src/teleballoon-v1.1/Makefile @@ -103,7 +103,7 @@ quiet ?= $($1) all: $(PROG) $(PROG): $(REL) Makefile - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ ao_product.h: ao-make-product.5c ../Version diff --git a/src/teleballoon-v1.1/ao_balloon.c b/src/teleballoon-v1.1/ao_balloon.c index e8972655..12752d1f 100644 --- a/src/teleballoon-v1.1/ao_balloon.c +++ b/src/teleballoon-v1.1/ao_balloon.c @@ -105,8 +105,8 @@ ao_flight(void) #if HAS_GPS /* Record current GPS position by waking up GPS log tasks */ - ao_wakeup(&ao_gps_data); - ao_wakeup(&ao_gps_tracking_data); + ao_gps_new = AO_GPS_NEW_DATA | AO_GPS_NEW_TRACKING; + ao_wakeup(&ao_gps_new); #endif ao_wakeup(DATA_TO_XDATA(&ao_flight_state)); diff --git a/src/telebt-v1.0/Makefile b/src/telebt-v1.0/Makefile index 911a8b09..40853fc3 100644 --- a/src/telebt-v1.0/Makefile +++ b/src/telebt-v1.0/Makefile @@ -82,7 +82,7 @@ quiet ?= $($1) all: $(PROG) $(PROG): $(REL) Makefile - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ ao_product.h: ao-make-product.5c ../Version diff --git a/src/telefire-v0.1/Makefile b/src/telefire-v0.1/Makefile index 712b2e8b..f9e11698 100644 --- a/src/telefire-v0.1/Makefile +++ b/src/telefire-v0.1/Makefile @@ -84,10 +84,10 @@ endif # Otherwise, print the full command line. quiet ?= $($1) -all: ../$(PROG) +all: $(PROG) -../$(PROG): $(REL) Makefile - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. +$(PROG): $(REL) Makefile + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ ao_product.h: ao-make-product.5c ../Version diff --git a/src/telefire-v0.1/ao_pins.h b/src/telefire-v0.1/ao_pins.h index 774d59f4..f7a3ff2c 100644 --- a/src/telefire-v0.1/ao_pins.h +++ b/src/telefire-v0.1/ao_pins.h @@ -40,13 +40,10 @@ #define AO_LED_CONTINUITY(c) (1 << ((c) + 2)) #define AO_LED_CONTINUITY_MASK (0xc) -#define AO_LED_RX 0x10 -#define AO_LED_TX 0x20 -#define AO_LED_ARMED 0x40 -#define AO_LED_POWER 0x80 - -#define AO_LED_RED AO_LED_TX -#define AO_LED_GREEN AO_LED_RX +#define AO_LED_ARMED 0x10 +#define AO_LED_RED 0x20 +#define AO_LED_AMBER 0x40 +#define AO_LED_GREEN 0x80 #define LEDS_AVAILABLE (0xff) #define HAS_EXTERNAL_TEMP 0 diff --git a/src/telefire-v0.2/.gitignore b/src/telefire-v0.2/.gitignore new file mode 100644 index 00000000..4d4f4200 --- /dev/null +++ b/src/telefire-v0.2/.gitignore @@ -0,0 +1,2 @@ +telefire-* +ao_product.h diff --git a/src/telefire-v0.2/.sdcdbrc b/src/telefire-v0.2/.sdcdbrc new file mode 100644 index 00000000..b9f6129c --- /dev/null +++ b/src/telefire-v0.2/.sdcdbrc @@ -0,0 +1,2 @@ +--directory=../cc1111:../product:../core:../drivers:. + diff --git a/src/telefire-v0.2/Makefile b/src/telefire-v0.2/Makefile new file mode 100644 index 00000000..a820990a --- /dev/null +++ b/src/telefire-v0.2/Makefile @@ -0,0 +1,103 @@ +# +# TeleFire build file +# + +TELEFIRE_VER=0.2 +TELEFIRE_DEF=0_2 + +vpath %.c ..:../core:../cc1111:../drivers:../product +vpath %.h ..:../core:../cc1111:../drivers:../product +vpath ao-make-product.5c ../util + +ifndef VERSION +include ../Version +endif + +INC = \ + ao.h \ + ao_pins.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_pad.h \ + cc1111.h \ + ao_product.h + +CORE_SRC = \ + ao_cmd.c \ + ao_config.c \ + ao_convert.c \ + ao_mutex.c \ + ao_panic.c \ + ao_stdio.c \ + ao_storage.c \ + ao_task.c \ + ao_freq.c + +CC1111_SRC = \ + ao_adc.c \ + ao_aes.c \ + ao_beep.c \ + ao_dma.c \ + ao_intflash.c \ + ao_radio.c \ + ao_radio_cmac.c \ + ao_romconfig.c \ + ao_serial.c \ + ao_spi.c \ + ao_string.c \ + ao_timer.c \ + ao_usb.c \ + _bp.c + +DRIVER_SRC = \ + ao_pca9922.c \ + ao_74hc165.c \ + ao_pad.c \ + ao_radio_cmac_cmd.c + +PRODUCT_SRC = \ + ao_telefire.c + +SRC = \ + $(CORE_SRC) \ + $(CC1111_SRC) \ + $(DRIVER_SRC) \ + $(PRODUCT_SRC) + +PROGNAME = telefire-v$(TELEFIRE_VER) +PROG = $(PROGNAME)-$(VERSION).ihx +PRODUCT=TeleFire-v$(TELEFIRE_VER) +PRODUCT_DEF=-DTELEFIRE_V_$(TELEFIRE_DEF) +IDPRODUCT=0x000f +CODESIZE=0x6700 + +include ../cc1111/Makefile.cc1111 + +NICKLE=nickle +CHECK_STACK=sh ../util/check-stack + +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): $(REL) Makefile + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) + $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ + +ao_product.h: ao-make-product.5c ../Version + $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ + +distclean: clean + +clean: clean-cc1111 + +install: + +uninstall: + diff --git a/src/telefire-v0.2/ao_pins.h b/src/telefire-v0.2/ao_pins.h new file mode 100644 index 00000000..96e6b066 --- /dev/null +++ b/src/telefire-v0.2/ao_pins.h @@ -0,0 +1,141 @@ +/* + * Copyright © 2010 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +#define HAS_RADIO 1 + +#define HAS_FLIGHT 0 +#define HAS_USB 1 +#define HAS_BEEP 0 +#define HAS_GPS 0 +#define HAS_SERIAL_1 0 +#define HAS_ADC 1 +#define HAS_DBG 0 +#define HAS_EEPROM 1 +#define HAS_LOG 0 +#define HAS_PAD 1 +#define USE_INTERNAL_FLASH 1 +#define DBG_ON_P1 0 +#define IGNITE_ON_P2 0 +#define IGNITE_ON_P1 1 +#define IGNITE_ON_P0 0 +#define PACKET_HAS_MASTER 0 +#define PACKET_HAS_SLAVE 0 + +#define AO_LED_CONTINUITY(c) (1 << (c)) +#define AO_LED_CONTINUITY_MASK (0xf) +#define AO_LED_ARMED 0x10 +#define AO_LED_RED 0x20 +#define AO_LED_AMBER 0x40 +#define AO_LED_GREEN 0x80 + +#define LEDS_AVAILABLE (0xff) +#define HAS_EXTERNAL_TEMP 0 +#define HAS_ACCEL_REF 0 +#define SPI_CS_ON_P1 1 +#define HAS_AES 1 +#define DMA_SHARE_AES_RADIO 1 + +#define SPI_CS_PORT P1 +#define SPI_CS_SEL P1SEL +#define SPI_CS_DIR P1DIR + +#define SPI_CONST 0x00 + +#define HAS_SPI_0 0 +#define HAS_SPI_1 1 +#define SPI_1_ALT_1 0 +#define SPI_1_ALT_2 1 + +#define HAS_74HC165 1 +#define AO_74HC165_CS_PORT P1 +#define AO_74HC165_CS_PIN 4 +#define AO_74HC165_CS P1_4 + +#define AO_PCA9922_CS_PORT P2 +#define AO_PCA9922_CS_PIN 0 +#define AO_PCA9922_CS P2_0 + +#define AO_PAD_NUM 4 +#define AO_PAD_PORT P1 +#define AO_PAD_DIR P1DIR + +#define AO_PAD_PIN_0 0 +#define AO_PAD_0 P1_0 +#define AO_PAD_ADC_0 0 + +#define AO_PAD_PIN_1 1 +#define AO_PAD_1 P1_1 +#define AO_PAD_ADC_1 1 + +#define AO_PAD_PIN_2 2 +#define AO_PAD_2 P1_2 +#define AO_PAD_ADC_2 2 + +#define AO_PAD_PIN_3 3 +#define AO_PAD_3 P1_3 +#define AO_PAD_ADC_3 3 + +#define AO_PAD_ALL_PINS ((1 << AO_PAD_PIN_0) | (1 << AO_PAD_PIN_1) | (1 << AO_PAD_PIN_2) | (1 << AO_PAD_PIN_3)) +#define AO_PAD_ALL_CHANNELS ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3)) + +#define AO_SIREN_PORT P2 +#define AO_SIREN_DIR P2DIR +#define AO_SIREN_PIN 3 +#define AO_SIREN P2_3 + +#define AO_STROBE_PORT P2 +#define AO_STROBE_DIR P2DIR +#define AO_STROBE_PIN 4 +#define AO_STROBE P2_4 + +/* test these values with real igniters */ +#define AO_PAD_RELAY_CLOSED 3524 +#define AO_PAD_NO_IGNITER 16904 +#define AO_PAD_GOOD_IGNITER 22514 + +#define AO_PAD_ADC_PYRO 4 +#define AO_PAD_ADC_BATT 5 + +#define AO_ADC_FIRST_PIN 0 + +struct ao_adc { + int16_t sense[AO_PAD_NUM]; + int16_t pyro; + int16_t batt; +}; + +#define AO_ADC_DUMP(p) \ + printf ("tick: %5u 0: %5d 1: %5d 2: %5d 3: %5d pyro: %5d batt %5d\n", \ + (p)->tick, \ + (p)->adc.sense[0], \ + (p)->adc.sense[1], \ + (p)->adc.sense[2], \ + (p)->adc.sense[3], \ + (p)->adc.pyro, \ + (p)->adc.batt) + +#define AO_ADC_PINS ((1 << AO_PAD_ADC_0) | \ + (1 << AO_PAD_ADC_1) | \ + (1 << AO_PAD_ADC_2) | \ + (1 << AO_PAD_ADC_3) | \ + (1 << AO_PAD_ADC_PYRO) | \ + (1 << AO_PAD_ADC_BATT)) + +#endif /* _AO_PINS_H_ */ diff --git a/src/telefire-v0.2/ao_telefire.c b/src/telefire-v0.2/ao_telefire.c new file mode 100644 index 00000000..f27ca5e8 --- /dev/null +++ b/src/telefire-v0.2/ao_telefire.c @@ -0,0 +1,45 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_pad.h> +#include <ao_74hc165.h> +#include <ao_radio_cmac_cmd.h> + +void +main(void) +{ + ao_clock_init(); + + ao_led_init(LEDS_AVAILABLE); + + ao_task_init(); + + ao_timer_init(); + ao_adc_init(); + ao_cmd_init(); + ao_spi_init(); + ao_74hc165_init(); + ao_storage_init(); + ao_usb_init(); + ao_radio_init(); + ao_aes_init(); + ao_pad_init(); +// ao_radio_cmac_cmd_init(); + ao_config_init(); + ao_start_scheduler(); +} diff --git a/src/telegps-v0.1/Makefile b/src/telegps-v0.1/Makefile index 2c41235b..49e325ac 100644 --- a/src/telegps-v0.1/Makefile +++ b/src/telegps-v0.1/Makefile @@ -52,11 +52,11 @@ ALTOS_SRC = \ ao_exti_stm.c \ ao_serial_stm.c \ ao_gps_skytraq.c \ + ao_gps_show.c \ ao_cc115l.c \ ao_fec_tx.c \ ao_rfpa0133.c \ ao_aprs.c \ - ao_storage.c \ ao_eeprom_stm.c \ ao_sdcard.c \ ao_bufio.c \ @@ -74,14 +74,15 @@ CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STA PROGNAME=telegps-v0.1 PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx SRC=$(ALTOS_SRC) ao_telegps.c OBJ=$(SRC:.c=.o) -all: $(PROG) +all: $(PROG) $(HEX) $(PROG): Makefile $(OBJ) altos.ld - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) $(OBJ): $(INC) @@ -91,7 +92,7 @@ ao_product.h: ao-make-product.5c ../Version distclean: clean clean: - rm -f *.o $(PROGNAME)-*.elf + rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx rm -f ao_product.h install: diff --git a/src/telegps-v0.1/ao_pins.h b/src/telegps-v0.1/ao_pins.h index 5bea2681..7ff59956 100644 --- a/src/telegps-v0.1/ao_pins.h +++ b/src/telegps-v0.1/ao_pins.h @@ -65,7 +65,10 @@ #define ao_gps_fifo (ao_stm_usart2.rx_fifo) #define HAS_EEPROM 1 -#define USE_INTERNAL_FLASH 1 +#define USE_INTERNAL_FLASH 0 +#define USE_EEPROM_CONFIG 1 +#define USE_STORAGE_CONFIG 0 + #define HAS_USB 1 #define HAS_BEEP 0 #define HAS_RADIO 1 diff --git a/src/telegps-v0.1/ao_telegps.c b/src/telegps-v0.1/ao_telegps.c index 68116bfb..bc37b504 100644 --- a/src/telegps-v0.1/ao_telegps.c +++ b/src/telegps-v0.1/ao_telegps.c @@ -18,6 +18,7 @@ #include <ao.h> #include <ao_exti.h> #include <ao_fat.h> +#include <ao_eeprom.h> uint16_t ao_flight_number = 1; @@ -40,7 +41,7 @@ main(void) ao_dma_init(); ao_exti_init(); - ao_storage_init(); + ao_eeprom_init(); ao_serial_init(); diff --git a/src/telegps-v0.3/.gitignore b/src/telegps-v0.3/.gitignore new file mode 100644 index 00000000..892c3acc --- /dev/null +++ b/src/telegps-v0.3/.gitignore @@ -0,0 +1,3 @@ +ao_product.h +ao_serial_lpc.h +*.elf diff --git a/src/telegps-v0.3/Makefile b/src/telegps-v0.3/Makefile new file mode 100644 index 00000000..bb9c8c64 --- /dev/null +++ b/src/telegps-v0.3/Makefile @@ -0,0 +1,84 @@ +# +# AltOS build +# +# + +include ../lpc/Makefile.defs + +INC = \ + ao.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_pins.h \ + ao_product.h \ + ao_task.h \ + ao_whiten.h \ + ao_cc115l.h \ + ao_fec.h \ + lpc.h \ + Makefile + + +ALTOS_SRC = \ + ao_interrupt.c \ + ao_boot_chain.c \ + ao_product.c \ + ao_romconfig.c \ + ao_cmd.c \ + ao_config.c \ + ao_task.c \ + ao_stdio.c \ + ao_panic.c \ + ao_timer_lpc.c \ + ao_mutex.c \ + ao_freq.c \ + ao_spi_lpc.c \ + ao_usb_lpc.c \ + ao_exti_lpc.c \ + ao_serial_lpc.c \ + ao_gps_ublox.c \ + ao_gps_show.c \ + ao_cc115l.c \ + ao_fec_tx.c \ + ao_aprs.c \ + ao_telemetry.c \ + ao_storage.c \ + ao_m25.c \ + ao_log.c \ + ao_log_mega.c \ + ao_gps_report_mega.c \ + $(SAMPLE_PROFILE) + +PRODUCT=TeleGPS-v0.3 +PRODUCT_DEF=-DTELEGPS +IDPRODUCT=0x0025 + +CFLAGS = $(PRODUCT_DEF) $(LPC_CFLAGS) $(PROFILE_DEF) -Os -g + +PROGNAME=telegps-v0.3 +PROG=$(PROGNAME)-$(VERSION).elf + +SRC=$(ALTOS_SRC) ao_telegps.c +OBJ=$(SRC:.c=.o) + +all: $(PROG) + +LDFLAGS=-L../lpc -Wl,-Taltos.ld + +$(PROG): Makefile $(OBJ) altos.ld + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) + +$(OBJ): $(INC) + +ao_product.h: ao-make-product.5c ../Version + $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ + +distclean: clean + +clean: + rm -f *.o ao_serial_lpc.h $(PROGNAME)-*.elf $(PROGNAME)-*.ihx + rm -f ao_product.h + +install: + +uninstall: diff --git a/src/telegps-v0.3/ao_pins.h b/src/telegps-v0.3/ao_pins.h new file mode 100644 index 00000000..a4afaa54 --- /dev/null +++ b/src/telegps-v0.3/ao_pins.h @@ -0,0 +1,114 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +#define AO_STACK_SIZE 448 + +#define IS_FLASH_LOADER 0 + +/* Crystal on the board */ +#define AO_LPC_CLKIN 12000000 + +/* Main clock frequency. 48MHz for USB so we don't use the USB PLL */ +#define AO_LPC_CLKOUT 48000000 + +/* System clock frequency */ +#define AO_LPC_SYSCLK 24000000 + +#define HAS_SERIAL_0 1 +#define SERIAL_0_18_19 1 +#define USE_SERIAL_0_STDIN 0 + +#define ao_gps_getchar ao_serial0_getchar +#define ao_gps_putchar ao_serial0_putchar +#define ao_gps_set_speed ao_serial0_set_speed +#define ao_gps_fifo (ao_usart_rx_fifo) + +#define HAS_EEPROM 1 +#define USE_INTERNAL_FLASH 0 +#define HAS_USB 1 +#define HAS_BEEP 0 +#define HAS_RADIO 1 +#define HAS_TELEMETRY 1 +#define HAS_RDF 0 +#define HAS_APRS 1 +#define HAS_RADIO_RECV 0 + +#define HAS_USB_PULLUP 1 +#define AO_USB_PULLUP_PORT 0 +#define AO_USB_PULLUP_PIN 7 + +/* Flash part */ +#define HAS_SPI_0 1 +#define SPI_SCK0_P0_6 1 +#define SPI_0_OSPEEDR AO_SPI_OSPEED_12MHz + +/* Radio */ +#define HAS_SPI_1 1 +#define SPI_SCK1_P1_15 1 +#define SPI_MISO1_P0_22 1 +#define SPI_MOSI1_P0_21 1 + +#define HAS_GPS 1 +#define HAS_FLIGHT 0 +#define HAS_ADC 0 +#define HAS_LOG 1 + +#define AO_CONFIG_DEFAULT_APRS_INTERVAL 5 +#define AO_CONFIG_DEFAULT_RADIO_POWER 0xc0 + +/* + * GPS + */ + +#define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_9600 + +/* + * Radio (cc115l) + */ + +/* gets pretty close to 434.550 */ + +#define AO_RADIO_CAL_DEFAULT 0x10b6a5 + +#define HAS_RADIO_POWER 1 +#define AO_FEC_DEBUG 0 +#define AO_CC115L_SPI_CS_PORT 0 +#define AO_CC115L_SPI_CS_PIN 3 +#define AO_CC115L_SPI_BUS 0 + +#define AO_CC115L_FIFO_INT_GPIO_IOCFG CC115L_IOCFG2 +#define AO_CC115L_FIFO_INT_PORT 0 +#define AO_CC115L_FIFO_INT_PIN 20 + +#define AO_CC115L_DONE_INT_GPIO_IOCFG CC115L_IOCFG0 +#define AO_CC115L_DONE_INT_PORT 0 +#define AO_CC115L_DONE_INT_PIN 2 + +/* + * Flash (M25) + */ +#define M25_MAX_CHIPS 1 +#define AO_M25_SPI_CS_PORT 0 +#define AO_M25_SPI_CS_MASK (1 << 23) +#define AO_M25_SPI_BUS 1 + +#define PACKET_HAS_SLAVE 0 + +#endif /* _AO_PINS_H_ */ diff --git a/src/telegps-v0.3/ao_telegps.c b/src/telegps-v0.3/ao_telegps.c new file mode 100644 index 00000000..608817e7 --- /dev/null +++ b/src/telegps-v0.3/ao_telegps.c @@ -0,0 +1,65 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_exti.h> +#include <ao_fat.h> + +uint16_t ao_flight_number = 1; + +int +main(void) +{ + ao_clock_init(); + +#if HAS_STACK_GUARD + ao_mpu_init(); +#endif + + ao_task_init(); + ao_timer_init(); + + + ao_spi_init(); + ao_exti_init(); + + ao_storage_init(); + + ao_serial_init(); + + ao_cmd_init(); + + ao_usb_init(); + ao_radio_init(); + + ao_gps_init(); +#if HAS_LOG + ao_gps_report_mega_init(); + ao_log_init(); +#endif + + ao_telemetry_init(); + ao_telemetry_set_interval(AO_SEC_TO_TICKS(1)); + +#if HAS_SAMPLE_PROFILE + ao_sample_profile_init(); +#endif + ao_config_init(); + + ao_start_scheduler(); + return 0; +} diff --git a/src/telegps-v0.3/flash-loader/Makefile b/src/telegps-v0.3/flash-loader/Makefile new file mode 100644 index 00000000..6f4ce0d8 --- /dev/null +++ b/src/telegps-v0.3/flash-loader/Makefile @@ -0,0 +1,8 @@ +# +# AltOS flash loader build +# +# + +TOPDIR=../.. +HARDWARE=telegps-v0.3 +include $(TOPDIR)/lpc/Makefile-flash.defs diff --git a/src/telegps-v0.3/flash-loader/ao_pins.h b/src/telegps-v0.3/flash-loader/ao_pins.h new file mode 100644 index 00000000..91097a25 --- /dev/null +++ b/src/telegps-v0.3/flash-loader/ao_pins.h @@ -0,0 +1,33 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +#include <ao_flash_lpc_pins.h> + +#define AO_BOOT_PIN 1 +#define AO_BOOT_APPLICATION_GPIO 0 +#define AO_BOOT_APPLICATION_PIN 19 +#define AO_BOOT_APPLICATION_VALUE 1 +#define AO_BOOT_APPLICATION_MODE AO_EXTI_MODE_PULL_UP + +#define HAS_USB_PULLUP 1 +#define AO_USB_PULLUP_PORT 0 +#define AO_USB_PULLUP_PIN 7 + +#endif /* _AO_PINS_H_ */ diff --git a/src/telelco-v0.1/Makefile b/src/telelco-v0.1/Makefile index 24083308..44d9237f 100644 --- a/src/telelco-v0.1/Makefile +++ b/src/telelco-v0.1/Makefile @@ -69,14 +69,15 @@ CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) -Os -g PROGNAME=telelco-v0.1 PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx SRC=$(ALTOS_SRC) ao_telelco.c OBJ=$(SRC:.c=.o) -all: $(PROG) +all: $(PROG) $(HEX) $(PROG): Makefile $(OBJ) altos.ld - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) ../altitude.h: make-altitude nickle $< > $@ @@ -89,7 +90,7 @@ ao_product.h: ao-make-product.5c ../Version distclean: clean clean: - rm -f *.o $(PROGNAME)-*.elf + rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx rm -f ao_product.h install: diff --git a/src/telelco-v0.2/Makefile b/src/telelco-v0.2/Makefile index cc6e62c4..f28bdd32 100644 --- a/src/telelco-v0.2/Makefile +++ b/src/telelco-v0.2/Makefile @@ -21,6 +21,7 @@ INC = \ ao_radio_spi.h \ ao_radio_cmac.h \ ao_cc1120_CC1120.h \ + ao_debounce.h \ stm32l.h # @@ -47,8 +48,8 @@ ALTOS_SRC = \ ao_dma_stm.c \ ao_spi_stm.c \ ao_beep_stm.c \ - ao_storage.c \ ao_eeprom_stm.c \ + ao_fast_timer.c \ ao_lcd_stm.c \ ao_usb_stm.c \ ao_exti_stm.c \ @@ -59,6 +60,7 @@ ALTOS_SRC = \ ao_fec_tx.c \ ao_fec_rx.c \ ao_seven_segment.c \ + ao_debounce.c \ ao_quadrature.c \ ao_button.c \ ao_event.c \ @@ -75,14 +77,15 @@ CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) -Os -g PROGNAME=telelco-v0.2 PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx SRC=$(ALTOS_SRC) ao_telelco.c OBJ=$(SRC:.c=.o) -all: $(PROG) +all: $(PROG) $(HEX) $(PROG): Makefile $(OBJ) altos.ld - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) ../altitude.h: make-altitude nickle $< > $@ @@ -95,7 +98,7 @@ ao_product.h: ao-make-product.5c ../Version distclean: clean clean: - rm -f *.o $(PROGNAME)-*.elf + rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx rm -f ao_product.h install: diff --git a/src/telelco-v0.2/ao_lco.c b/src/telelco-v0.2/ao_lco.c index 418c0509..e8d16ca9 100644 --- a/src/telelco-v0.2/ao_lco.c +++ b/src/telelco-v0.2/ao_lco.c @@ -49,16 +49,16 @@ static uint16_t ao_lco_tick_offset; static struct ao_pad_query ao_pad_query; static void -ao_lco_set_pad(void) +ao_lco_set_pad(uint8_t pad) { - ao_seven_segment_set(AO_LCO_PAD_DIGIT, ao_lco_pad + 1); + ao_seven_segment_set(AO_LCO_PAD_DIGIT, pad + 1); } static void -ao_lco_set_box(void) +ao_lco_set_box(uint8_t box) { - ao_seven_segment_set(AO_LCO_BOX_DIGIT_1, ao_lco_box % 10); - ao_seven_segment_set(AO_LCO_BOX_DIGIT_10, ao_lco_box / 10); + ao_seven_segment_set(AO_LCO_BOX_DIGIT_1, box % 10); + ao_seven_segment_set(AO_LCO_BOX_DIGIT_10, box / 10); } #define MASK_SIZE(n) (((n) + 7) >> 3) @@ -103,8 +103,6 @@ ao_lco_input(void) int8_t dir, new_box, new_pad; ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200)); - ao_lco_set_pad(); - ao_lco_set_box(); for (;;) { ao_event_get(&event); PRINTD("event type %d unit %d value %d\n", @@ -114,11 +112,9 @@ ao_lco_input(void) switch (event.unit) { case AO_QUADRATURE_PAD: if (!ao_lco_armed) { - if (event.value == ao_lco_pad) - break; - dir = ((int8_t) event.value - (int8_t) ao_lco_pad) > 0 ? 1 : -1; - new_pad = event.value; - while (!ao_lco_pad_present(new_pad)) { + dir = (int8_t) event.value; + new_pad = ao_lco_pad; + do { new_pad += dir; if (new_pad > AO_PAD_MAX_CHANNELS) new_pad = 0; @@ -126,34 +122,30 @@ ao_lco_input(void) new_pad = AO_PAD_MAX_CHANNELS - 1; if (new_pad == ao_lco_pad) break; - } + } while (!ao_lco_pad_present(new_pad)); if (new_pad != ao_lco_pad) { ao_lco_pad = new_pad; - ao_quadrature_count[AO_QUADRATURE_PAD] = ao_lco_pad; - ao_lco_set_pad(); + ao_lco_set_pad(ao_lco_pad); } } break; case AO_QUADRATURE_BOX: if (!ao_lco_armed) { - if (event.value == ao_lco_box) - break; - dir = ((int8_t) event.value - (int8_t) ao_lco_box) > 0 ? 1 : -1; - new_box = event.value; - while (!ao_lco_box_present(new_box)) { + dir = (int8_t) event.value; + new_box = ao_lco_box; + do { new_box += dir; if (new_box > ao_lco_max_box) new_box = ao_lco_min_box; else if (new_box < ao_lco_min_box) - new_box = ao_lco_min_box; + new_box = ao_lco_max_box; if (new_box == ao_lco_box) break; - } - ao_quadrature_count[AO_QUADRATURE_PAD] = new_box; + } while (!ao_lco_box_present(new_box)); if (ao_lco_box != new_box) { ao_lco_box = new_box; ao_lco_got_channels = 0; - ao_lco_set_box(); + ao_lco_set_box(ao_lco_box); } } break; @@ -219,7 +211,7 @@ ao_lco_update(void) ao_lco_valid = 1; if (!c) { ao_lco_pad = ao_lco_pad_first(); - ao_lco_set_pad(); + ao_lco_set_pad(ao_lco_pad); } } else ao_lco_valid = 0; @@ -232,13 +224,24 @@ ao_lco_update(void) query.igniter_status[2], query.igniter_status[3]); #endif - ao_wakeup(&ao_pad_query); } static void +ao_lco_box_reset_present(void) +{ + ao_lco_min_box = 0xff; + ao_lco_max_box = 0x00; + memset(ao_lco_box_mask, 0, sizeof (ao_lco_box_mask)); +} + +static void ao_lco_box_set_present(uint8_t box) { + if (box < ao_lco_min_box) + ao_lco_min_box = box; + if (box > ao_lco_max_box) + ao_lco_max_box = box; if (box >= AO_PAD_MAX_BOXES) return; ao_lco_box_mask[MASK_ID(box)] |= 1 << MASK_SHIFT(box); @@ -249,19 +252,18 @@ ao_lco_search(void) { uint16_t tick_offset; int8_t r; - - ao_lco_min_box = 0xff; - ao_lco_max_box = 0x00; - for (ao_lco_box = 0; ao_lco_box < AO_PAD_MAX_BOXES; ao_lco_box++) { - if ((ao_lco_box % 10) == 0) - ao_lco_set_box(); - r = ao_lco_query(ao_lco_box, &ao_pad_query, &ao_lco_tick_offset); + uint8_t box; + + ao_lco_box_reset_present(); + for (box = 0; box < AO_PAD_MAX_BOXES; box++) { + if ((box % 10) == 0) + ao_lco_set_box(box); + tick_offset = 0; + r = ao_lco_query(box, &ao_pad_query, &tick_offset); + PRINTD("box %d result %d\n", box, r); if (r == AO_RADIO_CMAC_OK) { - if (ao_lco_box < ao_lco_min_box) - ao_lco_min_box = ao_lco_box; - if (ao_lco_box > ao_lco_max_box) - ao_lco_max_box = ao_lco_box; - ao_lco_box_set_present(ao_lco_box); + ao_lco_box_set_present(box); + ao_delay(AO_MS_TO_TICKS(30)); } } if (ao_lco_min_box <= ao_lco_max_box) @@ -271,6 +273,8 @@ ao_lco_search(void) ao_lco_valid = 0; ao_lco_got_channels = 0; ao_lco_pad = 0; + ao_lco_set_pad(ao_lco_pad); + ao_lco_set_box(ao_lco_box); } static void @@ -382,6 +386,7 @@ ao_lco_set_debug(void) __code struct ao_cmds ao_lco_cmds[] = { { ao_lco_set_debug, "D <0 off, 1 on>\0Debug" }, + { ao_lco_search, "s\0Search for pad boxes" }, { 0, NULL } }; #endif diff --git a/src/telelco-v0.2/ao_pins.h b/src/telelco-v0.2/ao_pins.h index 07ea1b45..62f221a1 100644 --- a/src/telelco-v0.2/ao_pins.h +++ b/src/telelco-v0.2/ao_pins.h @@ -43,6 +43,8 @@ #define HAS_EEPROM 1 #define USE_INTERNAL_FLASH 1 +#define USE_EEPROM_CONFIG 1 +#define USE_STORAGE_CONFIG 0 #define HAS_USB 1 #define HAS_BEEP 1 #define HAS_RADIO 1 @@ -235,7 +237,6 @@ */ #define AO_QUADRATURE_COUNT 2 -#define AO_QUADRATURE_MODE 0 #define AO_QUADRATURE_0_PORT &stm_gpioe #define AO_QUADRATURE_0_A 3 diff --git a/src/telelco-v0.2/ao_telelco.c b/src/telelco-v0.2/ao_telelco.c index 66bf0ba1..d9f7c693 100644 --- a/src/telelco-v0.2/ao_telelco.c +++ b/src/telelco-v0.2/ao_telelco.c @@ -28,6 +28,7 @@ #include <ao_lco.h> #include <ao_lco_cmd.h> #include <ao_radio_cmac_cmd.h> +#include <ao_eeprom.h> int main(void) @@ -52,7 +53,7 @@ main(void) ao_quadrature_init(); ao_button_init(); - ao_storage_init(); + ao_eeprom_init(); ao_radio_init(); diff --git a/src/telemega-v0.1/Makefile b/src/telemega-v0.1/Makefile index a72d08f2..0145f49c 100644 --- a/src/telemega-v0.1/Makefile +++ b/src/telemega-v0.1/Makefile @@ -25,6 +25,8 @@ INC = \ ao_task.h \ ao_whiten.h \ ao_sample_profile.h \ + ao_quaternion.h \ + math.h \ ao_mpu.h \ stm32l.h \ Makefile @@ -44,6 +46,20 @@ INC = \ #STACK_GUARD=ao_mpu_stm.c #STACK_GUARD_DEF=-DHAS_STACK_GUARD=1 +MATH_SRC=\ + ef_acos.c \ + ef_sqrt.c \ + ef_rem_pio2.c \ + kf_cos.c \ + kf_sin.c \ + kf_rem_pio2.c \ + sf_copysign.c \ + sf_cos.c \ + sf_sin.c \ + sf_fabs.c \ + sf_floor.c \ + sf_scalbn.c + ALTOS_SRC = \ ao_boot_chain.c \ ao_interrupt.c \ @@ -59,6 +75,7 @@ ALTOS_SRC = \ ao_mutex.c \ ao_serial_stm.c \ ao_gps_skytraq.c \ + ao_gps_show.c \ ao_gps_report_mega.c \ ao_ignite.c \ ao_freq.c \ @@ -73,6 +90,7 @@ ALTOS_SRC = \ ao_hmc5883.c \ ao_adc_stm.c \ ao_beep_stm.c \ + ao_eeprom_stm.c \ ao_storage.c \ ao_m25.c \ ao_usb_stm.c \ @@ -92,6 +110,7 @@ ALTOS_SRC = \ ao_companion.c \ ao_pyro.c \ ao_aprs.c \ + $(MATH_SRC) \ $(PROFILE) \ $(SAMPLE_PROFILE) \ $(STACK_GUARD) @@ -104,14 +123,15 @@ CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STA PROGNAME=telemega-v0.1 PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx SRC=$(ALTOS_SRC) ao_telemega.c OBJ=$(SRC:.c=.o) -all: $(PROG) +all: $(PROG) $(HEX) $(PROG): Makefile $(OBJ) altos.ld - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) ../altitude-pa.h: make-altitude-pa nickle $< > $@ @@ -124,7 +144,7 @@ ao_product.h: ao-make-product.5c ../Version distclean: clean clean: - rm -f *.o $(PROGNAME)-*.elf + rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx rm -f ao_product.h install: diff --git a/src/telemega-v0.1/ao_pins.h b/src/telemega-v0.1/ao_pins.h index 4c645871..daeb9f17 100644 --- a/src/telemega-v0.1/ao_pins.h +++ b/src/telemega-v0.1/ao_pins.h @@ -64,8 +64,11 @@ #define ao_gps_set_speed ao_serial3_set_speed #define ao_gps_fifo (ao_stm_usart3.rx_fifo) +#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX (1024 * 1024) #define HAS_EEPROM 1 #define USE_INTERNAL_FLASH 0 +#define USE_EEPROM_CONFIG 1 +#define USE_STORAGE_CONFIG 0 #define HAS_USB 1 #define HAS_BEEP 1 #define HAS_RADIO 1 @@ -111,6 +114,7 @@ #define HAS_GPS 1 #define HAS_FLIGHT 1 #define HAS_ADC 1 +#define HAS_ADC_TEMP 1 #define HAS_LOG 1 /* @@ -120,28 +124,35 @@ #define HAS_IGNITE 1 #define HAS_IGNITE_REPORT 1 -#define AO_SENSE_DROGUE(p) ((p)->adc.sense[0]) -#define AO_SENSE_MAIN(p) ((p)->adc.sense[1]) +#define AO_SENSE_PYRO(p,n) ((p)->adc.sense[n]) +#define AO_SENSE_DROGUE(p) ((p)->adc.sense[4]) +#define AO_SENSE_MAIN(p) ((p)->adc.sense[5]) #define AO_IGNITER_CLOSED 400 #define AO_IGNITER_OPEN 60 -#define AO_IGNITER_DROGUE_PORT (&stm_gpiod) -#define AO_IGNITER_DROGUE_PIN 6 +/* Pyro A */ +#define AO_PYRO_PORT_0 (&stm_gpiod) +#define AO_PYRO_PIN_0 6 -#define AO_IGNITER_MAIN_PORT (&stm_gpiod) -#define AO_IGNITER_MAIN_PIN 7 +/* Pyro B */ +#define AO_PYRO_PORT_1 (&stm_gpiod) +#define AO_PYRO_PIN_1 7 -#define AO_PYRO_PORT_0 (&stm_gpiob) -#define AO_PYRO_PIN_0 5 +/* Pyro C */ +#define AO_PYRO_PORT_2 (&stm_gpiob) +#define AO_PYRO_PIN_2 5 -#define AO_PYRO_PORT_1 (&stm_gpioe) -#define AO_PYRO_PIN_1 4 +/* Pyro D */ +#define AO_PYRO_PORT_3 (&stm_gpioe) +#define AO_PYRO_PIN_3 4 -#define AO_PYRO_PORT_2 (&stm_gpioe) -#define AO_PYRO_PIN_2 6 +/* Drogue */ +#define AO_IGNITER_DROGUE_PORT (&stm_gpioe) +#define AO_IGNITER_DROGUE_PIN 6 -#define AO_PYRO_PORT_3 (&stm_gpioe) -#define AO_PYRO_PIN_3 5 +/* Main */ +#define AO_IGNITER_MAIN_PORT (&stm_gpioe) +#define AO_IGNITER_MAIN_PIN 5 /* Number of general purpose pyro channels available */ #define AO_PYRO_NUM 4 @@ -159,11 +170,16 @@ struct ao_adc { int16_t sense[AO_ADC_NUM_SENSE]; int16_t v_batt; int16_t v_pbatt; - int16_t accel_ref; - int16_t accel; int16_t temp; }; +#define AO_ADC_DUMP(p) \ + printf("tick: %5u A: %5d B: %5d C: %5d D: %5d drogue: %5d main: %5d batt: %5d pbatt: %5d temp: %5d\n", \ + (p)->tick, \ + (p)->adc.sense[0], (p)->adc.sense[1], (p)->adc.sense[2], \ + (p)->adc.sense[3], (p)->adc.sense[4], (p)->adc.sense[5], \ + (p)->adc.v_batt, (p)->adc.v_pbatt, (p)->adc.temp) + #define AO_ADC_SENSE_A 0 #define AO_ADC_SENSE_A_PORT (&stm_gpioa) #define AO_ADC_SENSE_A_PIN 0 @@ -180,13 +196,13 @@ struct ao_adc { #define AO_ADC_SENSE_D_PORT (&stm_gpioa) #define AO_ADC_SENSE_D_PIN 3 -#define AO_ADC_SENSE_E 4 -#define AO_ADC_SENSE_E_PORT (&stm_gpioa) -#define AO_ADC_SENSE_E_PIN 4 +#define AO_ADC_SENSE_DROGUE 4 +#define AO_ADC_SENSE_DROGUE_PORT (&stm_gpioa) +#define AO_ADC_SENSE_DROGUE_PIN 4 -#define AO_ADC_SENSE_F 22 -#define AO_ADC_SENSE_F_PORT (&stm_gpioe) -#define AO_ADC_SENSE_F_PIN 7 +#define AO_ADC_SENSE_MAIN 22 +#define AO_ADC_SENSE_MAIN_PORT (&stm_gpioe) +#define AO_ADC_SENSE_MAIN_PIN 7 #define AO_ADC_V_BATT 8 #define AO_ADC_V_BATT_PORT (&stm_gpiob) @@ -196,22 +212,13 @@ struct ao_adc { #define AO_ADC_V_PBATT_PORT (&stm_gpiob) #define AO_ADC_V_PBATT_PIN 1 -#define AO_ADC_ACCEL_REF 10 -#define AO_ADC_ACCEL_REF_PORT (&stm_gpioc) -#define AO_ADC_ACCEL_REF_PIN 0 - -#define AO_ADC_ACCEL 11 -#define AO_ADC_ACCEL_PORT (&stm_gpioc) -#define AO_ADC_ACCEL_PIN 1 - #define AO_ADC_TEMP 16 #define AO_ADC_RCC_AHBENR ((1 << STM_RCC_AHBENR_GPIOAEN) | \ (1 << STM_RCC_AHBENR_GPIOEEN) | \ - (1 << STM_RCC_AHBENR_GPIOBEN) | \ - (1 << STM_RCC_AHBENR_GPIOCEN)) + (1 << STM_RCC_AHBENR_GPIOBEN)) -#define AO_NUM_ADC_PIN (AO_ADC_NUM_SENSE + 4) +#define AO_NUM_ADC_PIN (AO_ADC_NUM_SENSE + 2) #define AO_ADC_PIN0_PORT AO_ADC_SENSE_A_PORT #define AO_ADC_PIN0_PIN AO_ADC_SENSE_A_PIN @@ -221,32 +228,26 @@ struct ao_adc { #define AO_ADC_PIN2_PIN AO_ADC_SENSE_C_PIN #define AO_ADC_PIN3_PORT AO_ADC_SENSE_D_PORT #define AO_ADC_PIN3_PIN AO_ADC_SENSE_D_PIN -#define AO_ADC_PIN4_PORT AO_ADC_SENSE_E_PORT -#define AO_ADC_PIN4_PIN AO_ADC_SENSE_E_PIN -#define AO_ADC_PIN5_PORT AO_ADC_SENSE_F_PORT -#define AO_ADC_PIN5_PIN AO_ADC_SENSE_F_PIN +#define AO_ADC_PIN4_PORT AO_ADC_SENSE_DROGUE_PORT +#define AO_ADC_PIN4_PIN AO_ADC_SENSE_DROGUE_PIN +#define AO_ADC_PIN5_PORT AO_ADC_SENSE_MAIN_PORT +#define AO_ADC_PIN5_PIN AO_ADC_SENSE_MAIN_PIN #define AO_ADC_PIN6_PORT AO_ADC_V_BATT_PORT #define AO_ADC_PIN6_PIN AO_ADC_V_BATT_PIN #define AO_ADC_PIN7_PORT AO_ADC_V_PBATT_PORT #define AO_ADC_PIN7_PIN AO_ADC_V_PBATT_PIN -#define AO_ADC_PIN8_PORT AO_ADC_ACCEL_REF_PORT -#define AO_ADC_PIN8_PIN AO_ADC_ACCEL_REF_PIN -#define AO_ADC_PIN9_PORT AO_ADC_ACCEL_PORT -#define AO_ADC_PIN9_PIN AO_ADC_ACCEL_PIN -#define AO_NUM_ADC (AO_ADC_NUM_SENSE + 5) +#define AO_NUM_ADC (AO_ADC_NUM_SENSE + 3) #define AO_ADC_SQ1 AO_ADC_SENSE_A #define AO_ADC_SQ2 AO_ADC_SENSE_B #define AO_ADC_SQ3 AO_ADC_SENSE_C #define AO_ADC_SQ4 AO_ADC_SENSE_D -#define AO_ADC_SQ5 AO_ADC_SENSE_E -#define AO_ADC_SQ6 AO_ADC_SENSE_F +#define AO_ADC_SQ5 AO_ADC_SENSE_DROGUE +#define AO_ADC_SQ6 AO_ADC_SENSE_MAIN #define AO_ADC_SQ7 AO_ADC_V_BATT #define AO_ADC_SQ8 AO_ADC_V_PBATT -#define AO_ADC_SQ9 AO_ADC_ACCEL_REF -#define AO_ADC_SQ10 AO_ADC_ACCEL -#define AO_ADC_SQ11 AO_ADC_TEMP +#define AO_ADC_SQ9 AO_ADC_TEMP /* * Pressure sensor settings @@ -296,7 +297,6 @@ struct ao_adc { #define AO_CC1120_MARC_GPIO 3 #define AO_CC1120_MARC_GPIO_IOCFG CC1120_IOCFG3 - #define HAS_BOOT_RADIO 0 /* @@ -316,8 +316,7 @@ struct ao_adc { #define AO_MPU6000_INT_PORT (&stm_gpioc) #define AO_MPU6000_INT_PIN 13 #define AO_MPU6000_I2C_INDEX STM_I2C_INDEX(1) - -#define HAS_HIGHG_ACCEL 0 +#define HAS_IMU 1 /* * mma655x diff --git a/src/telemega-v0.1/ao_telemega.c b/src/telemega-v0.1/ao_telemega.c index fbdab64a..7b035269 100644 --- a/src/telemega-v0.1/ao_telemega.c +++ b/src/telemega-v0.1/ao_telemega.c @@ -24,6 +24,7 @@ #include <ao_packet.h> #include <ao_companion.h> #include <ao_profile.h> +#include <ao_eeprom.h> #if HAS_SAMPLE_PROFILE #include <ao_sample_profile.h> #endif @@ -71,6 +72,7 @@ main(void) ao_mma655x_init(); #endif + ao_eeprom_init(); ao_storage_init(); ao_flight_init(); diff --git a/src/telemega-v0.3/.gitignore b/src/telemega-v1.0/.gitignore index e67759a2..e67759a2 100644 --- a/src/telemega-v0.3/.gitignore +++ b/src/telemega-v1.0/.gitignore diff --git a/src/telemega-v1.0/Makefile b/src/telemega-v1.0/Makefile new file mode 100644 index 00000000..543f7e74 --- /dev/null +++ b/src/telemega-v1.0/Makefile @@ -0,0 +1,153 @@ +# +# AltOS build +# +# + +include ../stm/Makefile.defs + +INC = \ + ao.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_companion.h \ + ao_data.h \ + ao_sample.h \ + ao_pins.h \ + altitude-pa.h \ + ao_kalman.h \ + ao_product.h \ + ao_ms5607.h \ + ao_hmc5883.h \ + ao_mpu6000.h \ + ao_mma655x.h \ + ao_cc1120_CC1120.h \ + ao_profile.h \ + ao_task.h \ + ao_whiten.h \ + ao_sample_profile.h \ + ao_quaternion.h \ + math.h \ + ao_mpu.h \ + stm32l.h \ + math.h \ + Makefile + +# +# Common AltOS sources +# +# ao_hmc5883.c + +#PROFILE=ao_profile.c +#PROFILE_DEF=-DAO_PROFILE=1 + +#SAMPLE_PROFILE=ao_sample_profile.c \ +# ao_sample_profile_timer.c +#SAMPLE_PROFILE_DEF=-DHAS_SAMPLE_PROFILE=1 + +#STACK_GUARD=ao_mpu_stm.c +#STACK_GUARD_DEF=-DHAS_STACK_GUARD=1 + +MATH_SRC=\ + ef_acos.c \ + ef_sqrt.c \ + ef_rem_pio2.c \ + kf_cos.c \ + kf_sin.c \ + kf_rem_pio2.c \ + sf_copysign.c \ + sf_cos.c \ + sf_fabs.c \ + sf_floor.c \ + sf_scalbn.c \ + sf_sin.c + +ALTOS_SRC = \ + ao_boot_chain.c \ + ao_interrupt.c \ + ao_product.c \ + ao_romconfig.c \ + ao_cmd.c \ + ao_config.c \ + ao_task.c \ + ao_led.c \ + ao_stdio.c \ + ao_panic.c \ + ao_timer.c \ + ao_mutex.c \ + ao_serial_stm.c \ + ao_gps_ublox.c \ + ao_gps_show.c \ + ao_gps_report_mega.c \ + ao_ignite.c \ + ao_freq.c \ + ao_dma_stm.c \ + ao_spi_stm.c \ + ao_cc1120.c \ + ao_fec_tx.c \ + ao_fec_rx.c \ + ao_data.c \ + ao_ms5607.c \ + ao_mma655x.c \ + ao_hmc5883.c \ + ao_adc_stm.c \ + ao_beep_stm.c \ + ao_eeprom_stm.c \ + ao_storage.c \ + ao_m25.c \ + ao_usb_stm.c \ + ao_exti_stm.c \ + ao_report.c \ + ao_i2c_stm.c \ + ao_mpu6000.c \ + ao_convert_pa.c \ + ao_log.c \ + ao_log_mega.c \ + ao_sample.c \ + ao_kalman.c \ + ao_flight.c \ + ao_telemetry.c \ + ao_packet_slave.c \ + ao_packet.c \ + ao_companion.c \ + ao_pyro.c \ + ao_aprs.c \ + $(MATH_SRC) \ + $(PROFILE) \ + $(SAMPLE_PROFILE) \ + $(STACK_GUARD) + +PRODUCT=TeleMega-v1.0 +PRODUCT_DEF=-DTELEMEGA +IDPRODUCT=0x0023 + +CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STACK_GUARD_DEF) -Os -g + +PROGNAME=telemega-v1.0 +PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx + +SRC=$(ALTOS_SRC) ao_telemega.c +OBJ=$(SRC:.c=.o) + +all: $(PROG) $(HEX) + +$(PROG): Makefile $(OBJ) altos.ld + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) + +../altitude-pa.h: make-altitude-pa + nickle $< > $@ + +$(OBJ): $(INC) + +ao_product.h: ao-make-product.5c ../Version + $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ + +distclean: clean + +clean: + rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx + rm -f ao_product.h + +install: + +uninstall: diff --git a/src/telemega-v0.3/ao_pins.h b/src/telemega-v1.0/ao_pins.h index b1504d28..95644dae 100644 --- a/src/telemega-v0.3/ao_pins.h +++ b/src/telemega-v1.0/ao_pins.h @@ -64,8 +64,11 @@ #define ao_gps_set_speed ao_serial3_set_speed #define ao_gps_fifo (ao_stm_usart3.rx_fifo) +#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX (1024 * 1024) #define HAS_EEPROM 1 #define USE_INTERNAL_FLASH 0 +#define USE_EEPROM_CONFIG 1 +#define USE_STORAGE_CONFIG 0 #define HAS_USB 1 #define HAS_BEEP 1 #define HAS_RADIO 1 @@ -121,28 +124,35 @@ #define HAS_IGNITE 1 #define HAS_IGNITE_REPORT 1 -#define AO_SENSE_DROGUE(p) ((p)->adc.sense[0]) -#define AO_SENSE_MAIN(p) ((p)->adc.sense[1]) +#define AO_SENSE_PYRO(p,n) ((p)->adc.sense[n]) +#define AO_SENSE_DROGUE(p) ((p)->adc.sense[4]) +#define AO_SENSE_MAIN(p) ((p)->adc.sense[5]) #define AO_IGNITER_CLOSED 400 #define AO_IGNITER_OPEN 60 -#define AO_IGNITER_DROGUE_PORT (&stm_gpiod) -#define AO_IGNITER_DROGUE_PIN 6 +/* Pyro A */ +#define AO_PYRO_PORT_0 (&stm_gpiod) +#define AO_PYRO_PIN_0 6 -#define AO_IGNITER_MAIN_PORT (&stm_gpiod) -#define AO_IGNITER_MAIN_PIN 7 +/* Pyro B */ +#define AO_PYRO_PORT_1 (&stm_gpiod) +#define AO_PYRO_PIN_1 7 -#define AO_PYRO_PORT_0 (&stm_gpiob) -#define AO_PYRO_PIN_0 5 +/* Pyro C */ +#define AO_PYRO_PORT_2 (&stm_gpiob) +#define AO_PYRO_PIN_2 5 -#define AO_PYRO_PORT_1 (&stm_gpioe) -#define AO_PYRO_PIN_1 4 +/* Pyro D */ +#define AO_PYRO_PORT_3 (&stm_gpioe) +#define AO_PYRO_PIN_3 4 -#define AO_PYRO_PORT_2 (&stm_gpioe) -#define AO_PYRO_PIN_2 6 +/* Drogue */ +#define AO_IGNITER_DROGUE_PORT (&stm_gpioe) +#define AO_IGNITER_DROGUE_PIN 6 -#define AO_PYRO_PORT_3 (&stm_gpioe) -#define AO_PYRO_PIN_3 5 +/* Main */ +#define AO_IGNITER_MAIN_PORT (&stm_gpioe) +#define AO_IGNITER_MAIN_PIN 5 /* Number of general purpose pyro channels available */ #define AO_PYRO_NUM 4 @@ -163,6 +173,13 @@ struct ao_adc { int16_t temp; }; +#define AO_ADC_DUMP(p) \ + printf("tick: %5u A: %5d B: %5d C: %5d D: %5d drogue: %5d main: %5d batt: %5d pbatt: %5d temp: %5d\n", \ + (p)->tick, \ + (p)->adc.sense[0], (p)->adc.sense[1], (p)->adc.sense[2], \ + (p)->adc.sense[3], (p)->adc.sense[4], (p)->adc.sense[5], \ + (p)->adc.v_batt, (p)->adc.v_pbatt, (p)->adc.temp) + #define AO_ADC_SENSE_A 0 #define AO_ADC_SENSE_A_PORT (&stm_gpioa) #define AO_ADC_SENSE_A_PIN 0 @@ -179,13 +196,13 @@ struct ao_adc { #define AO_ADC_SENSE_D_PORT (&stm_gpioa) #define AO_ADC_SENSE_D_PIN 3 -#define AO_ADC_SENSE_E 4 -#define AO_ADC_SENSE_E_PORT (&stm_gpioa) -#define AO_ADC_SENSE_E_PIN 4 +#define AO_ADC_SENSE_DROGUE 4 +#define AO_ADC_SENSE_DROGUE_PORT (&stm_gpioa) +#define AO_ADC_SENSE_DROGUE_PIN 4 -#define AO_ADC_SENSE_F 22 -#define AO_ADC_SENSE_F_PORT (&stm_gpioe) -#define AO_ADC_SENSE_F_PIN 7 +#define AO_ADC_SENSE_MAIN 22 +#define AO_ADC_SENSE_MAIN_PORT (&stm_gpioe) +#define AO_ADC_SENSE_MAIN_PIN 7 #define AO_ADC_V_BATT 8 #define AO_ADC_V_BATT_PORT (&stm_gpiob) @@ -211,10 +228,10 @@ struct ao_adc { #define AO_ADC_PIN2_PIN AO_ADC_SENSE_C_PIN #define AO_ADC_PIN3_PORT AO_ADC_SENSE_D_PORT #define AO_ADC_PIN3_PIN AO_ADC_SENSE_D_PIN -#define AO_ADC_PIN4_PORT AO_ADC_SENSE_E_PORT -#define AO_ADC_PIN4_PIN AO_ADC_SENSE_E_PIN -#define AO_ADC_PIN5_PORT AO_ADC_SENSE_F_PORT -#define AO_ADC_PIN5_PIN AO_ADC_SENSE_F_PIN +#define AO_ADC_PIN4_PORT AO_ADC_SENSE_DROGUE_PORT +#define AO_ADC_PIN4_PIN AO_ADC_SENSE_DROGUE_PIN +#define AO_ADC_PIN5_PORT AO_ADC_SENSE_MAIN_PORT +#define AO_ADC_PIN5_PIN AO_ADC_SENSE_MAIN_PIN #define AO_ADC_PIN6_PORT AO_ADC_V_BATT_PORT #define AO_ADC_PIN6_PIN AO_ADC_V_BATT_PIN #define AO_ADC_PIN7_PORT AO_ADC_V_PBATT_PORT @@ -226,8 +243,8 @@ struct ao_adc { #define AO_ADC_SQ2 AO_ADC_SENSE_B #define AO_ADC_SQ3 AO_ADC_SENSE_C #define AO_ADC_SQ4 AO_ADC_SENSE_D -#define AO_ADC_SQ5 AO_ADC_SENSE_E -#define AO_ADC_SQ6 AO_ADC_SENSE_F +#define AO_ADC_SQ5 AO_ADC_SENSE_DROGUE +#define AO_ADC_SQ6 AO_ADC_SENSE_MAIN #define AO_ADC_SQ7 AO_ADC_V_BATT #define AO_ADC_SQ8 AO_ADC_V_PBATT #define AO_ADC_SQ9 AO_ADC_TEMP @@ -295,14 +312,13 @@ struct ao_adc { * mpu6000 */ -#define HAS_MPU6000 1 +#define HAS_MPU6000 1 #define AO_MPU6000_INT_PORT (&stm_gpioe) #define AO_MPU6000_INT_PIN 0 #define AO_MPU6000_SPI_BUS AO_SPI_1_PE13_PE14_PE15 #define AO_MPU6000_SPI_CS_PORT (&stm_gpiod) #define AO_MPU6000_SPI_CS_PIN 2 - -#define HAS_HIGHG_ACCEL 1 +#define HAS_IMU 1 /* * mma655x diff --git a/src/telemega-v0.3/ao_telemega.c b/src/telemega-v1.0/ao_telemega.c index fbdab64a..7b035269 100644 --- a/src/telemega-v0.3/ao_telemega.c +++ b/src/telemega-v1.0/ao_telemega.c @@ -24,6 +24,7 @@ #include <ao_packet.h> #include <ao_companion.h> #include <ao_profile.h> +#include <ao_eeprom.h> #if HAS_SAMPLE_PROFILE #include <ao_sample_profile.h> #endif @@ -71,6 +72,7 @@ main(void) ao_mma655x_init(); #endif + ao_eeprom_init(); ao_storage_init(); ao_flight_init(); diff --git a/src/telemega-v0.3/flash-loader/Makefile b/src/telemega-v1.0/flash-loader/Makefile index 8fda18cd..a11f800b 100644 --- a/src/telemega-v0.3/flash-loader/Makefile +++ b/src/telemega-v1.0/flash-loader/Makefile @@ -4,5 +4,5 @@ # TOPDIR=../.. -HARDWARE=telemega-v0.3 +HARDWARE=telemega-v1.0 include $(TOPDIR)/stm/Makefile-flash.defs diff --git a/src/telemega-v0.3/flash-loader/ao_pins.h b/src/telemega-v1.0/flash-loader/ao_pins.h index 1af92f13..1af92f13 100644 --- a/src/telemega-v0.3/flash-loader/ao_pins.h +++ b/src/telemega-v1.0/flash-loader/ao_pins.h diff --git a/src/telemetrum-v0.1-sky/Makefile b/src/telemetrum-v0.1-sky/Makefile index 69cd3461..a6634c29 100644 --- a/src/telemetrum-v0.1-sky/Makefile +++ b/src/telemetrum-v0.1-sky/Makefile @@ -11,6 +11,7 @@ TM_INC = \ TM_SRC = \ ao_gps_skytraq.c \ + ao_gps_show.c \ ao_25lc1024.c include ../product/Makefile.telemetrum diff --git a/src/telemetrum-v1.0/Makefile b/src/telemetrum-v1.0/Makefile index 4aae84c8..476a3b0a 100644 --- a/src/telemetrum-v1.0/Makefile +++ b/src/telemetrum-v1.0/Makefile @@ -11,6 +11,7 @@ TM_INC = \ TM_SRC = \ ao_companion.c \ ao_gps_skytraq.c \ + ao_gps_show.c \ ao_at45db161d.c include ../product/Makefile.telemetrum diff --git a/src/telemetrum-v1.1/Makefile b/src/telemetrum-v1.1/Makefile index 4bea03db..e44be7f9 100644 --- a/src/telemetrum-v1.1/Makefile +++ b/src/telemetrum-v1.1/Makefile @@ -11,6 +11,7 @@ TM_INC = TM_SRC = \ ao_companion.c \ ao_gps_skytraq.c \ + ao_gps_show.c \ ao_m25.c include ../product/Makefile.telemetrum diff --git a/src/telemetrum-v1.2/Makefile b/src/telemetrum-v1.2/Makefile index 4b650adf..f2285fbe 100644 --- a/src/telemetrum-v1.2/Makefile +++ b/src/telemetrum-v1.2/Makefile @@ -11,6 +11,7 @@ TM_INC = TM_SRC = \ ao_companion.c \ ao_gps_skytraq.c \ + ao_gps_show.c \ ao_m25.c include ../product/Makefile.telemetrum diff --git a/src/telemetrum-v2.0/.gitignore b/src/telemetrum-v2.0/.gitignore new file mode 100644 index 00000000..35ce24d4 --- /dev/null +++ b/src/telemetrum-v2.0/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +telemetrum-*.elf diff --git a/src/telemega-v0.3/Makefile b/src/telemetrum-v2.0/Makefile index 398c7dab..cebc9cab 100644 --- a/src/telemega-v0.3/Makefile +++ b/src/telemetrum-v2.0/Makefile @@ -17,8 +17,6 @@ INC = \ ao_kalman.h \ ao_product.h \ ao_ms5607.h \ - ao_hmc5883.h \ - ao_mpu6000.h \ ao_mma655x.h \ ao_cc1120_CC1120.h \ ao_profile.h \ @@ -29,11 +27,6 @@ INC = \ stm32l.h \ Makefile -# -# Common AltOS sources -# -# ao_hmc5883.c - #PROFILE=ao_profile.c #PROFILE_DEF=-DAO_PROFILE=1 @@ -59,7 +52,8 @@ ALTOS_SRC = \ ao_mutex.c \ ao_serial_stm.c \ ao_gps_ublox.c \ - ao_gps_report_mega.c \ + ao_gps_show.c \ + ao_gps_report_metrum.c \ ao_ignite.c \ ao_freq.c \ ao_dma_stm.c \ @@ -70,19 +64,17 @@ ALTOS_SRC = \ ao_data.c \ ao_ms5607.c \ ao_mma655x.c \ - ao_hmc5883.c \ ao_adc_stm.c \ ao_beep_stm.c \ ao_storage.c \ ao_m25.c \ ao_usb_stm.c \ ao_exti_stm.c \ + ao_eeprom_stm.c \ ao_report.c \ - ao_i2c_stm.c \ - ao_mpu6000.c \ ao_convert_pa.c \ ao_log.c \ - ao_log_mega.c \ + ao_log_metrum.c \ ao_sample.c \ ao_kalman.c \ ao_flight.c \ @@ -90,28 +82,28 @@ ALTOS_SRC = \ ao_packet_slave.c \ ao_packet.c \ ao_companion.c \ - ao_pyro.c \ ao_aprs.c \ $(PROFILE) \ $(SAMPLE_PROFILE) \ $(STACK_GUARD) -PRODUCT=TeleMega-v0.3 -PRODUCT_DEF=-DTELEMEGA -IDPRODUCT=0x0023 +PRODUCT=TeleMetrum-v2.0 +PRODUCT_DEF=-DTELEMETRUM_V_2_0 +IDPRODUCT=0x000b CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STACK_GUARD_DEF) -Os -g -PROGNAME=telemega-v0.3 +PROGNAME=telemetrum-v2.0 PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx -SRC=$(ALTOS_SRC) ao_telemega.c +SRC=$(ALTOS_SRC) ao_telemetrum.c OBJ=$(SRC:.c=.o) -all: $(PROG) +all: $(PROG) $(HEX) $(PROG): Makefile $(OBJ) altos.ld - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) ../altitude-pa.h: make-altitude-pa nickle $< > $@ @@ -124,7 +116,7 @@ ao_product.h: ao-make-product.5c ../Version distclean: clean clean: - rm -f *.o $(PROGNAME)-*.elf + rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx rm -f ao_product.h install: diff --git a/src/telemetrum-v2.0/ao_pins.h b/src/telemetrum-v2.0/ao_pins.h new file mode 100644 index 00000000..02f0f5e3 --- /dev/null +++ b/src/telemetrum-v2.0/ao_pins.h @@ -0,0 +1,293 @@ +/* + * Copyright © 2012 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +#define HAS_TASK_QUEUE 1 + +/* 8MHz High speed external crystal */ +#define AO_HSE 8000000 + +/* PLLVCO = 96MHz (so that USB will work) */ +#define AO_PLLMUL 12 +#define AO_RCC_CFGR_PLLMUL (STM_RCC_CFGR_PLLMUL_12) + +/* SYSCLK = 32MHz (no need to go faster than CPU) */ +#define AO_PLLDIV 3 +#define AO_RCC_CFGR_PLLDIV (STM_RCC_CFGR_PLLDIV_3) + +/* HCLK = 32MHz (CPU clock) */ +#define AO_AHB_PRESCALER 1 +#define AO_RCC_CFGR_HPRE_DIV STM_RCC_CFGR_HPRE_DIV_1 + +/* Run APB1 at 16MHz (HCLK/2) */ +#define AO_APB1_PRESCALER 2 +#define AO_RCC_CFGR_PPRE1_DIV STM_RCC_CFGR_PPRE2_DIV_2 + +/* Run APB2 at 16MHz (HCLK/2) */ +#define AO_APB2_PRESCALER 2 +#define AO_RCC_CFGR_PPRE2_DIV STM_RCC_CFGR_PPRE2_DIV_2 + +#define HAS_SERIAL_1 0 +#define USE_SERIAL_1_STDIN 0 +#define SERIAL_1_PB6_PB7 0 +#define SERIAL_1_PA9_PA10 1 + +#define HAS_SERIAL_2 0 +#define USE_SERIAL_2_STDIN 0 +#define SERIAL_2_PA2_PA3 0 +#define SERIAL_2_PD5_PD6 0 + +#define HAS_SERIAL_3 1 +#define USE_SERIAL_3_STDIN 0 +#define SERIAL_3_PB10_PB11 1 +#define SERIAL_3_PC10_PC11 0 +#define SERIAL_3_PD8_PD9 0 + +#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX (512 * 1024) +#define HAS_EEPROM 1 +#define USE_INTERNAL_FLASH 0 +#define USE_EEPROM_CONFIG 1 +#define USE_STORAGE_CONFIG 0 +#define HAS_USB 1 +#define HAS_BEEP 1 +#define BEEPER_CHANNEL 4 +#define HAS_RADIO 1 +#define HAS_TELEMETRY 1 +#define HAS_APRS 1 + +#define HAS_SPI_1 1 +#define SPI_1_PA5_PA6_PA7 1 /* Barometer */ +#define SPI_1_PB3_PB4_PB5 1 /* Accelerometer */ +#define SPI_1_PE13_PE14_PE15 0 +#define SPI_1_OSPEEDR STM_OSPEEDR_10MHz + +#define HAS_SPI_2 1 +#define SPI_2_PB13_PB14_PB15 1 /* Flash, Companion, Radio */ +#define SPI_2_PD1_PD3_PD4 0 +#define SPI_2_OSPEEDR STM_OSPEEDR_10MHz + +#define SPI_2_PORT (&stm_gpiob) +#define SPI_2_SCK_PIN 13 +#define SPI_2_MISO_PIN 14 +#define SPI_2_MOSI_PIN 15 + +#define HAS_I2C_1 0 +#define I2C_1_PB8_PB9 0 + +#define HAS_I2C_2 0 +#define I2C_2_PB10_PB11 0 + +#define PACKET_HAS_SLAVE 1 +#define PACKET_HAS_MASTER 0 + +#define LOW_LEVEL_DEBUG 0 + +#define LED_PORT_ENABLE STM_RCC_AHBENR_GPIOCEN +#define LED_PORT (&stm_gpioc) +#define LED_PIN_RED 14 +#define LED_PIN_GREEN 15 +#define AO_LED_RED (1 << LED_PIN_RED) +#define AO_LED_GREEN (1 << LED_PIN_GREEN) + +#define LEDS_AVAILABLE (AO_LED_RED | AO_LED_GREEN) + +#define HAS_GPS 1 +#define HAS_FLIGHT 1 +#define HAS_ADC 1 +#define HAS_ADC_TEMP 1 +#define HAS_LOG 1 + +/* + * Igniter + */ + +#define HAS_IGNITE 1 +#define HAS_IGNITE_REPORT 1 + +#define AO_SENSE_DROGUE(p) ((p)->adc.sense_a) +#define AO_SENSE_MAIN(p) ((p)->adc.sense_m) +#define AO_IGNITER_CLOSED 400 +#define AO_IGNITER_OPEN 60 + +/* Drogue */ +#define AO_IGNITER_DROGUE_PORT (&stm_gpioa) +#define AO_IGNITER_DROGUE_PIN 8 + +/* Main */ +#define AO_IGNITER_MAIN_PORT (&stm_gpioa) +#define AO_IGNITER_MAIN_PIN 9 + +#define AO_IGNITER_SET_DROGUE(v) stm_gpio_set(AO_IGNITER_DROGUE_PORT, AO_IGNITER_DROGUE_PIN, v) +#define AO_IGNITER_SET_MAIN(v) stm_gpio_set(AO_IGNITER_MAIN_PORT, AO_IGNITER_MAIN_PIN, v) + +/* + * ADC + */ +#define AO_DATA_RING 32 +#define AO_ADC_NUM_SENSE 2 + +struct ao_adc { + int16_t sense_a; + int16_t sense_m; + int16_t v_batt; + int16_t temp; +}; + +#define AO_ADC_DUMP(p) \ + printf("tick: %5u drogue: %5d main: %5d batt: %5d\n", \ + (p)->tick, \ + (p)->adc.sense_a, (p)->adc.sense_m, \ + (p)->adc.v_batt); + +#define AO_ADC_SENSE_DROGUE 0 +#define AO_ADC_SENSE_DROGUE_PORT (&stm_gpioa) +#define AO_ADC_SENSE_DROGUE_PIN 0 + +#define AO_ADC_SENSE_MAIN 1 +#define AO_ADC_SENSE_MAIN_PORT (&stm_gpioa) +#define AO_ADC_SENSE_MAIN_PIN 1 + +#define AO_ADC_V_BATT 8 +#define AO_ADC_V_BATT_PORT (&stm_gpiob) +#define AO_ADC_V_BATT_PIN 0 + +#define AO_ADC_TEMP 16 + +#define AO_ADC_RCC_AHBENR ((1 << STM_RCC_AHBENR_GPIOAEN) | \ + (1 << STM_RCC_AHBENR_GPIOEEN) | \ + (1 << STM_RCC_AHBENR_GPIOBEN)) + +#define AO_NUM_ADC_PIN 3 + +#define AO_ADC_PIN0_PORT AO_ADC_SENSE_DROGUE_PORT +#define AO_ADC_PIN0_PIN AO_ADC_SENSE_DROGUE_PIN +#define AO_ADC_PIN1_PORT AO_ADC_SENSE_MAIN_PORT +#define AO_ADC_PIN1_PIN AO_ADC_SENSE_MAIN_PIN +#define AO_ADC_PIN2_PORT AO_ADC_V_BATT_PORT +#define AO_ADC_PIN2_PIN AO_ADC_V_BATT_PIN + +#define AO_NUM_ADC (AO_NUM_ADC_PIN + 1) + +#define AO_ADC_SQ1 AO_ADC_SENSE_DROGUE +#define AO_ADC_SQ2 AO_ADC_SENSE_MAIN +#define AO_ADC_SQ3 AO_ADC_V_BATT +#define AO_ADC_SQ4 AO_ADC_TEMP + +/* + * GPS + */ + +#define AO_SERIAL_SPEED_UBLOX AO_SERIAL_SPEED_9600 + +#define ao_gps_getchar ao_serial3_getchar +#define ao_gps_putchar ao_serial3_putchar +#define ao_gps_set_speed ao_serial3_set_speed +#define ao_gps_fifo (ao_stm_usart3.rx_fifo) + +/* + * Pressure sensor settings + */ +#define HAS_MS5607 1 +#define HAS_MS5611 0 +#define AO_MS5607_PRIVATE_PINS 1 +#define AO_MS5607_CS_PORT (&stm_gpiob) +#define AO_MS5607_CS_PIN 12 +#define AO_MS5607_CS_MASK (1 << AO_MS5607_CS) +#define AO_MS5607_MISO_PORT (&stm_gpioa) +#define AO_MS5607_MISO_PIN 6 +#define AO_MS5607_MISO_MASK (1 << AO_MS5607_MISO) +#define AO_MS5607_SPI_INDEX AO_SPI_1_PA5_PA6_PA7 + +/* + * SPI Flash memory + */ + +#define M25_MAX_CHIPS 1 +#define AO_M25_SPI_CS_PORT (&stm_gpiob) +#define AO_M25_SPI_CS_MASK (1 << 8) +#define AO_M25_SPI_BUS AO_SPI_2_PB13_PB14_PB15 + +/* + * Radio (cc1120) + */ + +/* gets pretty close to 434.550 */ + +#define AO_RADIO_CAL_DEFAULT 0x6ca333 + +#define AO_FEC_DEBUG 0 +#define AO_CC1120_SPI_CS_PORT (&stm_gpioa) +#define AO_CC1120_SPI_CS_PIN 2 +#define AO_CC1120_SPI_BUS AO_SPI_2_PB13_PB14_PB15 +#define AO_CC1120_SPI stm_spi2 + +#define AO_CC1120_INT_PORT (&stm_gpioa) +#define AO_CC1120_INT_PIN (3) +#define AO_CC1120_MCU_WAKEUP_PORT (&stm_gpioa) +#define AO_CC1120_MCU_WAKEUP_PIN (4) + +#define AO_CC1120_INT_GPIO 2 +#define AO_CC1120_INT_GPIO_IOCFG CC1120_IOCFG2 + +#define AO_CC1120_MARC_GPIO 3 +#define AO_CC1120_MARC_GPIO_IOCFG CC1120_IOCFG3 + +#define HAS_BOOT_RADIO 0 + +#define HAS_HIGHG_ACCEL 1 + +/* + * mma655x + */ + +#define HAS_MMA655X 1 +#define AO_MMA655X_SPI_INDEX AO_SPI_1_PB3_PB4_PB5 +#define AO_MMA655X_CS_PORT (&stm_gpiob) +#define AO_MMA655X_CS_PIN 9 +#define AO_MMA655X_INVERT 1 + +#define NUM_CMDS 16 + +/* + * Companion + */ + +#define AO_COMPANION_CS_PORT (&stm_gpiob) +#define AO_COMPANION_CS_PIN (6) +#define AO_COMPANION_SPI_BUS AO_SPI_2_PB13_PB14_PB15 + +/* + * Monitor + */ + +#define HAS_MONITOR 0 +#define LEGACY_MONITOR 0 +#define HAS_MONITOR_PUT 1 +#define AO_MONITOR_LED 0 +#define HAS_RSSI 0 + +/* + * Profiling Viterbi decoding + */ + +#ifndef AO_PROFILE +#define AO_PROFILE 0 +#endif + +#endif /* _AO_PINS_H_ */ diff --git a/src/telemetrum-v2.0/ao_telemetrum.c b/src/telemetrum-v2.0/ao_telemetrum.c new file mode 100644 index 00000000..46599cb2 --- /dev/null +++ b/src/telemetrum-v2.0/ao_telemetrum.c @@ -0,0 +1,93 @@ +/* + * Copyright © 2011 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_ms5607.h> +#include <ao_mma655x.h> +#include <ao_log.h> +#include <ao_exti.h> +#include <ao_packet.h> +#include <ao_companion.h> +#include <ao_eeprom.h> +#include <ao_profile.h> +#if HAS_SAMPLE_PROFILE +#include <ao_sample_profile.h> +#endif +#if HAS_STACK_GUARD +#include <ao_mpu.h> +#endif + +int +main(void) +{ + ao_clock_init(); + +#if HAS_STACK_GUARD + ao_mpu_init(); +#endif + + ao_task_init(); + ao_serial_init(); + ao_led_init(LEDS_AVAILABLE); + ao_led_on(AO_LED_RED); + ao_timer_init(); + + ao_spi_init(); + ao_dma_init(); + ao_exti_init(); + + ao_adc_init(); +#if HAS_BEEP + ao_beep_init(); +#endif + ao_cmd_init(); + +#if HAS_MS5607 + ao_ms5607_init(); +#endif +#if HAS_MMA655X + ao_mma655x_init(); +#endif + + ao_eeprom_init(); + + ao_storage_init(); + + ao_flight_init(); + ao_log_init(); + ao_report_init(); + + ao_usb_init(); + ao_gps_init(); + ao_gps_report_metrum_init(); + ao_telemetry_init(); + ao_radio_init(); + ao_packet_slave_init(FALSE); + ao_igniter_init(); + ao_companion_init(); + + ao_config_init(); +#if AO_PROFILE + ao_profile_init(); +#endif +#if HAS_SAMPLE_PROFILE + ao_sample_profile_init(); +#endif + + ao_start_scheduler(); + return 0; +} diff --git a/src/telemetrum-v2.0/flash-loader/Makefile b/src/telemetrum-v2.0/flash-loader/Makefile new file mode 100644 index 00000000..cb99c51e --- /dev/null +++ b/src/telemetrum-v2.0/flash-loader/Makefile @@ -0,0 +1,8 @@ +# +# AltOS flash loader build +# +# + +TOPDIR=../.. +HARDWARE=telemetrum-v2.0 +include $(TOPDIR)/stm/Makefile-flash.defs diff --git a/src/telemetrum-v2.0/flash-loader/ao_pins.h b/src/telemetrum-v2.0/flash-loader/ao_pins.h new file mode 100644 index 00000000..304bb7c3 --- /dev/null +++ b/src/telemetrum-v2.0/flash-loader/ao_pins.h @@ -0,0 +1,34 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +/* External crystal at 8MHz */ +#define AO_HSE 8000000 + +#include <ao_flash_stm_pins.h> + +/* Companion port cs_companion0 PB6 */ + +#define AO_BOOT_PIN 1 +#define AO_BOOT_APPLICATION_GPIO stm_gpiob +#define AO_BOOT_APPLICATION_PIN 6 +#define AO_BOOT_APPLICATION_VALUE 1 +#define AO_BOOT_APPLICATION_MODE AO_EXTI_MODE_PULL_UP + +#endif /* _AO_PINS_H_ */ diff --git a/src/telemini-v2.0/.gitignore b/src/telemini-v2.0/.gitignore new file mode 100644 index 00000000..568925a2 --- /dev/null +++ b/src/telemini-v2.0/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +telemini-v2.0* diff --git a/src/telemini-v2.0/.sdcdbrc b/src/telemini-v2.0/.sdcdbrc new file mode 100644 index 00000000..b9f6129c --- /dev/null +++ b/src/telemini-v2.0/.sdcdbrc @@ -0,0 +1,2 @@ +--directory=../cc1111:../product:../core:../drivers:. + diff --git a/src/telemini-v2.0/Makefile b/src/telemini-v2.0/Makefile new file mode 100644 index 00000000..fcac2c48 --- /dev/null +++ b/src/telemini-v2.0/Makefile @@ -0,0 +1,115 @@ +# +# TeleMini build file +# + +TELEMINI_VER=2.0 +TELEMINI_DEF=2_0 + +vpath %.c ..:../core:../cc1111:../drivers:../product +vpath %.h ..:../core:../cc1111:../drivers:../product +vpath ao-make-product.5c ../util + +ifndef VERSION +include ../Version +endif + +INC = \ + ao.h \ + ao_pins.h \ + ao_arch.h \ + ao_arch_funcs.h \ + cc1111.h \ + ao_ms5607.h \ + ao_ms5607_convert_8051.c \ + ao_product.h \ + ao_int64.h \ + ao_sample.h \ + ao_exti.h \ + ao_task.h + +CORE_SRC = \ + ao_cmd.c \ + ao_config.c \ + ao_convert.c \ + ao_flight.c \ + ao_kalman.c \ + ao_log.c \ + ao_log_mini.c \ + ao_mutex.c \ + ao_panic.c \ + ao_report.c \ + ao_sample.c \ + ao_stdio.c \ + ao_storage.c \ + ao_task.c \ + ao_telemetry.c \ + ao_freq.c \ + ao_int64.c + +CC1111_SRC = \ + ao_adc.c \ + ao_dma.c \ + ao_ignite.c \ + ao_led.c \ + ao_packet.c \ + ao_packet_slave.c \ + ao_radio.c \ + ao_romconfig.c \ + ao_string.c \ + ao_spi.c \ + ao_usb.c \ + ao_convert_pa.c \ + ao_beep.c \ + ao_timer.c \ + ao_exti.c \ + _bp.c + +DRIVER_SRC = \ + ao_ms5607.c \ + ao_m25.c + +PRODUCT_SRC = \ + ao_telemini.c + +SRC = \ + $(CORE_SRC) \ + $(CC1111_SRC) \ + $(DRIVER_SRC) \ + $(PRODUCT_SRC) + +PROGNAME = telemini-v$(TELEMINI_VER) +PROG = $(PROGNAME)-$(VERSION).ihx +PRODUCT=TeleMini-v$(TELEMINI_VER) +PRODUCT_DEF=-DTELEMINI_V_$(TELEMINI_DEF) +IDPRODUCT=0x0027 + +include ../cc1111/Makefile.cc1111 + +NICKLE=nickle +CHECK_STACK=sh ../util/check-stack + +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): $(REL) Makefile + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) + $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ + +ao_product.h: ao-make-product.5c ../Version + $(call quiet,NICKLE,$<) $< -m altusmetrum.org -i $(IDPRODUCT) -p $(PRODUCT) -v $(VERSION) > $@ + +distclean: clean + +clean: clean-cc1111 + +install: + +uninstall: + diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h new file mode 100644 index 00000000..c4681ee2 --- /dev/null +++ b/src/telemini-v2.0/ao_pins.h @@ -0,0 +1,161 @@ +/* + * Copyright © 2010 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +#define HAS_RADIO 1 + +#define HAS_FLIGHT 1 +#define HAS_USB 1 +#define USB_FORCE_FLIGHT_IDLE 1 +#define HAS_BEEP 1 +#define HAS_GPS 0 +#define HAS_SERIAL_1 0 +#define HAS_EEPROM 1 +#define HAS_LOG 1 +#define USE_INTERNAL_FLASH 0 +#define HAS_DBG 0 +#define PACKET_HAS_SLAVE 1 + +#define AO_LED_GREEN 1 +#define AO_LED_RED 2 +#define LEDS_AVAILABLE (AO_LED_RED|AO_LED_GREEN) +#define HAS_EXTERNAL_TEMP 0 +#define HAS_ACCEL 0 +#define HAS_IGNITE 1 +#define HAS_IGNITE_REPORT 1 +#define HAS_MONITOR 0 + +/* + * SPI + */ + +#define HAS_SPI_0 1 +#define SPI_0_ALT_1 1 +#define HAS_SPI_1 1 +#define SPI_1_ALT_2 1 +#define SPI_CS_PORT P1 +#define SPI_CS_SEL P1SEL +#define SPI_CS_DIR P1DIR + +/* + * Flash + */ +#define AO_M25_SPI_BUS 1 +#define AO_M25_SPI_CS_PORT SPI_CS_PORT +#define AO_M25_SPI_CS_MASK 0x04 /* cs_flash is P1_2 */ +#define M25_MAX_CHIPS 1 + +/* + * MS5607 + */ + +#define HAS_MS5607 1 +#define HAS_MS5611 0 +#define AO_MS5607_PRIVATE_PINS 0 +#define AO_MS5607_CS_PORT P1 +#define AO_MS5607_CS_PIN 3 +#define AO_MS5607_CS P1_3 +#define AO_MS5607_CS_MASK (1 << AO_MS5607_CS_PIN) +#define AO_MS5607_MISO_PORT P0 +#define AO_MS5607_MISO_PIN 2 +#define AO_MS5607_MISO P0_2 +#define AO_MS5607_MISO_MASK (1 << AO_MS5607_MISO_PIN) +#define AO_MS5607_SPI_INDEX 0 +#define HAS_EXTI_0 1 + +/* + * Igniters + */ +#define AO_IGNITER_PORT P2 +#define AO_IGNITER_DROGUE_PORT AO_IGNITER_PORT +#define AO_IGNITER_DROGUE P2_3 +#define AO_IGNITER_MAIN P2_4 +#define AO_IGNITER_DIR P2DIR +#define AO_IGNITER_DROGUE_BIT (1 << 3) +#define AO_IGNITER_MAIN_BIT (1 << 4) +#define AO_IGNITER_DROGUE_PIN 3 +#define AO_IGNITER_MAIN_PIN 4 + +#define AO_IGNITER_DROGUE_PORT AO_IGNITER_PORT +#define AO_IGNITER_MAIN_PORT AO_IGNITER_PORT + +/* test these values with real igniters */ +#define AO_IGNITER_OPEN 1000 +#define AO_IGNITER_CLOSED 7000 +#define AO_IGNITER_FIRE_TIME AO_MS_TO_TICKS(50) +#define AO_IGNITER_CHARGE_TIME AO_MS_TO_TICKS(2000) + +#define AO_SEND_MINI +#define AO_LOG_FORMAT AO_LOG_FORMAT_TELEMINI + +/* + * ADC + */ + +#define HAS_ADC 1 +#define AO_ADC_FIRST_PIN 0 + +struct ao_adc { + int16_t sense_a; /* apogee continuity sense */ + int16_t sense_m; /* main continuity sense */ + int16_t v_batt; /* battery voltage */ +}; + +#define ao_data_count ao_adc_count + +#define AO_SENSE_DROGUE(p) ((p)->adc.sense_a) +#define AO_SENSE_MAIN(p) ((p)->adc.sense_m) + +#define AO_NUM_TASKS 10 + +#define AO_ADC_DUMP(p) \ + printf("tick: %5u apogee: %5d main: %5d batt: %5d\n", \ + (p)->tick, (p)->adc.sense_a, (p)->adc.sense_m, (p)->adc.v_batt) + +#define AO_ADC_PINS ((1 << 0) | (1 << 1) | (1 << 4)) + +#define FETCH_ADC() do { \ + a = (uint8_t __xdata *) (&ao_data_ring[ao_data_head].adc); \ + switch (sequence) { \ + case 4: \ + a += 4; \ + sequence = 0; \ + break; \ + case 1: \ + a += 2; \ + sequence = 4; \ + break; \ + case 0: \ + sequence = 1; \ + break; \ + } \ + a[0] = ADCL; \ + a[1] = ADCH; \ + if (sequence) { \ + ADCCON3 = ADCCON3_EREF_VDD | ADCCON3_EDIV_512 | sequence; \ + return; \ + } \ + AO_DATA_PRESENT(AO_DATA_ADC); \ + if (ao_data_present != AO_DATA_ALL) \ + return; \ + ao_data_ring[ao_data_head].ms5607_raw.pres = ao_ms5607_current.pres; \ + ao_data_ring[ao_data_head].ms5607_raw.temp = ao_ms5607_current.temp; \ + } while (0) + +#endif /* _AO_PINS_H_ */ diff --git a/src/telemini-v2.0/ao_telemini.c b/src/telemini-v2.0/ao_telemini.c new file mode 100644 index 00000000..0d8dd1cb --- /dev/null +++ b/src/telemini-v2.0/ao_telemini.c @@ -0,0 +1,65 @@ +/* + * Copyright © 2011 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include "ao.h" +#include "ao_pins.h" +#include <ao_exti.h> + +__xdata uint8_t ao_force_freq; + +void +main(void) +{ + ao_clock_init(); + +#if HAS_STACK_GUARD + ao_mpu_init(); +#endif + ao_task_init(); + + /* Turn on the red LED until the system is stable */ + ao_led_init(LEDS_AVAILABLE); + ao_led_on(AO_LED_RED); + + ao_timer_init(); + + ao_spi_init(); + ao_exti_init(); + ao_adc_init(); +#if HAS_BEEP + ao_beep_init(); +#endif + ao_cmd_init(); +#if HAS_MS5607 + ao_ms5607_init(); +#endif + ao_storage_init(); + + ao_flight_init(); + ao_log_init(); + ao_report_init(); + + ao_usb_init(); + ao_telemetry_init(); + ao_radio_init(); + ao_packet_slave_init(TRUE); + + ao_igniter_init(); + + ao_config_init(); + ao_start_scheduler(); +} diff --git a/src/telepyro-v0.1/Makefile b/src/telepyro-v0.1/Makefile index 6743ba66..025b324a 100644 --- a/src/telepyro-v0.1/Makefile +++ b/src/telepyro-v0.1/Makefile @@ -5,18 +5,12 @@ vpath % .:..:../core:../product:../drivers:../avr vpath ao-make-product.5c ../util +include ../avr/Makefile.defs + MCU=atmega32u4 DUDECPUTYPE=m32u4 #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 INC = \ ao.h \ diff --git a/src/telescience-pwm/.gitignore b/src/telescience-pwm/.gitignore index dfccadf8..5d9c0970 100644 --- a/src/telescience-pwm/.gitignore +++ b/src/telescience-pwm/.gitignore @@ -1,2 +1,2 @@ -telescience-v0.1* +telescience-pwm* ao_product.h diff --git a/src/telescience-pwm/Makefile b/src/telescience-pwm/Makefile index 43d77e2e..de81b8d7 100644 --- a/src/telescience-pwm/Makefile +++ b/src/telescience-pwm/Makefile @@ -5,18 +5,14 @@ vpath % ..:../core:../product:../drivers:../avr vpath ao-make-product.5c ../util +include ../avr/Makefile.defs + MCU=atmega32u4 DUDECPUTYPE=m32u4 #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 +#LDFLAGS=-L$(LDSCRIPTS) -Tavr5.x INC = \ ao.h \ diff --git a/src/telescience-v0.1/Makefile b/src/telescience-v0.1/Makefile index d24128ef..6e4eb6de 100644 --- a/src/telescience-v0.1/Makefile +++ b/src/telescience-v0.1/Makefile @@ -5,18 +5,14 @@ vpath % ..:../core:../product:../drivers:../avr vpath ao-make-product.5c ../util +include ../avr/Makefile.defs + MCU=atmega32u4 DUDECPUTYPE=m32u4 #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 +#LDFLAGS=-L$(LDSCRIPTS) -Tavr5.x INC = \ ao.h \ diff --git a/src/telescience-v0.2/Makefile b/src/telescience-v0.2/Makefile index f16ef268..6b7ea8c7 100644 --- a/src/telescience-v0.2/Makefile +++ b/src/telescience-v0.2/Makefile @@ -61,14 +61,15 @@ CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STA PROGNAME=telescience-v0.2 PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx SRC=$(ALTOS_SRC) ao_telescience.c OBJ=$(SRC:.c=.o) -all: $(PROG) +all: $(PROG) $(HEX) $(PROG): Makefile $(OBJ) altos.ld - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) $(OBJ): $(INC) @@ -78,7 +79,7 @@ ao_product.h: ao-make-product.5c ../Version distclean: clean clean: - rm -f *.o $(PROGNAME)-*.elf + rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx rm -f ao_product.h install: diff --git a/src/telescience-v0.2/flash-loader/Makefile b/src/telescience-v0.2/flash-loader/Makefile new file mode 100644 index 00000000..7a2ceb77 --- /dev/null +++ b/src/telescience-v0.2/flash-loader/Makefile @@ -0,0 +1,8 @@ +# +# AltOS flash loader build +# +# + +TOPDIR=../.. +HARDWARE=telescience-v0.2 +include $(TOPDIR)/stm/Makefile-flash.defs diff --git a/src/telescience-v0.2/flash-loader/ao_pins.h b/src/telescience-v0.2/flash-loader/ao_pins.h new file mode 100644 index 00000000..907ff341 --- /dev/null +++ b/src/telescience-v0.2/flash-loader/ao_pins.h @@ -0,0 +1,34 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +/* External crystal at 8MHz */ +#define AO_HSE 8000000 + +#include <ao_flash_stm_pins.h> + +/* Companion port SS PA4 */ + +#define AO_BOOT_PIN 1 +#define AO_BOOT_APPLICATION_GPIO stm_gpioa +#define AO_BOOT_APPLICATION_PIN 4 +#define AO_BOOT_APPLICATION_VALUE 1 +#define AO_BOOT_APPLICATION_MODE AO_EXTI_MODE_PULL_UP + +#endif /* _AO_PINS_H_ */ diff --git a/src/teleshield-v0.1/Makefile b/src/teleshield-v0.1/Makefile index ab2a025f..e8b262ef 100644 --- a/src/teleshield-v0.1/Makefile +++ b/src/teleshield-v0.1/Makefile @@ -92,10 +92,10 @@ endif # Otherwise, print the full command line. quiet ?= $($1) -all: ../$(PROG) +all: $(PROG) -../$(PROG): $(REL) Makefile - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. +$(PROG): $(REL) Makefile + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ ao_product.h: ao-make-product.5c ../Version diff --git a/src/teleterra-v0.2/Makefile b/src/teleterra-v0.2/Makefile index 65db57ce..88637360 100644 --- a/src/teleterra-v0.2/Makefile +++ b/src/teleterra-v0.2/Makefile @@ -52,7 +52,8 @@ CC1111_SRC = \ DRIVER_SRC = \ ao_m25.c \ ao_lcd.c \ - ao_gps_skytraq.c + ao_gps_skytraq.c \ + ao_gps_show.c PRODUCT_SRC = \ ao_teleterra_0_2.c \ @@ -85,10 +86,10 @@ endif # Otherwise, print the full command line. quiet ?= $($1) -all: ../$(PROG) +all: $(PROG) -../$(PROG): $(REL) Makefile - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. +$(PROG): $(REL) Makefile + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ ao_product.h: ao-make-product.5c ../Version diff --git a/src/test/.gitignore b/src/test/.gitignore index 90af6517..b8b2513e 100644 --- a/src/test/.gitignore +++ b/src/test/.gitignore @@ -5,6 +5,10 @@ ao_flight_test_baro ao_flight_test_accel ao_gps_test ao_gps_test_skytraq +ao_gps_test_ublox +ao_int64_test +ao_ms5607_convert_test +ao_quaternion_test ao_convert_test ao_convert_pa_test ao_fat_test diff --git a/src/test/Makefile b/src/test/Makefile index 76d50f16..7ae06a80 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -1,14 +1,15 @@ -vpath % ..:../core:../drivers:../util:../micropeak:../aes +vpath % ..:../core:../drivers:../util:../micropeak:../aes:../product PROGS=ao_flight_test ao_flight_test_baro ao_flight_test_accel ao_flight_test_noisy_accel ao_flight_test_mm \ ao_gps_test ao_gps_test_skytraq ao_gps_test_ublox ao_convert_test ao_convert_pa_test ao_fec_test \ - ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test + ao_aprs_test ao_micropeak_test ao_fat_test ao_aes_test ao_int64_test \ + ao_ms5607_convert_test ao_quaternion_test -INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h +INCS=ao_kalman.h ao_ms5607.h ao_log.h ao_data.h altitude-pa.h altitude.h ao_quaternion.h KALMAN=make-kalman -CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -O0 -g -Wall +CFLAGS=-I.. -I. -I../core -I../drivers -I../micropeak -I../product -O0 -g -Wall all: $(PROGS) ao_aprs_data.wav @@ -29,16 +30,16 @@ ao_flight_test_baro: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalm ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS) cc $(CFLAGS) -o $@ -DFORCE_ACCEL=1 ao_flight_test.c -ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS) +ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c ao_pyro.c ao_pyro.h $(INCS) cc -DTELEMEGA=1 $(CFLAGS) -o $@ $< -lm ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h cc $(CFLAGS) -o $@ $< -ao_gps_test_skytraq: ao_gps_test_skytraq.c ao_gps_skytraq.c ao_gps_print.c ao_host.h +ao_gps_test_skytraq: ao_gps_test_skytraq.c ao_gps_skytraq.c ao_gps_print.c ao_gps_show.c ao_host.h cc $(CFLAGS) -o $@ $< -ao_gps_test_ublox: ao_gps_test_ublox.c ao_gps_ublox.c ao_gps_print.c ao_host.h ao_gps_ublox.h +ao_gps_test_ublox: ao_gps_test_ublox.c ao_gps_ublox.c ao_gps_print.c ao_gps_show.c ao_host.h ao_gps_ublox.h cc $(CFLAGS) -o $@ $< ao_convert_test: ao_convert_test.c ao_convert.c altitude.h @@ -73,3 +74,12 @@ ao_fat_test: ao_fat_test.c ao_fat.c ao_bufio.c ao_aes_test: ao_aes_test.c ao_aes.c ao_aes_tables.c cc $(CFLAGS) -o $@ ao_aes_test.c + +ao_int64_test: ao_int64_test.c ao_int64.c ao_int64.h + cc $(CFLAGS) -o $@ ao_int64_test.c + +ao_ms5607_convert_test: ao_ms5607_convert_test.c ao_ms5607_convert_8051.c ao_int64.c ao_int64.h + cc $(CFLAGS) -o $@ ao_ms5607_convert_test.c + +ao_quaternion_test: ao_quaternion_test.c ao_quaternion.h + cc $(CFLAGS) -o $@ ao_quaternion_test.c -lm diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 99bed7ee..0abb4090 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -20,10 +20,13 @@ #include <stdint.h> #include <stdio.h> #include <stdlib.h> +#include <stddef.h> #include <string.h> #include <getopt.h> #include <math.h> +#define GRAVITY 9.80665 + #define AO_HERTZ 100 #define HAS_ADC 1 @@ -35,11 +38,17 @@ #define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) #define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + +int ao_gps_new; + #if TELEMEGA #define AO_ADC_NUM_SENSE 6 #define HAS_MS5607 1 #define HAS_MPU6000 1 #define HAS_MMA655X 1 +#define HAS_HMC5883 1 struct ao_adc { int16_t sense[AO_ADC_NUM_SENSE]; @@ -83,6 +92,95 @@ struct ao_adc { #include <ao_data.h> #include <ao_log.h> +#include <ao_telemetry.h> + +#if TELEMEGA +int ao_gps_count; +struct ao_telemetry_location ao_gps_first; +struct ao_telemetry_location ao_gps_prev; +struct ao_telemetry_location ao_gps_static; + +struct ao_telemetry_satellite ao_gps_tracking; + +static inline double sqr(double a) { return a * a; } + +void +cc_great_circle (double start_lat, double start_lon, + double end_lat, double end_lon, + double *dist, double *bearing) +{ + const double rad = M_PI / 180; + const double earth_radius = 6371.2 * 1000; /* in meters */ + double lat1 = rad * start_lat; + double lon1 = rad * -start_lon; + double lat2 = rad * end_lat; + double lon2 = rad * -end_lon; + +// double d_lat = lat2 - lat1; + double d_lon = lon2 - lon1; + + /* From http://en.wikipedia.org/wiki/Great-circle_distance */ + double vdn = sqrt(sqr(cos(lat2) * sin(d_lon)) + + sqr(cos(lat1) * sin(lat2) - + sin(lat1) * cos(lat2) * cos(d_lon))); + double vdd = sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(d_lon); + double d = atan2(vdn,vdd); + double course; + + if (cos(lat1) < 1e-20) { + if (lat1 > 0) + course = M_PI; + else + course = -M_PI; + } else { + if (d < 1e-10) + course = 0; + else + course = acos((sin(lat2)-sin(lat1)*cos(d)) / + (sin(d)*cos(lat1))); + if (sin(lon2-lon1) > 0) + course = 2 * M_PI-course; + } + *dist = d * earth_radius; + *bearing = course * 180/M_PI; +} + +double +ao_distance_from_pad(void) +{ + double dist, bearing; + if (!ao_gps_count) + return 0; + + cc_great_circle(ao_gps_first.latitude / 1e7, + ao_gps_first.longitude / 1e7, + ao_gps_static.latitude / 1e7, + ao_gps_static.longitude / 1e7, + &dist, &bearing); + return dist; +} + +double +ao_gps_angle(void) +{ + double dist, bearing; + double height; + double angle; + + if (ao_gps_count < 2) + return 0; + + cc_great_circle(ao_gps_prev.latitude / 1e7, + ao_gps_prev.longitude / 1e7, + ao_gps_static.latitude / 1e7, + ao_gps_static.longitude / 1e7, + &dist, &bearing); + height = ao_gps_static.altitude - ao_gps_prev.altitude; + + angle = atan2(dist, height); + return angle * 180/M_PI; +} +#endif #define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) #define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) @@ -120,6 +218,7 @@ int ao_summary = 0; #define ao_rdf_set(rdf) #define ao_packet_slave_start() #define ao_packet_slave_stop() +#define flush() enum ao_igniter { ao_igniter_drogue = 0, @@ -137,6 +236,18 @@ int tick_offset; static int32_t ao_k_height; +int16_t +ao_time(void) +{ + return ao_data_static.tick; +} + +void +ao_delay(int16_t interval) +{ + return; +} + void ao_ignite(enum ao_igniter igniter) { @@ -200,6 +311,8 @@ struct ao_cmds { #include <ao_ms5607.h> struct ao_ms5607_prom ms5607_prom; #include "ao_ms5607_convert.c" +#define AO_PYRO_NUM 4 +#include <ao_pyro.h> #else #include "ao_convert.c" #endif @@ -210,6 +323,12 @@ struct ao_config { int16_t accel_minus_g; uint8_t pad_orientation; uint16_t apogee_lockout; +#if TELEMEGA + struct ao_pyro pyro[AO_PYRO_NUM]; /* minor version 12 */ + int16_t accel_zero_along; + int16_t accel_zero_across; + int16_t accel_zero_through; +#endif }; #define AO_PAD_ORIENTATION_ANTENNA_UP 0 @@ -222,7 +341,6 @@ struct ao_config ao_config; #define DATA_TO_XDATA(x) (x) -#define GRAVITY 9.80665 extern int16_t ao_ground_accel, ao_flight_accel; extern int16_t ao_accel_2g; @@ -246,6 +364,22 @@ uint16_t prev_tick; #include "ao_sqrt.c" #include "ao_sample.c" #include "ao_flight.c" +#if TELEMEGA +#define AO_PYRO_NUM 4 + +#define AO_PYRO_0 0 +#define AO_PYRO_1 1 +#define AO_PYRO_2 2 +#define AO_PYRO_3 3 + +static void +ao_pyro_pin_set(uint8_t pin, uint8_t value) +{ + printf ("set pyro %d %d\n", pin, value); +} + +#include "ao_pyro.c" +#endif #define to_double(f) ((f) / 65536.0) @@ -305,17 +439,20 @@ ao_test_exit(void) exit(0); } -#if HAS_MPU6000 -static double -ao_mpu6000_accel(int16_t sensor) -{ - return sensor / 32767.0 * MPU6000_ACCEL_FULLSCALE * GRAVITY; -} +#ifdef TELEMEGA +struct ao_azel { + int az; + int el; +}; -static double -ao_mpu6000_gyro(int32_t sensor) +static void +azel (struct ao_azel *r, struct ao_quaternion *q) { - return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE; + double v; + + r->az = floor (atan2(q->y, q->x) * 180/M_PI + 0.5); + v = sqrt (q->x*q->x + q->y*q->y); + r->el = floor (atan2(q->z, v) * 180/M_PI + 0.5); } #endif @@ -342,6 +479,7 @@ ao_insert(void) double height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height; #endif + (void) accel; if (!tick_offset) tick_offset = -ao_data_static.tick; if ((prev_tick - ao_data_static.tick) > 0x400) @@ -372,25 +510,115 @@ ao_insert(void) } if (!ao_summary) { +#if TELEMEGA + static struct ao_quaternion ao_ground_mag; + static int ao_ground_mag_set; + + if (!ao_ground_mag_set) { + ao_quaternion_init_vector (&ao_ground_mag, + ao_data_mag_across(&ao_data_static), + ao_data_mag_through(&ao_data_static), + ao_data_mag_along(&ao_data_static)); + ao_quaternion_normalize(&ao_ground_mag, &ao_ground_mag); + ao_quaternion_rotate(&ao_ground_mag, &ao_ground_mag, &ao_rotation); + ao_ground_mag_set = 1; + } + + struct ao_quaternion ao_mag, ao_mag_rot; + + ao_quaternion_init_vector(&ao_mag, + ao_data_mag_across(&ao_data_static), + ao_data_mag_through(&ao_data_static), + ao_data_mag_along(&ao_data_static)); + + ao_quaternion_normalize(&ao_mag, &ao_mag); + ao_quaternion_rotate(&ao_mag_rot, &ao_mag, &ao_rotation); + + float ao_dot; + int ao_mag_angle; + + ao_dot = ao_quaternion_dot(&ao_mag_rot, &ao_ground_mag); + + struct ao_azel ground_azel, mag_azel, rot_azel; + + azel(&ground_azel, &ao_ground_mag); + azel(&mag_azel, &ao_mag); + azel(&rot_azel, &ao_mag_rot); + + ao_mag_angle = floor (acos(ao_dot) * 180 / M_PI + 0.5); + + (void) ao_mag_angle; + + static struct ao_quaternion ao_x = { .r = 0, .x = 1, .y = 0, .z = 0 }; + struct ao_quaternion ao_out; + + ao_quaternion_rotate(&ao_out, &ao_x, &ao_rotation); + + int out = floor (atan2(ao_out.y, ao_out.x) * 180 / M_PI); + +#if 0 + printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d mag_tilt %4d mag_rot %4d\n", + time, + ao_state_names[ao_flight_state], + ao_k_height / 65536.0, + ao_sample_orient, out, + mag_azel.el, + mag_azel.az); +#endif + printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d dist %12.2f gps_tilt %4d gps_sats %2d\n", + time, + ao_state_names[ao_flight_state], + ao_k_height / 65536.0, + ao_sample_orient, out, + ao_distance_from_pad(), + (int) floor (ao_gps_angle() + 0.5), + (ao_gps_static.flags & 0xf) * 10); + +#if 0 + printf ("\t\tstate %-8.8s ground az: %4d el %4d mag az %4d el %4d rot az %4d el %4d el_diff %4d az_diff %4d angle %4d tilt %4d ground %8.5f %8.5f %8.5f cur %8.5f %8.5f %8.5f rot %8.5f %8.5f %8.5f\n", + ao_state_names[ao_flight_state], + ground_azel.az, ground_azel.el, + mag_azel.az, mag_azel.el, + rot_azel.az, rot_azel.el, + ground_azel.el - rot_azel.el, + ground_azel.az - rot_azel.az, + ao_mag_angle, + ao_sample_orient, + ao_ground_mag.x, + ao_ground_mag.y, + ao_ground_mag.z, + ao_mag.x, + ao_mag.y, + ao_mag.z, + ao_mag_rot.x, + ao_mag_rot.y, + ao_mag_rot.z); +#endif +#endif + +#if 0 printf("%7.2f height %8.2f accel %8.3f " #if TELEMEGA - "roll %8.3f angle %8.3f qangle %8.3f " - "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f " + "angle %5d " + "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f mag_x %8d mag_y %8d, mag_z %8d mag_angle %4d " #endif "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n", time, height, accel, #if TELEMEGA - ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0, - ao_mpu6000_gyro(ao_sample_angle) / 100.0, - ao_sample_qangle, + ao_sample_orient, + ao_mpu6000_accel(ao_data_static.mpu6000.accel_x), ao_mpu6000_accel(ao_data_static.mpu6000.accel_y), ao_mpu6000_accel(ao_data_static.mpu6000.accel_z), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y), ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z), + ao_data_static.hmc5883.x, + ao_data_static.hmc5883.y, + ao_data_static.hmc5883.z, + ao_mag_angle, #endif ao_state_names[ao_flight_state], ao_k_height / 65536.0, @@ -400,6 +628,7 @@ ao_insert(void) drogue_height, main_height, ao_error_h_sq_avg); +#endif // if (ao_flight_state == ao_flight_landed) // ao_test_exit(); @@ -407,125 +636,6 @@ ao_insert(void) } } -#define AO_MAX_CALLSIGN 8 -#define AO_MAX_VERSION 8 -#define AO_MAX_TELEMETRY 128 - -struct ao_telemetry_generic { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t payload[27]; /* 5 */ - /* 32 */ -}; - -#define AO_TELEMETRY_SENSOR_TELEMETRUM 0x01 -#define AO_TELEMETRY_SENSOR_TELEMINI 0x02 -#define AO_TELEMETRY_SENSOR_TELENANO 0x03 - -struct ao_telemetry_sensor { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t state; /* 5 flight state */ - int16_t accel; /* 6 accelerometer (TM only) */ - int16_t pres; /* 8 pressure sensor */ - int16_t temp; /* 10 temperature sensor */ - int16_t v_batt; /* 12 battery voltage */ - int16_t sense_d; /* 14 drogue continuity sense (TM/Tm) */ - int16_t sense_m; /* 16 main continuity sense (TM/Tm) */ - - int16_t acceleration; /* 18 m/s² * 16 */ - int16_t speed; /* 20 m/s * 16 */ - int16_t height; /* 22 m */ - - int16_t ground_pres; /* 24 average pres on pad */ - int16_t ground_accel; /* 26 average accel on pad */ - int16_t accel_plus_g; /* 28 accel calibration at +1g */ - int16_t accel_minus_g; /* 30 accel calibration at -1g */ - /* 32 */ -}; - -#define AO_TELEMETRY_CONFIGURATION 0x04 - -struct ao_telemetry_configuration { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t device; /* 5 device type */ - uint16_t flight; /* 6 flight number */ - uint8_t config_major; /* 8 Config major version */ - uint8_t config_minor; /* 9 Config minor version */ - uint16_t apogee_delay; /* 10 Apogee deploy delay in seconds */ - uint16_t main_deploy; /* 12 Main deploy alt in meters */ - uint16_t flight_log_max; /* 14 Maximum flight log size in kB */ - char callsign[AO_MAX_CALLSIGN]; /* 16 Radio operator identity */ - char version[AO_MAX_VERSION]; /* 24 Software version */ - /* 32 */ -}; - -#define AO_TELEMETRY_LOCATION 0x05 - -#define AO_GPS_MODE_NOT_VALID 'N' -#define AO_GPS_MODE_AUTONOMOUS 'A' -#define AO_GPS_MODE_DIFFERENTIAL 'D' -#define AO_GPS_MODE_ESTIMATED 'E' -#define AO_GPS_MODE_MANUAL 'M' -#define AO_GPS_MODE_SIMULATED 'S' - -struct ao_telemetry_location { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t flags; /* 5 Number of sats and other flags */ - int16_t altitude; /* 6 GPS reported altitude (m) */ - int32_t latitude; /* 8 latitude (degrees * 10⁷) */ - int32_t longitude; /* 12 longitude (degrees * 10⁷) */ - uint8_t year; /* 16 (- 2000) */ - uint8_t month; /* 17 (1-12) */ - uint8_t day; /* 18 (1-31) */ - uint8_t hour; /* 19 (0-23) */ - uint8_t minute; /* 20 (0-59) */ - uint8_t second; /* 21 (0-59) */ - uint8_t pdop; /* 22 (m * 5) */ - uint8_t hdop; /* 23 (m * 5) */ - uint8_t vdop; /* 24 (m * 5) */ - uint8_t mode; /* 25 */ - uint16_t ground_speed; /* 26 cm/s */ - int16_t climb_rate; /* 28 cm/s */ - uint8_t course; /* 30 degrees / 2 */ - uint8_t unused[1]; /* 31 */ - /* 32 */ -}; - -#define AO_TELEMETRY_SATELLITE 0x06 - -struct ao_telemetry_satellite_info { - uint8_t svid; - uint8_t c_n_1; -}; - -struct ao_telemetry_satellite { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t channels; /* 5 number of reported sats */ - - struct ao_telemetry_satellite_info sats[12]; /* 6 */ - uint8_t unused[2]; /* 30 */ - /* 32 */ -}; - -union ao_telemetry_all { - struct ao_telemetry_generic generic; - struct ao_telemetry_sensor sensor; - struct ao_telemetry_configuration configuration; - struct ao_telemetry_location location; - struct ao_telemetry_satellite satellite; -}; uint16_t uint16(uint8_t *bytes, int off) @@ -555,207 +665,6 @@ int32(uint8_t *bytes, int off) static int log_format; -#if TELEMEGA - -static double -ao_vec_norm(double x, double y, double z) -{ - return x*x + y*y + z*z; -} - -static void -ao_vec_normalize(double *x, double *y, double *z) -{ - double scale = 1/sqrt(ao_vec_norm(*x, *y, *z)); - - *x *= scale; - *y *= scale; - *z *= scale; -} - -struct ao_quat { - double q0, q1, q2, q3; -}; - -static void -ao_quat_mul(struct ao_quat *r, struct ao_quat *a, struct ao_quat *b) -{ - r->q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3; - r->q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2; - r->q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1; - r->q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0; -} - -#if 0 -static void -ao_quat_scale(struct ao_quat *r, struct ao_quat *a, double s) -{ - r->q0 = a->q0 * s; - r->q1 = a->q1 * s; - r->q2 = a->q2 * s; - r->q3 = a->q3 * s; -} -#endif - -static void -ao_quat_conj(struct ao_quat *r, struct ao_quat *a) -{ - r->q0 = a->q0; - r->q1 = -a->q1; - r->q2 = -a->q2; - r->q3 = -a->q3; -} - -static void -ao_quat_rot(struct ao_quat *r, struct ao_quat *a, struct ao_quat *q) -{ - struct ao_quat t; - struct ao_quat c; - ao_quat_mul(&t, q, a); - ao_quat_conj(&c, q); - ao_quat_mul(r, &t, &c); -} - -static void -ao_quat_from_angle(struct ao_quat *r, - double x_rad, - double y_rad, - double z_rad) -{ - double angle = sqrt (x_rad * x_rad + y_rad * y_rad + z_rad * z_rad); - double s = sin(angle/2); - double c = cos(angle/2); - - r->q0 = c; - r->q1 = x_rad * s / angle; - r->q2 = y_rad * s / angle; - r->q3 = z_rad * s / angle; -} - -static void -ao_quat_from_vector(struct ao_quat *r, double x, double y, double z) -{ - ao_vec_normalize(&x, &y, &z); - double x_rad = atan2(z, y); - double y_rad = atan2(x, z); - double z_rad = atan2(y, x); - - ao_quat_from_angle(r, x_rad, y_rad, z_rad); -} - -static double -ao_quat_norm(struct ao_quat *a) -{ - return (a->q0 * a->q0 + - a->q1 * a->q1 + - a->q2 * a->q2 + - a->q3 * a->q3); -} - -static void -ao_quat_normalize(struct ao_quat *a) -{ - double norm = ao_quat_norm(a); - - if (norm) { - double m = 1/sqrt(norm); - - a->q0 *= m; - a->q1 *= m; - a->q2 *= m; - a->q3 *= m; - } -} - -static struct ao_quat ao_up, ao_current; -static struct ao_quat ao_orient; -static int ao_orient_tick; - -void -set_orientation(double x, double y, double z, int tick) -{ - struct ao_quat t; - - printf ("set_orientation %g %g %g\n", x, y, z); - ao_quat_from_vector(&ao_orient, x, y, z); - ao_up.q1 = ao_up.q2 = 0; - ao_up.q0 = ao_up.q3 = sqrt(2)/2; - ao_orient_tick = tick; - - ao_orient.q0 = 1; - ao_orient.q1 = 0; - ao_orient.q2 = 0; - ao_orient.q3 = 0; - - printf ("orient (%g) %g %g %g up (%g) %g %g %g\n", - ao_orient.q0, - ao_orient.q1, - ao_orient.q2, - ao_orient.q3, - ao_up.q0, - ao_up.q1, - ao_up.q2, - ao_up.q3); - - ao_quat_rot(&t, &ao_up, &ao_orient); - printf ("pad orient (%g) %g %g %g\n", - t.q0, - t.q1, - t.q2, - t.q3); - -} - -void -update_orientation (double rate_x, double rate_y, double rate_z, int tick) -{ - struct ao_quat q_dot; - double lambda; - double dt = (tick - ao_orient_tick) / 100.0; - - ao_orient_tick = tick; - -// lambda = 1 - ao_quat_norm(&ao_orient); - lambda = 0; - - q_dot.q0 = -0.5 * (ao_orient.q1 * rate_x + ao_orient.q2 * rate_y + ao_orient.q3 * rate_z) + lambda * ao_orient.q0; - q_dot.q1 = 0.5 * (ao_orient.q0 * rate_x + ao_orient.q2 * rate_z - ao_orient.q3 * rate_y) + lambda * ao_orient.q1; - q_dot.q2 = 0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2; - q_dot.q3 = 0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3; - -#if 0 - printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt); - printf ("q_dot (%g) %g %g %g\n", - q_dot.q0, - q_dot.q1, - q_dot.q2, - q_dot.q3); -#endif - - ao_orient.q0 += q_dot.q0 * dt; - ao_orient.q1 += q_dot.q1 * dt; - ao_orient.q2 += q_dot.q2 * dt; - ao_orient.q3 += q_dot.q3 * dt; - - ao_quat_normalize(&ao_orient); - - ao_quat_rot(&ao_current, &ao_up, &ao_orient); - - ao_sample_qangle = 180 / M_PI * acos(ao_current.q3 * sqrt(2)); -#if 0 - printf ("orient (%g) %g %g %g current (%g) %g %g %g\n", - ao_orient.q0, - ao_orient.q1, - ao_orient.q2, - ao_orient.q3, - ao_current.q0, - ao_current.q1, - ao_current.q2, - ao_current.q3); -#endif -} -#endif - void ao_sleep(void *wchan) { @@ -771,6 +680,10 @@ ao_sleep(void *wchan) char *words[64]; int nword; +#if TELEMEGA + if (ao_flight_state >= ao_flight_boost && ao_flight_state < ao_flight_landed) + ao_pyro_check(); +#endif for (;;) { if (ao_records_read > 2 && ao_flight_state == ao_flight_startup) { @@ -826,43 +739,31 @@ ao_sleep(void *wchan) ao_data_static.ms5607_raw.temp = int32(bytes, 4); ao_ms5607_convert(&ao_data_static.ms5607_raw, &value); ao_data_static.mpu6000.accel_x = int16(bytes, 8); - ao_data_static.mpu6000.accel_y = -int16(bytes, 10); + ao_data_static.mpu6000.accel_y = int16(bytes, 10); ao_data_static.mpu6000.accel_z = int16(bytes, 12); ao_data_static.mpu6000.gyro_x = int16(bytes, 14); - ao_data_static.mpu6000.gyro_y = -int16(bytes, 16); + ao_data_static.mpu6000.gyro_y = int16(bytes, 16); ao_data_static.mpu6000.gyro_z = int16(bytes, 18); + ao_data_static.hmc5883.x = int16(bytes, 20); + ao_data_static.hmc5883.y = int16(bytes, 22); + ao_data_static.hmc5883.z = int16(bytes, 24); #if HAS_MMA655X ao_data_static.mma655x = int16(bytes, 26); #endif - if (ao_records_read == 0) - ao_ground_mpu6000 = ao_data_static.mpu6000; - else if (ao_records_read < 10) { -#define f(f) ao_ground_mpu6000.f = ao_ground_mpu6000.f + ((ao_data_static.mpu6000.f - ao_ground_mpu6000.f) >> 2) - f(accel_x); - f(accel_y); - f(accel_z); - f(gyro_x); - f(gyro_y); - f(gyro_z); - - double accel_x = ao_mpu6000_accel(ao_ground_mpu6000.accel_x); - double accel_y = ao_mpu6000_accel(ao_ground_mpu6000.accel_y); - double accel_z = ao_mpu6000_accel(ao_ground_mpu6000.accel_z); - - /* X and Y are in the ground plane, arbitraryily picked as MPU X and Z axes - * Z is normal to the ground, the MPU y axis - */ - set_orientation(accel_x, accel_z, accel_y, tick); - } else { - double rate_x = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x); - double rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y); - double rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z); - - update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick); - } ao_records_read++; ao_insert(); return; + case 'G': + ao_gps_prev = ao_gps_static; + ao_gps_static.tick = tick; + ao_gps_static.latitude = int32(bytes, 0); + ao_gps_static.longitude = int32(bytes, 4); + ao_gps_static.altitude = int32(bytes, 8); + ao_gps_static.flags = bytes[13]; + if (!ao_gps_count) + ao_gps_first = ao_gps_static; + ao_gps_count++; + break; } continue; } else if (nword == 3 && strcmp(words[0], "ms5607") == 0) { @@ -883,6 +784,23 @@ ao_sleep(void *wchan) else if (strcmp(words[1], "crc:") == 0) ms5607_prom.crc = strtoul(words[2], NULL, 10); continue; + } else if (nword >= 3 && strcmp(words[0], "Pyro") == 0) { + int p = strtoul(words[1], NULL, 10); + int i, j; + struct ao_pyro *pyro = &ao_config.pyro[p]; + + for (i = 2; i < nword; i++) { + for (j = 0; j < NUM_PYRO_VALUES; j++) + if (!strcmp (words[2], ao_pyro_values[j].name)) + break; + if (j == NUM_PYRO_VALUES) + continue; + pyro->flags |= ao_pyro_values[j].flag; + if (ao_pyro_values[j].offset != NO_VALUE && i + 1 < nword) { + int16_t val = strtoul(words[++i], NULL, 10); + *((int16_t *) ((char *) pyro + ao_pyro_values[j].offset)) = val; + } + } } #else if (nword == 4 && log_format != AO_LOG_FORMAT_TELEMEGA) { @@ -899,6 +817,13 @@ ao_sleep(void *wchan) } else if (nword >= 6 && strcmp(words[0], "Accel") == 0) { ao_config.accel_plus_g = atoi(words[3]); ao_config.accel_minus_g = atoi(words[5]); +#ifdef TELEMEGA + } else if (nword >= 8 && strcmp(words[0], "IMU") == 0) { + ao_config.accel_zero_along = atoi(words[3]); + ao_config.accel_zero_across = atoi(words[5]); + ao_config.accel_zero_through = atoi(words[7]); + printf ("%d %d %d\n", ao_config.accel_zero_along, ao_config.accel_zero_across, ao_config.accel_zero_through); +#endif } else if (nword >= 4 && strcmp(words[0], "Main") == 0) { ao_config.main_deploy = atoi(words[2]); } else if (nword >= 3 && strcmp(words[0], "Apogee") == 0 && diff --git a/src/test/ao_gps_test.c b/src/test/ao_gps_test.c index 3844a326..e799ab0f 100644 --- a/src/test/ao_gps_test.c +++ b/src/test/ao_gps_test.c @@ -26,6 +26,9 @@ #define AO_GPS_NUM_SAT_MASK (0xf << 0) #define AO_GPS_NUM_SAT_SHIFT (0) +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + #define AO_GPS_VALID (1 << 4) #define AO_GPS_RUNNING (1 << 5) #define AO_GPS_DATE_VALID (1 << 6) @@ -427,11 +430,18 @@ void ao_dump_state(void *wchan) { int i; - if (wchan == &ao_gps_data) + + if (wchan != &ao_gps_new) + return; + + if (ao_gps_new & AO_GPS_NEW_DATA) { ao_gps_print(&ao_gps_data); - else + putchar('\n'); + } + if (ao_gps_new & AO_GPS_NEW_TRACKING) { ao_gps_tracking_print(&ao_gps_tracking_data); - putchar('\n'); + putchar('\n'); + } return; printf ("%02d:%02d:%02d", ao_gps_data.hour, ao_gps_data.minute, diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c index 81008b39..1b590d5e 100644 --- a/src/test/ao_gps_test_skytraq.c +++ b/src/test/ao_gps_test_skytraq.c @@ -26,6 +26,9 @@ #define AO_GPS_NUM_SAT_MASK (0xf << 0) #define AO_GPS_NUM_SAT_SHIFT (0) +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + #define AO_GPS_VALID (1 << 4) #define AO_GPS_RUNNING (1 << 5) #define AO_GPS_DATE_VALID (1 << 6) @@ -75,6 +78,11 @@ struct ao_gps_tracking_orig { #define ao_telemetry_satellite ao_gps_tracking_orig #define ao_telemetry_satellite_info ao_gps_sat_orig +extern __xdata struct ao_telemetry_location ao_gps_data; +extern __xdata struct ao_telemetry_satellite ao_gps_tracking_data; + +uint8_t ao_gps_mutex; + void ao_mutex_get(uint8_t *mutex) { @@ -432,17 +440,14 @@ uint8_t ao_task_minimize_latency; #define ao_usb_getchar() 0 #include "ao_gps_print.c" +#include "ao_gps_show.c" #include "ao_gps_skytraq.c" void ao_dump_state(void *wchan) { - if (wchan == &ao_gps_data) - ao_gps_print(&ao_gps_data); - else - ao_gps_tracking_print(&ao_gps_tracking_data); - putchar('\n'); - return; + if (wchan == &ao_gps_new) + ao_gps_show(); } int diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c index 80671735..4eb4b837 100644 --- a/src/test/ao_gps_test_ublox.c +++ b/src/test/ao_gps_test_ublox.c @@ -26,6 +26,9 @@ #define AO_GPS_NUM_SAT_MASK (0xf << 0) #define AO_GPS_NUM_SAT_SHIFT (0) +#define AO_GPS_NEW_DATA 1 +#define AO_GPS_NEW_TRACKING 2 + #define AO_GPS_VALID (1 << 4) #define AO_GPS_RUNNING (1 << 5) #define AO_GPS_DATE_VALID (1 << 6) @@ -77,6 +80,11 @@ struct ao_telemetry_satellite { #define ao_gps_tracking_orig ao_telemetry_satellite #define ao_gps_sat_orig ao_telemetry_satellite_info +extern __xdata struct ao_telemetry_location ao_gps_data; +extern __xdata struct ao_telemetry_satellite ao_gps_tracking_data; + +uint8_t ao_gps_mutex; + void ao_mutex_get(uint8_t *mutex) { @@ -125,7 +133,7 @@ static uint16_t recv_len; static void check_ublox_message(char *which, uint8_t *msg); char -ao_serial1_getchar(void) +ao_gps_getchar(void) { char c; uint8_t uc; @@ -158,7 +166,7 @@ static int message_len; static uint16_t send_len; void -ao_serial1_putchar(char c) +ao_gps_putchar(char c) { int i; uint8_t uc = (uint8_t) c; @@ -191,7 +199,7 @@ ao_serial1_putchar(char c) #define AO_SERIAL_SPEED_115200 3 static void -ao_serial1_set_speed(uint8_t speed) +ao_gps_set_speed(uint8_t speed) { int fd = ao_gps_fd; struct termios termios; @@ -224,6 +232,7 @@ uint8_t ao_task_minimize_latency; #define ao_usb_getchar() 0 #include "ao_gps_print.c" +#include "ao_gps_show.c" #include "ao_gps_ublox.c" static void @@ -341,11 +350,8 @@ check_ublox_message(char *which, uint8_t *msg) void ao_dump_state(void *wchan) { - if (wchan == &ao_gps_data) - ao_gps_print(&ao_gps_data); - else - ao_gps_tracking_print(&ao_gps_tracking_data); - putchar('\n'); + if (wchan == &ao_gps_new) + ao_gps_show(); return; } diff --git a/src/test/ao_int64_test.c b/src/test/ao_int64_test.c new file mode 100644 index 00000000..8557a1c7 --- /dev/null +++ b/src/test/ao_int64_test.c @@ -0,0 +1,115 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#define __data +#define __pdata +#define __xdata +#define __reentrant + +#include <ao_int64.h> +#include <ao_int64.c> +#include <stdio.h> +#include <stdlib.h> + +int errors; + +#define test_o(op,func,mod,a,b,ao_a,ao_b) do { \ + r = (a) op (b); \ + func(&ao_r, ao_a, ao_b); \ + c = ao_cast64(&ao_r); \ + if (c != r) { \ + printf ("trial %4d: %lld " #func mod " %lld = %lld (should be %lld)\n", \ + trial, (int64_t) (a), (int64_t) b, c, r); \ + ++errors; \ + } \ + } while (0) + +#define test(op,func,a,b,ao_a,ao_b) test_o(op,func,"",a,b,ao_a,ao_b) + +#define test_a(op,func,a,b,ao_a,ao_b) do { \ + ao_r = *ao_a; \ + test_o(op,func,"_a",a,b,&ao_r,ao_b); \ + } while (0) + +#define test_b(op,func,a,b,ao_a,ao_b) do { \ + ao_r = *ao_b; \ + test_o(op,func,"_b",a,b,ao_a,&ao_r); \ + } while (0) + +#define test_x(op,func,a,b,ao_a,ao_b) do { \ + ao_r = *ao_a; \ + test_o(op,func,"_xa",a,a,&ao_r,&ao_r); \ + ao_r = *ao_b; \ + test_o(op,func,"_xb",b,b,&ao_r,&ao_r); \ + } while (0) + +void +do_test(int trial, int64_t a, int64_t b) +{ + int64_t r, c; + ao_int64_t ao_a, ao_b, ao_r; + + ao_int64_init64(&ao_a, a >> 32, a); + ao_int64_init64(&ao_b, b >> 32, b); + + test(+, ao_plus64, a, b, &ao_a, &ao_b); + test_a(+, ao_plus64, a, b, &ao_a, &ao_b); + test_b(+, ao_plus64, a, b, &ao_a, &ao_b); + test_x(+, ao_plus64, a, b, &ao_a, &ao_b); + test(-, ao_minus64, a, b, &ao_a, &ao_b); + test_a(-, ao_minus64, a, b, &ao_a, &ao_b); + test_b(-, ao_minus64, a, b, &ao_a, &ao_b); + test_x(-, ao_minus64, a, b, &ao_a, &ao_b); + test(*, ao_mul64_32_32,(int64_t) (int32_t) a, (int32_t) b, (int32_t) a, (int32_t) b); + test(*, ao_mul64, a, b, &ao_a, &ao_b); + test_a(*, ao_mul64, a, b, &ao_a, &ao_b); + test_b(*, ao_mul64, a, b, &ao_a, &ao_b); + test_x(*, ao_mul64, a, b, &ao_a, &ao_b); + test(*, ao_mul64_64_16, a, (uint16_t) b, &ao_a, (uint16_t) b); + test_a(*, ao_mul64_64_16, a, (uint16_t) b, &ao_a, (uint16_t) b); + test(>>, ao_rshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f); + test_a(>>, ao_rshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f); + test(<<, ao_lshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f); + test_a(<<, ao_lshift64, a, (uint8_t) b & 0x3f, &ao_a, (uint8_t) b & 0x3f); +} + +#define TESTS 10000000 + +static int64_t +random64(void) +{ + return (int64_t) random() + ((int64_t) random() << 31) /* + ((int64_t) random() << 33) */; +} + +int +main (int argc, char **argv) +{ + int i, start; + + if (argv[1]) + start = atoi(argv[1]); + else + start = 0; + srandom(1000); + for (i = 0; i < TESTS; i++) { + int64_t a = random64(); + int64_t b = random64(); + if (i >= start) + do_test(i, a, b); + } + return errors; +} diff --git a/src/test/ao_ms5607_convert_test.c b/src/test/ao_ms5607_convert_test.c new file mode 100644 index 00000000..ad593204 --- /dev/null +++ b/src/test/ao_ms5607_convert_test.c @@ -0,0 +1,96 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#define __xdata +#define __data +#define __pdata +#define __reentrant + +#include <stdint.h> +#include <ao_ms5607.h> + +struct ao_ms5607_prom ms5607_prom = { + 0x002c, + 0xa6e0, + 0x988e, + 0x6814, + 0x5eff, + 0x8468, + 0x6c86, + 0xa271, +}; + +int32_t D1_mm = 6179630; +int32_t D2_mm = 8933155; + +#include <ao_ms5607_convert.c> +#define ao_ms5607_convert ao_ms5607_convert_8051 +#include <ao_ms5607_convert_8051.c> +#include <ao_int64.c> +#include <stdio.h> +#include <stdlib.h> + +struct ao_ms5607_sample ao_sample = { + 6179630, + 8933155 +}; + +int errors; + +void test(int trial, struct ao_ms5607_sample *sample) +{ + struct ao_ms5607_value value, value_8051; + + ao_ms5607_convert(sample, &value); + ao_ms5607_convert_8051(sample, &value_8051); + if (value.temp != value_8051.temp || value.pres != value_8051.pres) { + ++errors; + printf ("trial %d: %d, %d -> %d, %d (should be %d, %d)\n", + trial, + sample->pres, sample->temp, + value_8051.pres, value_8051.temp, + value.pres, value.temp); + } +} + +#define TESTS 10000000 + +#include <stdlib.h> + +static int32_t rand24(void) { return random() & 0xffffff; } + +int +main(int argc, char **argv) +{ + struct ao_ms5607_sample sample; + int i, start; + + if (argv[1]) + start = atoi(argv[1]); + else + start = 0; + + srandom(10000); + test(-1, &ao_sample); + for (i = 0; i < TESTS; i++) { + sample.pres = rand24(); + sample.temp = rand24(); + if (i >= start) + test(i, &sample); + } + return errors; +} diff --git a/src/test/ao_quaternion_test.c b/src/test/ao_quaternion_test.c new file mode 100644 index 00000000..b630f1d3 --- /dev/null +++ b/src/test/ao_quaternion_test.c @@ -0,0 +1,154 @@ +/* + * Copyright © 2013 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#define _GNU_SOURCE + +#include <stdint.h> +#include <stdio.h> +#include <stdlib.h> +#include <stddef.h> +#include <string.h> +#include <getopt.h> +#include <math.h> + +#include "ao_quaternion.h" + +#if 0 +static void +print_q(char *name, struct ao_quaternion *q) +{ + printf ("%8.8s: r%8.5f x%8.5f y%8.5f z%8.5f ", name, + q->r, q->x, q->y, q->z); +} +#endif + +#define STEPS 16 + +#define DEG (1.0f * 3.1415926535f / 180.0f) + +struct ao_rotation { + int steps; + float x, y, z; +}; + +static struct ao_rotation ao_path[] = { + { .steps = 45, .x = 2*DEG, .y = 0, .z = 0 }, + + { .steps = 45, .x = 0, .y = 2*DEG, .z = 0 }, + + { .steps = 45, .x = -2*DEG, .y = 0, .z = 0 }, + + { .steps = 45, .x = 0, .y = -2*DEG, .z = 0 }, +}; + +#define NUM_PATH (sizeof ao_path / sizeof ao_path[0]) + +static int close(float a, float b) { + return fabsf (a - b) < 1e-5; +} + +static int check_quaternion(char *where, struct ao_quaternion *got, struct ao_quaternion *expect) { + if (!close (got->r, expect->r) || + !close (got->x, expect->x) || + !close (got->y, expect->y) || + !close (got->z, expect->z)) + { + printf ("%s: got r%8.5f x%8.5f y%8.5f z%8.5f expect r%8.5f x%8.5f y%8.5f z%8.5f\n", + where, + got->r, got->x, got->y, got->z, + expect->r, expect->x, expect->y, expect->z); + return 1; + } + return 0; +} + +int main(int argc, char **argv) +{ + struct ao_quaternion position; + struct ao_quaternion position_expect; + struct ao_quaternion rotation; + struct ao_quaternion rotated; + struct ao_quaternion little_rotation; + int i; + int p; + int ret = 0; + + /* vector */ + ao_quaternion_init_vector(&position, 1, 1, 1); + ao_quaternion_init_vector(&position_expect, -1, -1, 1); + + /* zero rotation */ + ao_quaternion_init_zero_rotation(&rotation); + +#define dump() do { \ + \ + ao_quaternion_rotate(&rotated, &position, &rotation); \ + print_q("rotated", &rotated); \ + print_q("rotation", &rotation); \ + printf ("\n"); \ + } while (0) + +// dump(); + + for (p = 0; p < NUM_PATH; p++) { + ao_quaternion_init_half_euler(&little_rotation, + ao_path[p].x / 2.0f, + ao_path[p].y / 2.0f, + ao_path[p].z / 2.0f); +// printf ("\t\tx: %8.4f, y: %8.4f, z: %8.4f ", ao_path[p].x, ao_path[p].y, ao_path[p].z); +// print_q("step", &little_rotation); +// printf("\n"); + for (i = 0; i < ao_path[p].steps; i++) { + ao_quaternion_multiply(&rotation, &little_rotation, &rotation); + + ao_quaternion_normalize(&rotation, &rotation); + +// dump(); + } + } + + ao_quaternion_rotate(&rotated, &position, &rotation); + + ret += check_quaternion("rotation", &rotated, &position_expect); + + struct ao_quaternion vertical; + struct ao_quaternion angle; + struct ao_quaternion rot; + + ao_quaternion_init_vector(&vertical, 0, 0, 1); + ao_quaternion_init_vector(&angle, 0, 0, 1); + + ao_quaternion_init_half_euler(&rot, + M_PI * 3.0 / 8.0 , 0, 0); + + ao_quaternion_rotate(&angle, &angle, &rot); + + struct ao_quaternion rot_compute; + + ao_quaternion_vectors_to_rotation(&rot_compute, &vertical, &angle); + + ret += check_quaternion("vector rotation", &rot_compute, &rot); + + struct ao_quaternion rotd; + + ao_quaternion_rotate(&rotd, &vertical, &rot_compute); + + ret += check_quaternion("vector rotated", &rotd, &angle); + + return ret; +} + diff --git a/src/test/mega.flights b/src/test/mega.flights new file mode 100644 index 00000000..71c44e65 --- /dev/null +++ b/src/test/mega.flights @@ -0,0 +1,12 @@ +2013-10-12-serial-0094-flight-0001.eeprom +2013-09-02-serial-094-flight-002.eeprom +2013-03-02-serial-069-flight-002.eeprom +2013-03-02-serial-069-flight-001.eeprom +2013-05-27-serial-084-flight-001.mega +2013-05-26-serial-085-flight-002.mega +2013-05-19-serial-084-flight-003.mega +2013-04-21-serial-069-flight-002.mega +2012-10-20-serial-068-flight-004.mega +2012-07-03-serial-058-flight-007.mega +2012-06-30-serial-058-flight-006.mega +2012-06-30-serial-058-flight-005.mega diff --git a/src/test/plot-rot b/src/test/plot-rot new file mode 100755 index 00000000..b6b77c93 --- /dev/null +++ b/src/test/plot-rot @@ -0,0 +1,30 @@ +#!/bin/sh + +case $# in +1) + file="$1" + title="$1" + ;; +2) + file="$1" + title="$2" + ;; +*) + echo "Usage: $0 <data-file> <title>" + exit 1 +esac + +gnuplot -persist << EOF +set ylabel "altitude (m)" +set y2label "angle (d)" +set xlabel "time (s)" +set xtics border out nomirror +set ytics border out nomirror +set y2tics border out nomirror +set title "$title" +plot "$file" using 1:5 with lines axes x1y1 title "height",\ +"$file" using 1:9 with lines axes x1y2 title "gyro rot", \ +"$file" using 1:7 with lines axes x1y2 title "gyro tilt", \ +"$file" using 1:13 with lines axes x1y2 title "mag rot", \ +"$file" using 1:11 with lines axes x1y2 title "mag tilt" +EOF diff --git a/src/test/plotmm b/src/test/plotmm index 5f5bd2ca..27f8ddcd 100755 --- a/src/test/plotmm +++ b/src/test/plotmm @@ -1,11 +1,29 @@ +#!/bin/sh + +case $# in +1) + file="$1" + title="$1" + ;; +2) + file="$1" + title="$2" + ;; +*) + echo "Usage: $0 <data-file> <title>" + exit 1 +esac + gnuplot -persist << EOF -set ylabel "altitude (m)" +set ylabel "distance (m)" set y2label "angle (d)" set xlabel "time (s)" set xtics border out nomirror set ytics border out nomirror set y2tics border out nomirror -plot "$1" using 1:3 with lines axes x1y1 title "raw height",\ -"$1" using 1:9 with lines axes x1y2 title "angle",\ -"$1" using 1:11 with lines axes x1y2 title "qangle" +set title "$title" +plot "$file" using 1:5 with lines axes x1y1 title "height",\ +"$file" using 1:7 with lines axes x1y2 title "angle",\ +"$file" using 1:13 with lines axes x1y2 title "gps angle",\ +"$file" using 1:15 with lines axes x1y2 title "sats" EOF diff --git a/src/test/run-all-mm b/src/test/run-all-mm new file mode 100755 index 00000000..d9c2043d --- /dev/null +++ b/src/test/run-all-mm @@ -0,0 +1,2 @@ +#!/bin/sh +./run-mm `cat mega.flights` diff --git a/src/test/run-mm b/src/test/run-mm index 6f3d97a2..ae5e5f42 100755 --- a/src/test/run-mm +++ b/src/test/run-mm @@ -3,15 +3,20 @@ DIR=~/misc/rockets/flights for i in "$@"; do -case "$i" in - */*) - file="$i" - ;; - *) - file="$DIR/$i" - ;; -esac -./ao_flight_test_mm "$file" > run-out.mm + case "$i" in + */*) + file="$i" + ;; + *) + file="$DIR/$i" + ;; + esac + base=`basename "$i" .eeprom` + + ./ao_flight_test_mm "$file" > $base.plot + + sh ./plotmm $base.plot `basename "$file"` +done #./ao_flight_test_accel "$file" > run-out.accel #"run-out.accel" using 1:9 with lines lt 4 axes x1y1 title "accel height",\ @@ -21,21 +26,21 @@ esac #"run-out.accel" using 1:17 with lines lt 4 axes x1y1 title "accel main",\ # -gnuplot << EOF -set ylabel "altitude (m)" -set y2label "velocity (m/s), acceleration(m/s²)" -set xlabel "time (s)" -set xtics border out nomirror -set ytics border out nomirror -set y2tics border out nomirror -set title "$i" -plot "run-out.mm" using 1:3 with lines lw 2 lt 1 axes x1y1 title "raw height",\ -"run-out.mm" using 1:5 with lines lw 2 lt 1 axes x1y2 title "raw accel",\ -"run-out.mm" using 1:21 with lines lt 2 axes x1y1 title "mm height",\ -"run-out.mm" using 1:23 with lines lt 2 axes x1y2 title "mm speed",\ -"run-out.mm" using 1:25 with lines lt 2 axes x1y2 title "mm accel",\ -"run-out.mm" using 1:29 with lines lt 2 axes x1y1 title "mm drogue",\ -"run-out.mm" using 1:31 with lines lt 2 axes x1y1 title "mm main" -pause mouse close -EOF -done
\ No newline at end of file +#gnuplot << EOF +#set ylabel "altitude (m)" +#set y2label "velocity (m/s), acceleration(m/s²)" +#set xlabel "time (s)" +#set xtics border out nomirror +#set ytics border out nomirror +#set y2tics border out nomirror +#set title "$i" +#plot "run-out.mm" using 1:3 with lines lw 2 lt 1 axes x1y1 title "raw height",\ +#"run-out.mm" using 1:5 with lines lw 2 lt 1 axes x1y2 title "raw accel",\ +#"run-out.mm" using 1:21 with lines lt 2 axes x1y1 title "mm height",\ +#"run-out.mm" using 1:23 with lines lt 2 axes x1y2 title "mm speed",\ +#"run-out.mm" using 1:25 with lines lt 2 axes x1y2 title "mm accel",\ +#"run-out.mm" using 1:29 with lines lt 2 axes x1y1 title "mm drogue",\ +#"run-out.mm" using 1:31 with lines lt 2 axes x1y1 title "mm main" +#pause mouse close +#EOF +#done
\ No newline at end of file diff --git a/src/tidongle/Makefile b/src/tidongle/Makefile index 1514c4df..b2ba537b 100644 --- a/src/tidongle/Makefile +++ b/src/tidongle/Makefile @@ -72,10 +72,10 @@ endif # Otherwise, print the full command line. quiet ?= $($1) -all: ../$(PROG) +all: $(PROG) -../$(PROG): $(REL) Makefile - $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) && cp $(PROG) $(PMAP) .. +$(PROG): $(REL) Makefile + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(REL) $(call quiet,CHECK_STACK) ../cc1111/ao_arch.h $(PMEM) || rm $@ ao_product.h: ao-make-product.5c ../Version |