From 58eda1736d8cffa6e3b5afce6f5faabbb179045e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 23 Mar 2018 17:34:23 -0600 Subject: altos/ao_cc1120: Only call ao_packet_slave_stop when HAS_SLAVE Don't try to disable packet slave mode when the product doesn't support packet slave mode. Signed-off-by: Keith Packard --- src/drivers/ao_cc1120.c | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c index efe5f996..2f091485 100644 --- a/src/drivers/ao_cc1120.c +++ b/src/drivers/ao_cc1120.c @@ -1447,7 +1447,9 @@ ao_radio_test_recv(void) static void ao_radio_aprs(void) { +#if PACKET_HAS_SLAVE ao_packet_slave_stop(); +#endif ao_aprs_send(); } #endif -- cgit v1.2.3 From 865364d2738ae0b79a390eab62a5ddac1e890c71 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 23 Mar 2018 17:35:47 -0600 Subject: altos: Hack up code for TeleMetrum to make it just a tracker Support for a failed TeleMetrum device that uses just the GPS receiver and radio to create a TeleGPS-compatible device called 'tmgps'. Signed-off-by: Keith Packard --- src/tmgps-v2.0/.gitignore | 2 + src/tmgps-v2.0/Makefile | 117 +++++++++++++++++++++++ src/tmgps-v2.0/ao_pins.h | 236 ++++++++++++++++++++++++++++++++++++++++++++++ src/tmgps-v2.0/ao_tmgps.c | 81 ++++++++++++++++ 4 files changed, 436 insertions(+) create mode 100644 src/tmgps-v2.0/.gitignore create mode 100644 src/tmgps-v2.0/Makefile create mode 100644 src/tmgps-v2.0/ao_pins.h create mode 100644 src/tmgps-v2.0/ao_tmgps.c (limited to 'src') diff --git a/src/tmgps-v2.0/.gitignore b/src/tmgps-v2.0/.gitignore new file mode 100644 index 00000000..611e845e --- /dev/null +++ b/src/tmgps-v2.0/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +tmgps-*.elf diff --git a/src/tmgps-v2.0/Makefile b/src/tmgps-v2.0/Makefile new file mode 100644 index 00000000..9e3965c6 --- /dev/null +++ b/src/tmgps-v2.0/Makefile @@ -0,0 +1,117 @@ +# +# AltOS build +# +# + +include ../stm/Makefile.defs + +INC = \ + ao.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_boot.h \ + ao_companion.h \ + ao_data.h \ + ao_sample.h \ + ao_pins.h \ + ao_kalman.h \ + ao_product.h \ + ao_cc1120_CC1120.h \ + ao_profile.h \ + ao_task.h \ + ao_whiten.h \ + ao_sample_profile.h \ + ao_mpu.h \ + stm32l.h \ + Makefile + +#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_log.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_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_adc_stm.c \ + ao_storage.c \ + ao_m25.c \ + ao_usb_stm.c \ + ao_exti_stm.c \ + ao_eeprom_stm.c \ + ao_convert_volt.c \ + ao_log.c \ + ao_log_gps.c \ + ao_distance.c \ + ao_sqrt.c \ + ao_tracker.c \ + ao_telemetry.c \ + ao_aprs.c \ + $(MATH_SRC) \ + $(PROFILE) \ + $(SAMPLE_PROFILE) \ + $(STACK_GUARD) + +PRODUCT=TMGPS-v2.0 +PRODUCT_DEF=-DTMGPS_V_2_0 +IDPRODUCT=0x0025 + +CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STACK_GUARD_DEF) -Os -g + +PROGNAME=tmgps-v2.0 +PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx + +SRC=$(ALTOS_SRC) ao_tmgps.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/tmgps-v2.0/ao_pins.h b/src/tmgps-v2.0/ao_pins.h new file mode 100644 index 00000000..b4dcc10c --- /dev/null +++ b/src/tmgps-v2.0/ao_pins.h @@ -0,0 +1,236 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#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 AO_CONFIG_MAX_SIZE 1024 +#define LOG_ERASE_MARK 0x55 +#define LOG_MAX_ERASE 128 + +#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 0 +#define HAS_RADIO 1 +#define HAS_TELEMETRY 1 +#define HAS_RDF 1 +#define HAS_APRS 1 +#define HAS_COMPANION 0 + +#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 0 +#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 AO_LED_PANIC AO_LED_RED +#define AO_LED_GPS_LOCK AO_LED_GREEN + +#define LEDS_AVAILABLE (AO_LED_RED | AO_LED_GREEN) + +#define HAS_GPS 1 +#define HAS_FLIGHT 0 +#define HAS_ADC 1 +#define HAS_ADC_TEMP 0 +#define HAS_LOG 1 +#define HAS_TRACKER 1 +#define LOG_ADC 0 +#define AO_LOG_FORMAT AO_LOG_FORMAT_TELEGPS + +/* + * ADC + */ +#define AO_DATA_RING 32 +#define AO_ADC_NUM_SENSE 2 + +struct ao_adc { + int16_t v_batt; +}; + +#define AO_ADC_DUMP(p) \ + printf("tick: %5u batt: %5d\n", \ + (p)->tick, \ + (p)->adc.v_batt); + +#define AO_ADC_V_BATT 8 +#define AO_ADC_V_BATT_PORT (&stm_gpiob) +#define AO_ADC_V_BATT_PIN 0 + +#define AO_ADC_RCC_AHBENR ((1 << STM_RCC_AHBENR_GPIOBEN)) + +#define AO_NUM_ADC_PIN 1 + +#define AO_ADC_PIN0_PORT AO_ADC_V_BATT_PORT +#define AO_ADC_PIN0_PIN AO_ADC_V_BATT_PIN + +#define AO_NUM_ADC (AO_NUM_ADC_PIN) + +#define AO_ADC_SQ1 AO_ADC_V_BATT + +/* + * Voltage divider on ADC battery sampler + */ +#define AO_BATTERY_DIV_PLUS 56 /* 5.6k */ +#define AO_BATTERY_DIV_MINUS 100 /* 10k */ + +/* + * ADC reference in decivolts + */ +#define AO_ADC_REFERENCE_DV 33 + +/* + * 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) + +/* + * 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 NUM_CMDS 16 + +/* + * 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/tmgps-v2.0/ao_tmgps.c b/src/tmgps-v2.0/ao_tmgps.c new file mode 100644 index 00000000..cbd58eb2 --- /dev/null +++ b/src/tmgps-v2.0/ao_tmgps.c @@ -0,0 +1,81 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include +#include +#include +#include +#if HAS_SAMPLE_PROFILE +#include +#endif +#if HAS_STACK_GUARD +#include +#endif + +int +main(void) +{ + ao_clock_init(); + +#if HAS_STACK_GUARD + ao_mpu_init(); +#endif + + ao_task_init(); + ao_cmd_init(); + ao_config_init(); + + /* Turn on all of the LEDs to test them */ + ao_led_init(LEDS_AVAILABLE); + ao_led_on(LEDS_AVAILABLE); + + /* Internal systems */ + ao_timer_init(); + ao_dma_init(); + ao_exti_init(); + + /* SoC hardware */ + ao_spi_init(); + ao_adc_init(); + ao_serial_init(); + ao_usb_init(); + ao_eeprom_init(); + + /* External hardware */ + ao_storage_init(); + ao_radio_init(); + ao_gps_init(); + + /* Services */ + ao_log_init(); + ao_telemetry_init(); + ao_tracker_init(); + +#if AO_PROFILE + ao_profile_init(); +#endif +#if HAS_SAMPLE_PROFILE + ao_sample_profile_init(); +#endif + ao_led_off(LEDS_AVAILABLE); + + ao_start_scheduler(); + return 0; +} -- cgit v1.2.3 From 20dc0e6c8e365c1f4188189d506163d589c3bade Mon Sep 17 00:00:00 2001 From: Bdale Garbee Date: Sun, 22 Apr 2018 19:00:03 -0600 Subject: add a warning about Google limiting per-day access until/unless we pay --- ao-bringup/turnon_telegps | 2 +- doc/load-maps.inc | 6 +++++- pdclib | 2 +- src/stmf0/ao_usb_stm.c | 2 +- 4 files changed, 8 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/ao-bringup/turnon_telegps b/ao-bringup/turnon_telegps index b1da4373..5e4cd616 100755 --- a/ao-bringup/turnon_telegps +++ b/ao-bringup/turnon_telegps @@ -49,7 +49,7 @@ esac FLASH_FILE=$REPO/loaders/telegps-v2.0-altos-flash-*.bin ALTOS_FILE=$REPO/telegps-v2.0-*.elf -$DFU_UTIL -a 0 -s 0x08000000:leave -D $FLASH_FILE +$DFU_UTIL -v -v -R -a 0 -s 0x08000000:leave -D $FLASH_FILE sleep 3 diff --git a/doc/load-maps.inc b/doc/load-maps.inc index e7717d89..ccab4795 100644 --- a/doc/load-maps.inc +++ b/doc/load-maps.inc @@ -5,7 +5,11 @@ Before heading out to a new launch site, you can use this to load satellite images in case you don't have - internet connectivity at the site. + internet connectivity at the site. Try not to wait + until the last minute, though, particularly if you're + heading to a major launch. If too many people are + all trying to download map data at once, Google may + limit access until the next day. There's a drop-down menu of launch sites we know about; if your favorites aren't there, please let us diff --git a/pdclib b/pdclib index 3e68419a..20f71c3a 160000 --- a/pdclib +++ b/pdclib @@ -1 +1 @@ -Subproject commit 3e68419ad3ca3237ca16de4cf2a967f04129fe33 +Subproject commit 20f71c3a97eb4c7ecfa9754a0ca42855db935999 diff --git a/src/stmf0/ao_usb_stm.c b/src/stmf0/ao_usb_stm.c index bf08abc1..483d2419 100644 --- a/src/stmf0/ao_usb_stm.c +++ b/src/stmf0/ao_usb_stm.c @@ -1447,7 +1447,7 @@ ao_usb_enable(void) ao_arch_release_interrupts(); - for (t = 0; t < 1000; t++) + for (t = 0; t < 50000; t++) ao_arch_nop(); /* Enable USB pull-up */ -- cgit v1.2.3 From a414a32f86c9d8a2c5f576898c0f0dc75263ff85 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 21 Apr 2018 16:14:39 -0700 Subject: altos/stm: Simplify ao_usb_write a bit Remove unnecessary 'offset' param (was always passed zero). This also makes some code conditional on that value no longer necessary. Signed-off-by: Keith Packard --- src/stm/ao_usb_stm.c | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/stm/ao_usb_stm.c b/src/stm/ao_usb_stm.c index 595bddac..e436d264 100644 --- a/src/stm/ao_usb_stm.c +++ b/src/stm/ao_usb_stm.c @@ -446,15 +446,11 @@ ao_usb_write_short(uint16_t data, uint32_t *base, uint16_t offset) } static void -ao_usb_write(const uint8_t *src, uint32_t *base, uint16_t offset, uint16_t bytes) +ao_usb_write(const uint8_t *src, uint32_t *base, uint16_t bytes) { + uint16_t offset = 0; if (!bytes) return; - if (offset & 1) { - debug_data (" %02x", src[0]); - ao_usb_write_byte(*src++, base, offset++); - bytes--; - } while (bytes >= 2) { debug_data (" %02x %02x", src[0], src[1]); ao_usb_write_short((src[1] << 8) | src[0], base, offset); @@ -531,7 +527,7 @@ ao_usb_ep0_flush(void) ao_usb_ep0_in_len -= this_len; debug_data ("Flush EP0 len %d:", this_len); - ao_usb_write(ao_usb_ep0_in_data, ao_usb_ep0_tx_buffer, 0, this_len); + ao_usb_write(ao_usb_ep0_in_data, ao_usb_ep0_tx_buffer, this_len); debug_data ("\n"); ao_usb_ep0_in_data += this_len; @@ -850,7 +846,7 @@ _ao_usb_in_send(void) ao_usb_in_pending = 1; if (ao_usb_tx_count != AO_USB_IN_SIZE) ao_usb_in_flushed = 1; - ao_usb_write(ao_usb_tx_buffer, ao_usb_in_tx_buffer, 0, ao_usb_tx_count); + ao_usb_write(ao_usb_tx_buffer, ao_usb_in_tx_buffer, ao_usb_tx_count); ao_usb_bdt[AO_USB_IN_EPR].single.count_tx = ao_usb_tx_count; ao_usb_tx_count = 0; _ao_usb_set_stat_tx(AO_USB_IN_EPR, STM_USB_EPR_STAT_TX_VALID); -- cgit v1.2.3 From b478d3c3569d2f9df50b0030197468d14af67688 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 21 Apr 2018 16:17:26 -0700 Subject: altos: Use max of 64 previous orient values when checking pyro limits Instead of checking just a single measurement to see if the orientation is outside of the desired limits, use the maximum of 64 previous values to that rapidly changing orientation won't accidentally enable a pyro channel if sampled at the 'wrong time'. Signed-off-by: Keith Packard --- src/kernel/ao_pyro.c | 26 +++++++++++++--- src/kernel/ao_sample.c | 83 ++++++++++++++++++++++++++++++++++---------------- src/kernel/ao_sample.h | 3 ++ 3 files changed, 81 insertions(+), 31 deletions(-) (limited to 'src') diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index e5c30eec..5a556d59 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -81,6 +81,19 @@ int pyro_dbg; #define DBG(...) #endif +static angle_t +ao_sample_max_orient(void) +{ + uint8_t i; + angle_t max = ao_sample_orients[0]; + + for (i = 1; i < AO_NUM_ORIENT; i++) { + angle_t a = ao_sample_orients[i]; + if (a > max) + max = a; + } + return max; +} /* * Given a pyro structure, figure out * if the current flight state satisfies all @@ -90,6 +103,9 @@ static uint8_t ao_pyro_ready(struct ao_pyro *pyro) { enum ao_pyro_flag flag, flags; +#if HAS_GYRO + angle_t max_orient; +#endif flags = pyro->flags; while (flags != ao_pyro_none) { @@ -130,14 +146,16 @@ ao_pyro_ready(struct ao_pyro *pyro) #if HAS_GYRO case ao_pyro_orient_less: - if (ao_sample_orient <= pyro->orient_less) + max_orient = ao_sample_max_orient(); + if (max_orient <= pyro->orient_less) continue; - DBG("orient %d > %d\n", ao_sample_orient, pyro->orient_less); + DBG("orient %d > %d\n", max_orient, pyro->orient_less); break; case ao_pyro_orient_greater: - if (ao_sample_orient >= pyro->orient_greater) + max_orient = ao_sample_max_orient(); + if (max_orient >= pyro->orient_greater) continue; - DBG("orient %d < %d\n", ao_sample_orient, pyro->orient_greater); + DBG("orient %d < %d\n", max_orient, pyro->orient_greater); break; #endif diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c index 61519478..f8012e34 100644 --- a/src/kernel/ao_sample.c +++ b/src/kernel/ao_sample.c @@ -50,6 +50,8 @@ __pdata gyro_t ao_sample_roll; __pdata gyro_t ao_sample_pitch; __pdata gyro_t ao_sample_yaw; __pdata angle_t ao_sample_orient; +__pdata angle_t ao_sample_orients[AO_NUM_ORIENT]; +__pdata uint8_t ao_sample_orient_pos; #endif __data uint8_t ao_sample_data; @@ -115,6 +117,53 @@ ao_sample_preflight_add(void) ++nsamples; } +#if HAS_GYRO +static void +ao_sample_set_all_orients(void) +{ + int i; + for (i = 0; i < AO_NUM_ORIENT; i++) + ao_sample_orients[i] = ao_sample_orient; + ao_sample_orient_pos = 0; +} + +static void +ao_sample_set_one_orient(void) +{ + ao_sample_orients[ao_sample_orient_pos] = ao_sample_orient; + ao_sample_orient_pos = (ao_sample_orient_pos + 1) % AO_NUM_ORIENT; +} + +static void +ao_sample_compute_orient(void) +{ + /* 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 /* HAS_GYRO */ + static void ao_sample_preflight_set(void) { @@ -138,7 +187,7 @@ ao_sample_preflight_set(void) ao_sample_pitch_sum = 0; ao_sample_yaw_sum = 0; ao_sample_roll_sum = 0; - ao_sample_orient = 0; + ao_sample_set_all_orients(); struct ao_quaternion orient; @@ -168,6 +217,9 @@ ao_sample_preflight_set(void) if (ao_orient_test) printf("\n\treset\n"); #endif + + ao_sample_compute_orient(); + ao_sample_set_all_orients(); #endif nsamples = 0; } @@ -195,31 +247,6 @@ ao_sample_rotate(void) /* 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); - #if HAS_FLIGHT_DEBUG if (ao_orient_test) { printf ("rot %d %d %d orient %d \r", @@ -229,7 +256,8 @@ ao_sample_rotate(void) ao_sample_orient); } #endif - + ao_sample_compute_orient(); + ao_sample_set_one_orient(); } #endif @@ -367,6 +395,7 @@ ao_sample_init(void) ao_sample_yaw = 0; ao_sample_roll = 0; ao_sample_orient = 0; + ao_sample_set_all_orients(); #endif ao_sample_data = ao_data_head; ao_preflight = TRUE; diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h index fbef031d..5ae389be 100644 --- a/src/kernel/ao_sample.h +++ b/src/kernel/ao_sample.h @@ -146,7 +146,10 @@ 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; +#define AO_NUM_ORIENT 64 extern __pdata angle_t ao_sample_orient; +extern __pdata angle_t ao_sample_orients[AO_NUM_ORIENT]; +extern __pdata uint8_t ao_sample_orient_pos; #endif void ao_sample_init(void); -- cgit v1.2.3 From b47796991cb6c2edb85f9201a53515b4dd28b946 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 15 Jul 2014 22:52:35 -0700 Subject: altos/flash-loader: On STM, don't include ao_usb_disable This saves a bit of space, keeping the flash loader under 4k on fox Signed-off-by: Keith Packard --- src/product/ao_flash_pins.h | 1 + src/stm/ao_usb_stm.c | 6 ++++++ 2 files changed, 7 insertions(+) (limited to 'src') diff --git a/src/product/ao_flash_pins.h b/src/product/ao_flash_pins.h index ff8d3273..019b5425 100644 --- a/src/product/ao_flash_pins.h +++ b/src/product/ao_flash_pins.h @@ -25,6 +25,7 @@ #define HAS_USB 1 #define USE_USB_STDIO 0 +#define HAS_USB_DISABLE 0 #define HAS_BEEP 0 #define HAS_TASK 0 #define HAS_ECHO 0 diff --git a/src/stm/ao_usb_stm.c b/src/stm/ao_usb_stm.c index e436d264..b06814d2 100644 --- a/src/stm/ao_usb_stm.c +++ b/src/stm/ao_usb_stm.c @@ -970,6 +970,11 @@ ao_usb_getchar(void) return c; } +#ifndef HAS_USB_DISABLE +#define HAS_USB_DISABLE 1 +#endif + +#if HAS_USB_DISABLE void ao_usb_disable(void) { @@ -987,6 +992,7 @@ ao_usb_disable(void) stm_rcc.apb1enr &= ~(1 << STM_RCC_APB1ENR_USBEN); ao_arch_release_interrupts(); } +#endif void ao_usb_enable(void) -- cgit v1.2.3 From 23ba75c3c2ddde65dc543b52cd87b8a1433c5024 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 23 Jan 2013 19:03:26 -0800 Subject: altos/fox: Add fox1ihu bringup firmware Firmware capable of testing fox1ihu hardware Signed-off-by: Keith Packard --- src/fox1ihu/.gitignore | 2 + src/fox1ihu/Makefile | 76 ++++++++++++++++++ src/fox1ihu/ao_fox1ihu.c | 47 +++++++++++ src/fox1ihu/ao_pins.h | 204 +++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 329 insertions(+) create mode 100644 src/fox1ihu/.gitignore create mode 100644 src/fox1ihu/Makefile create mode 100644 src/fox1ihu/ao_fox1ihu.c create mode 100644 src/fox1ihu/ao_pins.h (limited to 'src') diff --git a/src/fox1ihu/.gitignore b/src/fox1ihu/.gitignore new file mode 100644 index 00000000..538cf8b2 --- /dev/null +++ b/src/fox1ihu/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +fox1ihu-*.elf diff --git a/src/fox1ihu/Makefile b/src/fox1ihu/Makefile new file mode 100644 index 00000000..f76633aa --- /dev/null +++ b/src/fox1ihu/Makefile @@ -0,0 +1,76 @@ +# +# AltOS build +# +# + +include ../stm/Makefile.defs + +INC = \ + ao.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_pins.h \ + ao_product.h \ + ao_task.h \ + stm32l.h \ + Makefile + +#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 + +ALTOS_SRC = \ + ao_interrupt.c \ + ao_product.c \ + ao_romconfig.c \ + ao_cmd.c \ + ao_task.c \ + ao_led.c \ + ao_stdio.c \ + ao_panic.c \ + ao_timer.c \ + ao_mutex.c \ + ao_dma_stm.c \ + ao_spi_stm.c \ + ao_usb_stm.c \ + ao_exti_stm.c \ + ao_adc_stm.c \ + ao_data.c + +PRODUCT=Fox1IHH-v0.1 +PRODUCT_DEF=-DFOX +IDPRODUCT=0x0024 + +CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STACK_GUARD_DEF) -Os -g + +PROGNAME=fox1ihu-v0.1 +PROG=$(PROGNAME)-$(VERSION).elf + +SRC=$(ALTOS_SRC) ao_fox1ihu.c +OBJ=$(SRC:.c=.o) + +all: $(PROG) + +$(PROG): Makefile $(OBJ) altos.ld + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(SAT_CLIB) -lgcc + +$(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 + rm -f ao_product.h + +install: + +uninstall: diff --git a/src/fox1ihu/ao_fox1ihu.c b/src/fox1ihu/ao_fox1ihu.c new file mode 100644 index 00000000..4d02f0da --- /dev/null +++ b/src/fox1ihu/ao_fox1ihu.c @@ -0,0 +1,47 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include +#include + +int +main(void) +{ + ao_clock_init(); + +#if HAS_STACK_GUARD + ao_mpu_init(); +#endif + + ao_task_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_cmd_init(); + + ao_usb_init(); + + ao_start_scheduler(); + return 0; +} diff --git a/src/fox1ihu/ao_pins.h b/src/fox1ihu/ao_pins.h new file mode 100644 index 00000000..e3d8d4d3 --- /dev/null +++ b/src/fox1ihu/ao_pins.h @@ -0,0 +1,204 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_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 1 +#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 0 +#define USE_SERIAL_3_STDIN 0 +#define SERIAL_3_PB10_PB11 0 +#define SERIAL_3_PC10_PC11 1 +#define SERIAL_3_PD8_PD9 0 + +#define HAS_EEPROM 0 +#define USE_INTERNAL_FLASH 0 +#define HAS_USB 1 +#define HAS_BEEP 0 +#define HAS_RADIO 0 +#define HAS_TELEMETRY 0 + +#define HAS_SPI_1 1 +#define SPI_1_PA5_PA6_PA7 0 +#define SPI_1_PB3_PB4_PB5 0 +#define SPI_1_PE13_PE14_PE15 1 /* */ + +#define HAS_SPI_2 0 +#define SPI_2_PB13_PB14_PB15 1 /* */ +#define SPI_2_PD1_PD3_PD4 0 + +#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 1 +#define I2C_1_PB6_PB7 1 +#define I2C_1_PB8_PB9 0 + +#define HAS_I2C_2 1 +#define I2C_2_PB10_PB11 1 + +#define PACKET_HAS_SLAVE 0 +#define PACKET_HAS_MASTER 0 + +#define LOW_LEVEL_DEBUG 0 + +#define LED_PORT_0_ENABLE STM_RCC_AHBENR_GPIOCEN +#define LED_PORT_0 (&stm_gpioc) +#define LED_PORT_0_MASK (0xff) +#define LED_PORT_0_SHIFT 0 +#define LED_PIN_RED 6 +#define LED_PIN_GREEN 7 +#define LED_PIN_RED_2 8 +#define LED_PIN_GREEN_2 9 +#define AO_LED_RED (1 << LED_PIN_RED) +#define AO_LED_GREEN (1 << LED_PIN_GREEN) +#define AO_LED_RED_2 (1 << LED_PIN_RED_2) +#define AO_LED_GREEN_2 (1 << LED_PIN_GREEN_2) + +#define LEDS_AVAILABLE (AO_LED_RED | AO_LED_GREEN | AO_LED_RED_2 | AO_LED_GREEN_2) + +#define HAS_GPS 0 +#define HAS_FLIGHT 0 +#define HAS_ADC 1 +#define HAS_LOG 0 + +/* + * ADC + */ +#define AO_DATA_RING 32 +#define AO_ADC_NUM_SENSE 6 + +struct ao_adc { + int16_t tx_pa_current; /* ADC_IN0 */ + int16_t tx_pa_temp; /* ADC_IN1 */ + int16_t tx_xo_temp; /* ADC_IN2 */ + int16_t rx_xo_temp; /* ADC_IN3 */ + int16_t ihu_current; /* ADC_IN8 */ + int16_t rx_cd; /* ADC_IN9 */ + int16_t ant_sense_1; /* ADC_IN10 */ + int16_t ant_sense_2; /* ADC_IN11 */ +}; + +#define AO_ADC_TX_PA_CURRENT 0 +#define AO_ADC_TX_PA_CURRENT_PORT (&stm_gpioa) +#define AO_ADC_TX_PA_CURRENT_PIN 0 + +#define AO_ADC_TX_PA_TEMP 1 +#define AO_ADC_TX_PA_TEMP_PORT (&stm_gpioa) +#define AO_ADC_TX_PA_TEMP_PIN 1 + +#define AO_ADC_TX_XO_TEMP 2 +#define AO_ADC_TX_XO_TEMP_PORT (&stm_gpioa) +#define AO_ADC_TX_XO_TEMP_PIN 2 + +#define AO_ADC_RX_XO_TEMP 3 +#define AO_ADC_RX_XO_TEMP_PORT (&stm_gpioa) +#define AO_ADC_RX_XO_TEMP_PIN 3 + +#define AO_ADC_IHU_CURRENT 8 +#define AO_ADC_IHU_CURRENT_PORT (&stm_gpiob) +#define AO_ADC_IHU_CURRENT_PIN 0 + +#define AO_ADC_RX_CD 9 +#define AO_ADC_RX_CD_PORT (&stm_gpiob) +#define AO_ADC_RX_CD_PIN 1 + +#define AO_ADC_ANT_SENSE_1 10 +#define AO_ADC_ANT_SENSE_1_PORT (&stm_gpioc) +#define AO_ADC_ANT_SENSE_1_PIN 0 + +#define AO_ADC_ANT_SENSE_2 11 +#define AO_ADC_ANT_SENSE_2_PORT (&stm_gpioc) +#define AO_ADC_ANT_SENSE_2_PIN 1 + +#define AO_ADC_TEMP 16 + +#define AO_ADC_RCC_AHBENR ((1 << STM_RCC_AHBENR_GPIOAEN) | \ + (1 << STM_RCC_AHBENR_GPIOBEN) | \ + (1 << STM_RCC_AHBENR_GPIOCEN)) + +#define AO_NUM_ADC_PIN (9) + +#define AO_ADC_PIN0_PORT AO_ADC_TX_PA_CURRENT_PORT +#define AO_ADC_PIN0_PIN AO_ADC_TX_PA_CURRENT_PIN +#define AO_ADC_PIN1_PORT AO_ADC_TX_PA_TEMP_PORT +#define AO_ADC_PIN1_PIN AO_ADC_TX_PA_TEMP_PIN +#define AO_ADC_PIN2_PORT AO_ADC_TX_XO_TEMP_PORT +#define AO_ADC_PIN2_PIN AO_ADC_TX_XO_TEMP_PIN +#define AO_ADC_PIN3_PORT AO_ADC_RX_XO_TEMP_PORT +#define AO_ADC_PIN3_PIN AO_ADC_RX_XO_TEMP_PIN +#define AO_ADC_PIN4_PORT AO_ADC_IHU_CURRENT_PORT +#define AO_ADC_PIN4_PIN AO_ADC_IHU_CURRENT_PIN +#define AO_ADC_PIN5_PORT AO_ADC_IHU_CURRENT_PORT +#define AO_ADC_PIN5_PIN AO_ADC_IHU_CURRENT_PIN +#define AO_ADC_PIN6_PORT AO_ADC_RX_CD_PORT +#define AO_ADC_PIN6_PIN AO_ADC_RX_CD_PIN +#define AO_ADC_PIN7_PORT AO_ADC_ANT_SENSE_1_PORT +#define AO_ADC_PIN7_PIN AO_ADC_ANT_SENSE_1_PIN +#define AO_ADC_PIN8_PORT AO_ADC_ANT_SENSE_2_PORT +#define AO_ADC_PIN8_PIN AO_ADC_ANT_SENSE_2_PIN + +#define AO_NUM_ADC (AO_NUM_ADC_PIN + 1) /* Add internal temp sensor */ + +#define AO_ADC_SQ1 AO_ADC_TX_PA_CURRENT +#define AO_ADC_SQ2 AO_ADC_TX_PA_TEMP +#define AO_ADC_SQ3 AO_ADC_TX_XO_TEMP +#define AO_ADC_SQ4 AO_ADC_RX_XO_TEMP +#define AO_ADC_SQ5 AO_ADC_IHU_CURRENT +#define AO_ADC_SQ6 AO_ADC_IHU_CURRENT +#define AO_ADC_SQ7 AO_ADC_RX_CD +#define AO_ADC_SQ8 AO_ADC_ANT_SENSE_1 +#define AO_ADC_SQ9 AO_ADC_ANT_SENSE_2 +#define AO_ADC_SQ10 AO_ADC_TEMP + +#endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From fbf01178e560d8a36916e295d7bdb0b0e98d2b02 Mon Sep 17 00:00:00 2001 From: Bdale Garbee Date: Wed, 23 Jan 2013 21:45:28 -0700 Subject: altos/fox: rename fox product It's Fox1IHU-v1 now --- src/fox1ihu/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/fox1ihu/Makefile b/src/fox1ihu/Makefile index f76633aa..714a308b 100644 --- a/src/fox1ihu/Makefile +++ b/src/fox1ihu/Makefile @@ -43,7 +43,7 @@ ALTOS_SRC = \ ao_adc_stm.c \ ao_data.c -PRODUCT=Fox1IHH-v0.1 +PRODUCT=Fox1IHU-v1 PRODUCT_DEF=-DFOX IDPRODUCT=0x0024 -- cgit v1.2.3 From ff2330a7fedfa7f6be3502ad0d591ab9e5ddddc6 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 23 Jan 2013 20:48:04 -0800 Subject: altos/fox: Initialize FOX adc Signed-off-by: Keith Packard --- src/fox1ihu/ao_fox1ihu.c | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/fox1ihu/ao_fox1ihu.c b/src/fox1ihu/ao_fox1ihu.c index 4d02f0da..9e76d267 100644 --- a/src/fox1ihu/ao_fox1ihu.c +++ b/src/fox1ihu/ao_fox1ihu.c @@ -38,6 +38,8 @@ main(void) ao_dma_init(); ao_exti_init(); + ao_adc_init(); + ao_cmd_init(); ao_usb_init(); -- cgit v1.2.3 From 8b783887d7f136d3a389316545b74f4755e43eb4 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 23 Jan 2013 21:05:26 -0800 Subject: altos/fox: Fix up Fox ADC bits Update to current schematic Signed-off-by: Keith Packard --- src/fox1ihu/ao_pins.h | 74 ++++++++++++++++++++++++++++++++++++--------------- 1 file changed, 52 insertions(+), 22 deletions(-) (limited to 'src') diff --git a/src/fox1ihu/ao_pins.h b/src/fox1ihu/ao_pins.h index e3d8d4d3..506a1360 100644 --- a/src/fox1ihu/ao_pins.h +++ b/src/fox1ihu/ao_pins.h @@ -110,6 +110,7 @@ #define HAS_GPS 0 #define HAS_FLIGHT 0 #define HAS_ADC 1 +#define HAS_ADC_TEMP 1 #define HAS_LOG 0 /* @@ -119,14 +120,18 @@ #define AO_ADC_NUM_SENSE 6 struct ao_adc { - int16_t tx_pa_current; /* ADC_IN0 */ - int16_t tx_pa_temp; /* ADC_IN1 */ - int16_t tx_xo_temp; /* ADC_IN2 */ - int16_t rx_xo_temp; /* ADC_IN3 */ - int16_t ihu_current; /* ADC_IN8 */ - int16_t rx_cd; /* ADC_IN9 */ - int16_t ant_sense_1; /* ADC_IN10 */ - int16_t ant_sense_2; /* ADC_IN11 */ + int16_t tx_pa_current; /* 0 ADC_IN0 */ + int16_t tx_pa_temp; /* 1 ADC_IN1 */ + int16_t tx_xo_temp; /* 2 ADC_IN2 */ + int16_t rx_xo_temp; /* 3 ADC_IN3 */ + int16_t ihu_current; /* 4 ADC_IN8 */ + int16_t rx_cd; /* 5 ADC_IN9 */ + int16_t ant_sense_1; /* 6 ADC_IN10 */ + int16_t ant_sense_2; /* 7 ADC_IN11 */ + int16_t gyro_x_1; /* 8 ADC_IN12 */ + int16_t gyro_z_1; /* 9 ADC_IN13 */ + int16_t gyro_x_2; /* 10 ADC_IN14 */ + int16_t gyro_z_2; /* 11 ADC_IN15 */ }; #define AO_ADC_TX_PA_CURRENT 0 @@ -161,13 +166,29 @@ struct ao_adc { #define AO_ADC_ANT_SENSE_2_PORT (&stm_gpioc) #define AO_ADC_ANT_SENSE_2_PIN 1 +#define AO_ADC_GYRO_X_1 12 +#define AO_ADC_GYRO_X_1_PORT (&stm_gpioc) +#define AO_ADC_GYRO_X_1_PIN 2 + +#define AO_ADC_GYRO_Z_1 13 +#define AO_ADC_GYRO_Z_1_PORT (&stm_gpioc) +#define AO_ADC_GYRO_Z_1_PIN 3 + +#define AO_ADC_GYRO_X_2 14 +#define AO_ADC_GYRO_X_2_PORT (&stm_gpioc) +#define AO_ADC_GYRO_X_2_PIN 4 + +#define AO_ADC_GYRO_Z_2 15 +#define AO_ADC_GYRO_Z_2_PORT (&stm_gpioc) +#define AO_ADC_GYRO_Z_2_PIN 5 + #define AO_ADC_TEMP 16 #define AO_ADC_RCC_AHBENR ((1 << STM_RCC_AHBENR_GPIOAEN) | \ (1 << STM_RCC_AHBENR_GPIOBEN) | \ (1 << STM_RCC_AHBENR_GPIOCEN)) -#define AO_NUM_ADC_PIN (9) +#define AO_NUM_ADC_PIN (12) #define AO_ADC_PIN0_PORT AO_ADC_TX_PA_CURRENT_PORT #define AO_ADC_PIN0_PIN AO_ADC_TX_PA_CURRENT_PIN @@ -179,14 +200,20 @@ struct ao_adc { #define AO_ADC_PIN3_PIN AO_ADC_RX_XO_TEMP_PIN #define AO_ADC_PIN4_PORT AO_ADC_IHU_CURRENT_PORT #define AO_ADC_PIN4_PIN AO_ADC_IHU_CURRENT_PIN -#define AO_ADC_PIN5_PORT AO_ADC_IHU_CURRENT_PORT -#define AO_ADC_PIN5_PIN AO_ADC_IHU_CURRENT_PIN -#define AO_ADC_PIN6_PORT AO_ADC_RX_CD_PORT -#define AO_ADC_PIN6_PIN AO_ADC_RX_CD_PIN -#define AO_ADC_PIN7_PORT AO_ADC_ANT_SENSE_1_PORT -#define AO_ADC_PIN7_PIN AO_ADC_ANT_SENSE_1_PIN -#define AO_ADC_PIN8_PORT AO_ADC_ANT_SENSE_2_PORT -#define AO_ADC_PIN8_PIN AO_ADC_ANT_SENSE_2_PIN +#define AO_ADC_PIN5_PORT AO_ADC_RX_CD_PORT +#define AO_ADC_PIN5_PIN AO_ADC_RX_CD_PIN +#define AO_ADC_PIN6_PORT AO_ADC_ANT_SENSE_1_PORT +#define AO_ADC_PIN6_PIN AO_ADC_ANT_SENSE_1_PIN +#define AO_ADC_PIN7_PORT AO_ADC_ANT_SENSE_2_PORT +#define AO_ADC_PIN7_PIN AO_ADC_ANT_SENSE_2_PIN +#define AO_ADC_PIN8_PORT AO_ADC_GYRO_X_1_PORT +#define AO_ADC_PIN8_PIN AO_ADC_GYRO_X_1_PIN +#define AO_ADC_PIN9_PORT AO_ADC_GYRO_Z_1_PORT +#define AO_ADC_PIN9_PIN AO_ADC_GYRO_Z_1_PIN +#define AO_ADC_PIN10_PORT AO_ADC_GYRO_X_2_PORT +#define AO_ADC_PIN10_PIN AO_ADC_GYRO_X_2_PIN +#define AO_ADC_PIN11_PORT AO_ADC_GYRO_Z_2_PORT +#define AO_ADC_PIN11_PIN AO_ADC_GYRO_Z_2_PIN #define AO_NUM_ADC (AO_NUM_ADC_PIN + 1) /* Add internal temp sensor */ @@ -195,10 +222,13 @@ struct ao_adc { #define AO_ADC_SQ3 AO_ADC_TX_XO_TEMP #define AO_ADC_SQ4 AO_ADC_RX_XO_TEMP #define AO_ADC_SQ5 AO_ADC_IHU_CURRENT -#define AO_ADC_SQ6 AO_ADC_IHU_CURRENT -#define AO_ADC_SQ7 AO_ADC_RX_CD -#define AO_ADC_SQ8 AO_ADC_ANT_SENSE_1 -#define AO_ADC_SQ9 AO_ADC_ANT_SENSE_2 -#define AO_ADC_SQ10 AO_ADC_TEMP +#define AO_ADC_SQ6 AO_ADC_RX_CD +#define AO_ADC_SQ7 AO_ADC_ANT_SENSE_1 +#define AO_ADC_SQ8 AO_ADC_ANT_SENSE_2 +#define AO_ADC_SQ9 AO_ADC_GYRO_X_1 +#define AO_ADC_SQ10 AO_ADC_GYRO_Z_1 +#define AO_ADC_SQ11 AO_ADC_GYRO_X_2 +#define AO_ADC_SQ12 AO_ADC_GYRO_Z_2 +#define AO_ADC_SQ13 AO_ADC_TEMP #endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From 524afb040aa34d93abae7d04afa1df7a626f9877 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 23 Jan 2013 21:36:18 -0800 Subject: altos/fox: Provide names for fox ADC channels Signed-off-by: Keith Packard --- src/fox1ihu/ao_pins.h | 13 +++++++++++++ 1 file changed, 13 insertions(+) (limited to 'src') diff --git a/src/fox1ihu/ao_pins.h b/src/fox1ihu/ao_pins.h index 506a1360..09fd5e99 100644 --- a/src/fox1ihu/ao_pins.h +++ b/src/fox1ihu/ao_pins.h @@ -218,17 +218,30 @@ struct ao_adc { #define AO_NUM_ADC (AO_NUM_ADC_PIN + 1) /* Add internal temp sensor */ #define AO_ADC_SQ1 AO_ADC_TX_PA_CURRENT +#define AO_ADC_SQ1_NAME "tx_pa_current" #define AO_ADC_SQ2 AO_ADC_TX_PA_TEMP +#define AO_ADC_SQ2_NAME "tx_pa_temp" #define AO_ADC_SQ3 AO_ADC_TX_XO_TEMP +#define AO_ADC_SQ3_NAME "tx_xo_temp" #define AO_ADC_SQ4 AO_ADC_RX_XO_TEMP +#define AO_ADC_SQ4_NAME "rx_xo_temp" #define AO_ADC_SQ5 AO_ADC_IHU_CURRENT +#define AO_ADC_SQ5_NAME "ihu_current" #define AO_ADC_SQ6 AO_ADC_RX_CD +#define AO_ADC_SQ6_NAME "rx_cd" #define AO_ADC_SQ7 AO_ADC_ANT_SENSE_1 +#define AO_ADC_SQ7_NAME "ant_sense_1" #define AO_ADC_SQ8 AO_ADC_ANT_SENSE_2 +#define AO_ADC_SQ8_NAME "ant_sense_2" #define AO_ADC_SQ9 AO_ADC_GYRO_X_1 +#define AO_ADC_SQ9_NAME "gyro_x_1" #define AO_ADC_SQ10 AO_ADC_GYRO_Z_1 +#define AO_ADC_SQ10_NAME "gyro_z_1" #define AO_ADC_SQ11 AO_ADC_GYRO_X_2 +#define AO_ADC_SQ11_NAME "gyro_x_2" #define AO_ADC_SQ12 AO_ADC_GYRO_Z_2 +#define AO_ADC_SQ12_NAME "gyro_z_2" #define AO_ADC_SQ13 AO_ADC_TEMP +#define AO_ADC_SQ13_NAME "temp" #endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From 6adf9d2a17d701ed0ceb742f322fa72723149980 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 23 Jan 2013 21:37:24 -0800 Subject: altos/fox: Turn on all of the fox lights Fox has two red and two green LEDs Signed-off-by: Keith Packard --- src/fox1ihu/ao_fox1ihu.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/fox1ihu/ao_fox1ihu.c b/src/fox1ihu/ao_fox1ihu.c index 9e76d267..43ae2ea8 100644 --- a/src/fox1ihu/ao_fox1ihu.c +++ b/src/fox1ihu/ao_fox1ihu.c @@ -31,7 +31,7 @@ main(void) ao_task_init(); ao_led_init(LEDS_AVAILABLE); - ao_led_on(AO_LED_RED); + ao_led_on(AO_LED_RED|AO_LED_GREEN|AO_LED_RED_2|AO_LED_GREEN_2); ao_timer_init(); ao_spi_init(); -- cgit v1.2.3 From e0c969910a516c018e64eaa73cdbe4bc2f979835 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 21 Mar 2013 09:49:15 -0700 Subject: altos/fox: Add watchdog timer Runs at 25Hz, can be enabled and disabled via the command line Signed-off-by: Keith Packard --- src/fox1ihu/Makefile | 3 ++- src/fox1ihu/ao_fox1ihu.c | 2 ++ src/fox1ihu/ao_pins.h | 6 ++++++ 3 files changed, 10 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/fox1ihu/Makefile b/src/fox1ihu/Makefile index 714a308b..94315e79 100644 --- a/src/fox1ihu/Makefile +++ b/src/fox1ihu/Makefile @@ -41,7 +41,8 @@ ALTOS_SRC = \ ao_usb_stm.c \ ao_exti_stm.c \ ao_adc_stm.c \ - ao_data.c + ao_data.c \ + ao_watchdog.c PRODUCT=Fox1IHU-v1 PRODUCT_DEF=-DFOX diff --git a/src/fox1ihu/ao_fox1ihu.c b/src/fox1ihu/ao_fox1ihu.c index 43ae2ea8..9f7821f7 100644 --- a/src/fox1ihu/ao_fox1ihu.c +++ b/src/fox1ihu/ao_fox1ihu.c @@ -44,6 +44,8 @@ main(void) ao_usb_init(); + ao_watchdog_init(); + ao_start_scheduler(); return 0; } diff --git a/src/fox1ihu/ao_pins.h b/src/fox1ihu/ao_pins.h index 09fd5e99..e73ef63a 100644 --- a/src/fox1ihu/ao_pins.h +++ b/src/fox1ihu/ao_pins.h @@ -244,4 +244,10 @@ struct ao_adc { #define AO_ADC_SQ13 AO_ADC_TEMP #define AO_ADC_SQ13_NAME "temp" +/* Watchdog timer */ + +#define AO_WATCHDOG_INTERVAL AO_MS_TO_TICKS(40) +#define AO_WATCHDOG_PORT (&stm_gpiod) +#define AO_WATCHDOG_BIT 3 + #endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From f517aa5e042af8dd7fd36be47e4b196f212e79b0 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 21 Mar 2013 10:16:35 -0700 Subject: altos/fox: Enable M25 driver for MRAM. The M25 driver can also talk to the MRAM chip, with a few adjustments Signed-off-by: Keith Packard --- src/fox1ihu/Makefile | 4 ++++ src/fox1ihu/ao_fox1ihu.c | 6 ++++-- src/fox1ihu/ao_pins.h | 7 +++++++ 3 files changed, 15 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/fox1ihu/Makefile b/src/fox1ihu/Makefile index 94315e79..d702cca8 100644 --- a/src/fox1ihu/Makefile +++ b/src/fox1ihu/Makefile @@ -11,6 +11,8 @@ INC = \ ao_arch_funcs.h \ ao_pins.h \ ao_product.h \ + ao_watchdog.h \ + ao_storage.h \ ao_task.h \ stm32l.h \ Makefile @@ -42,6 +44,8 @@ ALTOS_SRC = \ ao_exti_stm.c \ ao_adc_stm.c \ ao_data.c \ + ao_storage.c \ + ao_m25.c \ ao_watchdog.c PRODUCT=Fox1IHU-v1 diff --git a/src/fox1ihu/ao_fox1ihu.c b/src/fox1ihu/ao_fox1ihu.c index 9f7821f7..5bbad328 100644 --- a/src/fox1ihu/ao_fox1ihu.c +++ b/src/fox1ihu/ao_fox1ihu.c @@ -17,8 +17,8 @@ #include #include -#include -#include +#include +#include int main(void) @@ -44,6 +44,8 @@ main(void) ao_usb_init(); + ao_storage_init(); + ao_watchdog_init(); ao_start_scheduler(); diff --git a/src/fox1ihu/ao_pins.h b/src/fox1ihu/ao_pins.h index e73ef63a..f901a96b 100644 --- a/src/fox1ihu/ao_pins.h +++ b/src/fox1ihu/ao_pins.h @@ -250,4 +250,11 @@ struct ao_adc { #define AO_WATCHDOG_PORT (&stm_gpiod) #define AO_WATCHDOG_BIT 3 +/* MRAM device */ + +#define M25_MAX_CHIPS 1 +#define AO_M25_SPI_CS_PORT (&stm_gpiod) +#define AO_M25_SPI_CS_MASK (1 << 0) +#define AO_M25_SPI_BUS AO_SPI_2_PB13_PB14_PB15 + #endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From 4c37ccc63d37ff0a4382f6812060d3c5fd4b6cfb Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 25 Apr 2013 21:25:12 -0700 Subject: altos/fox: Add MRAM and SD card drivers Signed-off-by: Keith Packard --- src/fox1ihu/Makefile | 8 +++++++- src/fox1ihu/ao_fox1ihu.c | 3 +++ src/fox1ihu/ao_pins.h | 22 +++++++++++++++++----- 3 files changed, 27 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/fox1ihu/Makefile b/src/fox1ihu/Makefile index d702cca8..aa5c6b4a 100644 --- a/src/fox1ihu/Makefile +++ b/src/fox1ihu/Makefile @@ -15,6 +15,9 @@ INC = \ ao_storage.h \ ao_task.h \ stm32l.h \ + ao_sdcard.h \ + ao_bufio.h \ + ao_fat.h \ Makefile #PROFILE=ao_profile.c @@ -45,7 +48,10 @@ ALTOS_SRC = \ ao_adc_stm.c \ ao_data.c \ ao_storage.c \ - ao_m25.c \ + ao_mr25.c \ + ao_sdcard.c \ + ao_bufio.c \ + ao_fat.c \ ao_watchdog.c PRODUCT=Fox1IHU-v1 diff --git a/src/fox1ihu/ao_fox1ihu.c b/src/fox1ihu/ao_fox1ihu.c index 5bbad328..2e1a2fdf 100644 --- a/src/fox1ihu/ao_fox1ihu.c +++ b/src/fox1ihu/ao_fox1ihu.c @@ -19,6 +19,7 @@ #include #include #include +#include int main(void) @@ -48,6 +49,8 @@ main(void) ao_watchdog_init(); + ao_fat_init(); + ao_start_scheduler(); return 0; } diff --git a/src/fox1ihu/ao_pins.h b/src/fox1ihu/ao_pins.h index f901a96b..028360a5 100644 --- a/src/fox1ihu/ao_pins.h +++ b/src/fox1ihu/ao_pins.h @@ -70,10 +70,13 @@ #define SPI_1_PA5_PA6_PA7 0 #define SPI_1_PB3_PB4_PB5 0 #define SPI_1_PE13_PE14_PE15 1 /* */ +#define SPI_1_OSPEEDR STM_OSPEEDR_10MHz -#define HAS_SPI_2 0 +#define HAS_SPI_2 1 #define SPI_2_PB13_PB14_PB15 1 /* */ #define SPI_2_PD1_PD3_PD4 0 +#define SPI_2_OSPEEDR STM_OSPEEDR_10MHz +#define HAS_STORAGE_DEBUG 1 #define SPI_2_PORT (&stm_gpiob) #define SPI_2_SCK_PIN 13 @@ -252,9 +255,18 @@ struct ao_adc { /* MRAM device */ -#define M25_MAX_CHIPS 1 -#define AO_M25_SPI_CS_PORT (&stm_gpiod) -#define AO_M25_SPI_CS_MASK (1 << 0) -#define AO_M25_SPI_BUS AO_SPI_2_PB13_PB14_PB15 +#define AO_MR25_SPI_CS_PORT (&stm_gpiod) +#define AO_MR25_SPI_CS_PIN 0 +#define AO_MR25_SPI_BUS AO_SPI_2_PB13_PB14_PB15 + +/* SD card */ + +#define AO_SDCARD_SPI_CS_PORT (&stm_gpiod) +#define AO_SDCARD_SPI_CS_PIN 1 +#define AO_SDCARD_SPI_BUS AO_SPI_2_PB13_PB14_PB15 +#define AO_SDCARD_SPI_PORT (&stm_gpiob) +#define AO_SDCARD_SPI_SCK_PIN 13 +#define AO_SDCARD_SPI_MISO_PIN 14 +#define AO_SDCARD_SPI_MOSI_PIN 15 #endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From 36cf713ead58a52539c87de764b022ba0dcde27d Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 27 Apr 2013 16:06:48 -0700 Subject: altos/fox: Add fox1ihu flash loader Signed-off-by: Keith Packard --- src/fox1ihu/flash-loader/Makefile | 8 ++++++++ src/fox1ihu/flash-loader/ao_pins.h | 34 ++++++++++++++++++++++++++++++++++ 2 files changed, 42 insertions(+) create mode 100644 src/fox1ihu/flash-loader/Makefile create mode 100644 src/fox1ihu/flash-loader/ao_pins.h (limited to 'src') diff --git a/src/fox1ihu/flash-loader/Makefile b/src/fox1ihu/flash-loader/Makefile new file mode 100644 index 00000000..454bc6fb --- /dev/null +++ b/src/fox1ihu/flash-loader/Makefile @@ -0,0 +1,8 @@ +# +# AltOS flash loader build +# +# + +TOPDIR=../.. +HARDWARE=fox1ihu +include $(TOPDIR)/stm/Makefile-flash.defs diff --git a/src/fox1ihu/flash-loader/ao_pins.h b/src/fox1ihu/flash-loader/ao_pins.h new file mode 100644 index 00000000..bcc3cc9c --- /dev/null +++ b/src/fox1ihu/flash-loader/ao_pins.h @@ -0,0 +1,34 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +/* External crystal at 8MHz */ +#define AO_HSE 8000000 + +#include + +/* Detatched signal, PD6 */ + +#define AO_BOOT_PIN 1 +#define AO_BOOT_APPLICATION_GPIO stm_gpiod +#define AO_BOOT_APPLICATION_PIN 6 +#define AO_BOOT_APPLICATION_VALUE 1 +#define AO_BOOT_APPLICATION_MODE 0 + +#endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From e79202f95f0e5b4a01da31d3742a775bea62cc92 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 28 Apr 2013 00:05:39 -0700 Subject: altos/fox: Run app when PD6 is low Signed-off-by: Keith Packard --- src/fox1ihu/flash-loader/ao_pins.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/fox1ihu/flash-loader/ao_pins.h b/src/fox1ihu/flash-loader/ao_pins.h index bcc3cc9c..3a942c9c 100644 --- a/src/fox1ihu/flash-loader/ao_pins.h +++ b/src/fox1ihu/flash-loader/ao_pins.h @@ -28,7 +28,7 @@ #define AO_BOOT_PIN 1 #define AO_BOOT_APPLICATION_GPIO stm_gpiod #define AO_BOOT_APPLICATION_PIN 6 -#define AO_BOOT_APPLICATION_VALUE 1 +#define AO_BOOT_APPLICATION_VALUE 0 #define AO_BOOT_APPLICATION_MODE 0 #endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From ca58096665fc1a11ca5683f0e6aafaf3e153af37 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 28 Apr 2013 00:42:31 -0700 Subject: altos/fox: update product name to show v2, enable watchdog by default Signed-off-by: Keith Packard --- src/fox1ihu/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/fox1ihu/Makefile b/src/fox1ihu/Makefile index aa5c6b4a..d80cb7af 100644 --- a/src/fox1ihu/Makefile +++ b/src/fox1ihu/Makefile @@ -54,7 +54,7 @@ ALTOS_SRC = \ ao_fat.c \ ao_watchdog.c -PRODUCT=Fox1IHU-v1 +PRODUCT=Fox1IHU-v2 PRODUCT_DEF=-DFOX IDPRODUCT=0x0024 -- cgit v1.2.3 From 54da028c492464802b048cb949d392cd83994e75 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 31 Jan 2014 17:44:45 -0800 Subject: altos/fox: Transition to current altos build environment Fix the library usage, use $(LIBS) instead of $(SAT_CLIB) -lgcc Signed-off-by: Keith Packard --- src/fox1ihu/Makefile | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/fox1ihu/Makefile b/src/fox1ihu/Makefile index d80cb7af..e3226a24 100644 --- a/src/fox1ihu/Makefile +++ b/src/fox1ihu/Makefile @@ -31,6 +31,7 @@ INC = \ #STACK_GUARD_DEF=-DHAS_STACK_GUARD=1 ALTOS_SRC = \ + ao_boot_chain.c \ ao_interrupt.c \ ao_product.c \ ao_romconfig.c \ @@ -62,14 +63,15 @@ CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STA PROGNAME=fox1ihu-v0.1 PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx SRC=$(ALTOS_SRC) ao_fox1ihu.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) -- cgit v1.2.3 From 04aa1dfa7918dcf4eea9ec4cef5a31ac2a61a00d Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 31 Jan 2014 22:39:58 -0800 Subject: altos/fox: Update ADC to fox-1 IHU Revision 2 Change ADC pin definitions to match new hardware. Signed-off-by: Keith Packard --- src/fox1ihu/ao_pins.h | 189 +++++++++++++++++++++++++++++++------------------- 1 file changed, 117 insertions(+), 72 deletions(-) (limited to 'src') diff --git a/src/fox1ihu/ao_pins.h b/src/fox1ihu/ao_pins.h index 028360a5..b5dd7283 100644 --- a/src/fox1ihu/ao_pins.h +++ b/src/fox1ihu/ao_pins.h @@ -120,42 +120,58 @@ * ADC */ #define AO_DATA_RING 32 -#define AO_ADC_NUM_SENSE 6 struct ao_adc { int16_t tx_pa_current; /* 0 ADC_IN0 */ - int16_t tx_pa_temp; /* 1 ADC_IN1 */ - int16_t tx_xo_temp; /* 2 ADC_IN2 */ - int16_t rx_xo_temp; /* 3 ADC_IN3 */ - int16_t ihu_current; /* 4 ADC_IN8 */ - int16_t rx_cd; /* 5 ADC_IN9 */ - int16_t ant_sense_1; /* 6 ADC_IN10 */ - int16_t ant_sense_2; /* 7 ADC_IN11 */ - int16_t gyro_x_1; /* 8 ADC_IN12 */ - int16_t gyro_z_1; /* 9 ADC_IN13 */ - int16_t gyro_x_2; /* 10 ADC_IN14 */ - int16_t gyro_z_2; /* 11 ADC_IN15 */ + int16_t tx_temp; /* 1 ADC_IN1 */ + int16_t exp4_temp; /* 2 ADC_IN2 */ + int16_t rx_temp; /* 3 ADC_IN3 */ + int16_t tx_analog_1; /* 4 ADC_IN4 */ + int16_t sense_batt; /* 5 ADC_IN5 */ + int16_t rx_analog_1; /* 6 ADC_IN6 */ + int16_t rx_rssi; /* 7 ADC_IN8 */ + int16_t rx_cd; /* 8 ADC_IN9 */ + int16_t ant_sense_1; /* 9 ADC_IN10 */ + int16_t ant_sense_2; /* 10 ADC_IN11 */ + int16_t gyro_x_1; /* 11 ADC_IN12 */ + int16_t gyro_z_1; /* 12 ADC_IN13 */ + int16_t gyro_vref_1; /* 13 ADC_IN24 */ + int16_t gyro_x_2; /* 14 ADC_IN14 */ + int16_t gyro_z_2; /* 15 ADC_IN15 */ + int16_t gyro_vref_2; /* 16 ADC_IN25 */ }; #define AO_ADC_TX_PA_CURRENT 0 #define AO_ADC_TX_PA_CURRENT_PORT (&stm_gpioa) #define AO_ADC_TX_PA_CURRENT_PIN 0 -#define AO_ADC_TX_PA_TEMP 1 -#define AO_ADC_TX_PA_TEMP_PORT (&stm_gpioa) -#define AO_ADC_TX_PA_TEMP_PIN 1 +#define AO_ADC_TX_TEMP 1 +#define AO_ADC_TX_TEMP_PORT (&stm_gpioa) +#define AO_ADC_TX_TEMP_PIN 1 -#define AO_ADC_TX_XO_TEMP 2 -#define AO_ADC_TX_XO_TEMP_PORT (&stm_gpioa) -#define AO_ADC_TX_XO_TEMP_PIN 2 +#define AO_ADC_EXP4_TEMP 2 +#define AO_ADC_EXP4_TEMP_PORT (&stm_gpioa) +#define AO_ADC_EXP4_TEMP_PIN 2 -#define AO_ADC_RX_XO_TEMP 3 -#define AO_ADC_RX_XO_TEMP_PORT (&stm_gpioa) -#define AO_ADC_RX_XO_TEMP_PIN 3 +#define AO_ADC_RX_TEMP 3 +#define AO_ADC_RX_TEMP_PORT (&stm_gpioa) +#define AO_ADC_RX_TEMP_PIN 3 -#define AO_ADC_IHU_CURRENT 8 -#define AO_ADC_IHU_CURRENT_PORT (&stm_gpiob) -#define AO_ADC_IHU_CURRENT_PIN 0 +#define AO_ADC_TX_ANALOG_1 4 +#define AO_ADC_TX_ANALOG_1_PORT (&stm_gpioa) +#define AO_ADC_TX_ANALOG_1_PIN 4 + +#define AO_ADC_SENSE_BATT 5 +#define AO_ADC_SENSE_BATT_PORT (&stm_gpioa) +#define AO_ADC_SENSE_BATT_PIN 5 + +#define AO_ADC_RX_ANALOG_1 6 +#define AO_ADC_RX_ANALOG_1_PORT (&stm_gpioa) +#define AO_ADC_RX_ANALOG_1_PIN 6 + +#define AO_ADC_RX_RSSI 8 +#define AO_ADC_RX_RSSI_PORT (&stm_gpiob) +#define AO_ADC_RX_RSSI_PIN 0 #define AO_ADC_RX_CD 9 #define AO_ADC_RX_CD_PORT (&stm_gpiob) @@ -177,6 +193,10 @@ struct ao_adc { #define AO_ADC_GYRO_Z_1_PORT (&stm_gpioc) #define AO_ADC_GYRO_Z_1_PIN 3 +#define AO_ADC_GYRO_VREF_1 24 +#define AO_ADC_GYRO_VREF_1_PORT (&stm_gpioe) +#define AO_ADC_GYRO_VREF_1_PIN 9 + #define AO_ADC_GYRO_X_2 14 #define AO_ADC_GYRO_X_2_PORT (&stm_gpioc) #define AO_ADC_GYRO_X_2_PIN 4 @@ -185,67 +205,92 @@ struct ao_adc { #define AO_ADC_GYRO_Z_2_PORT (&stm_gpioc) #define AO_ADC_GYRO_Z_2_PIN 5 +#define AO_ADC_GYRO_VREF_2 25 +#define AO_ADC_GYRO_VREF_2_PORT (&stm_gpioe) +#define AO_ADC_GYRO_VREF_2_PIN 10 + #define AO_ADC_TEMP 16 #define AO_ADC_RCC_AHBENR ((1 << STM_RCC_AHBENR_GPIOAEN) | \ (1 << STM_RCC_AHBENR_GPIOBEN) | \ - (1 << STM_RCC_AHBENR_GPIOCEN)) + (1 << STM_RCC_AHBENR_GPIOCEN) | \ + (1 << STM_RCC_AHBENR_GPIOEEN)) -#define AO_NUM_ADC_PIN (12) +#define AO_NUM_ADC_PIN (17) #define AO_ADC_PIN0_PORT AO_ADC_TX_PA_CURRENT_PORT #define AO_ADC_PIN0_PIN AO_ADC_TX_PA_CURRENT_PIN -#define AO_ADC_PIN1_PORT AO_ADC_TX_PA_TEMP_PORT -#define AO_ADC_PIN1_PIN AO_ADC_TX_PA_TEMP_PIN -#define AO_ADC_PIN2_PORT AO_ADC_TX_XO_TEMP_PORT -#define AO_ADC_PIN2_PIN AO_ADC_TX_XO_TEMP_PIN -#define AO_ADC_PIN3_PORT AO_ADC_RX_XO_TEMP_PORT -#define AO_ADC_PIN3_PIN AO_ADC_RX_XO_TEMP_PIN -#define AO_ADC_PIN4_PORT AO_ADC_IHU_CURRENT_PORT -#define AO_ADC_PIN4_PIN AO_ADC_IHU_CURRENT_PIN -#define AO_ADC_PIN5_PORT AO_ADC_RX_CD_PORT -#define AO_ADC_PIN5_PIN AO_ADC_RX_CD_PIN -#define AO_ADC_PIN6_PORT AO_ADC_ANT_SENSE_1_PORT -#define AO_ADC_PIN6_PIN AO_ADC_ANT_SENSE_1_PIN -#define AO_ADC_PIN7_PORT AO_ADC_ANT_SENSE_2_PORT -#define AO_ADC_PIN7_PIN AO_ADC_ANT_SENSE_2_PIN -#define AO_ADC_PIN8_PORT AO_ADC_GYRO_X_1_PORT -#define AO_ADC_PIN8_PIN AO_ADC_GYRO_X_1_PIN -#define AO_ADC_PIN9_PORT AO_ADC_GYRO_Z_1_PORT -#define AO_ADC_PIN9_PIN AO_ADC_GYRO_Z_1_PIN -#define AO_ADC_PIN10_PORT AO_ADC_GYRO_X_2_PORT -#define AO_ADC_PIN10_PIN AO_ADC_GYRO_X_2_PIN -#define AO_ADC_PIN11_PORT AO_ADC_GYRO_Z_2_PORT -#define AO_ADC_PIN11_PIN AO_ADC_GYRO_Z_2_PIN +#define AO_ADC_PIN1_PORT AO_ADC_TX_TEMP_PORT +#define AO_ADC_PIN1_PIN AO_ADC_TX_TEMP_PIN +#define AO_ADC_PIN2_PORT AO_ADC_EXP4_TEMP_PORT +#define AO_ADC_PIN2_PIN AO_ADC_EXP4_TEMP_PIN +#define AO_ADC_PIN3_PORT AO_ADC_RX_TEMP_PORT +#define AO_ADC_PIN3_PIN AO_ADC_RX_TEMP_PIN +#define AO_ADC_PIN4_PORT AO_ADC_TX_ANALOG_1_PORT +#define AO_ADC_PIN4_PIN AO_ADC_TX_ANALOG_1_PIN +#define AO_ADC_PIN5_PORT AO_ADC_SENSE_BATT_PORT +#define AO_ADC_PIN5_PIN AO_ADC_SENSE_BATT_PIN +#define AO_ADC_PIN6_PORT AO_ADC_RX_ANALOG_1_PORT +#define AO_ADC_PIN6_PIN AO_ADC_RX_ANALOG_1_PIN +#define AO_ADC_PIN7_PORT AO_ADC_RX_RSSI_PORT +#define AO_ADC_PIN7_PIN AO_ADC_RX_RSSI_PIN +#define AO_ADC_PIN8_PORT AO_ADC_RX_CD_PORT +#define AO_ADC_PIN8_PIN AO_ADC_RX_CD_PIN +#define AO_ADC_PIN9_PORT AO_ADC_ANT_SENSE_1_PORT +#define AO_ADC_PIN9_PIN AO_ADC_ANT_SENSE_1_PIN +#define AO_ADC_PIN10_PORT AO_ADC_ANT_SENSE_2_PORT +#define AO_ADC_PIN10_PIN AO_ADC_ANT_SENSE_2_PIN +#define AO_ADC_PIN11_PORT AO_ADC_GYRO_X_1_PORT +#define AO_ADC_PIN11_PIN AO_ADC_GYRO_X_1_PIN +#define AO_ADC_PIN12_PORT AO_ADC_GYRO_Z_1_PORT +#define AO_ADC_PIN12_PIN AO_ADC_GYRO_Z_1_PIN +#define AO_ADC_PIN13_PORT AO_ADC_GYRO_VREF_1_PORT +#define AO_ADC_PIN13_PIN AO_ADC_GYRO_VREF_1_PIN +#define AO_ADC_PIN14_PORT AO_ADC_GYRO_X_2_PORT +#define AO_ADC_PIN14_PIN AO_ADC_GYRO_X_2_PIN +#define AO_ADC_PIN15_PORT AO_ADC_GYRO_Z_2_PORT +#define AO_ADC_PIN15_PIN AO_ADC_GYRO_Z_2_PIN +#define AO_ADC_PIN16_PORT AO_ADC_GYRO_VREF_2_PORT +#define AO_ADC_PIN16_PIN AO_ADC_GYRO_VREF_2_PIN #define AO_NUM_ADC (AO_NUM_ADC_PIN + 1) /* Add internal temp sensor */ #define AO_ADC_SQ1 AO_ADC_TX_PA_CURRENT #define AO_ADC_SQ1_NAME "tx_pa_current" -#define AO_ADC_SQ2 AO_ADC_TX_PA_TEMP -#define AO_ADC_SQ2_NAME "tx_pa_temp" -#define AO_ADC_SQ3 AO_ADC_TX_XO_TEMP -#define AO_ADC_SQ3_NAME "tx_xo_temp" -#define AO_ADC_SQ4 AO_ADC_RX_XO_TEMP -#define AO_ADC_SQ4_NAME "rx_xo_temp" -#define AO_ADC_SQ5 AO_ADC_IHU_CURRENT -#define AO_ADC_SQ5_NAME "ihu_current" -#define AO_ADC_SQ6 AO_ADC_RX_CD -#define AO_ADC_SQ6_NAME "rx_cd" -#define AO_ADC_SQ7 AO_ADC_ANT_SENSE_1 -#define AO_ADC_SQ7_NAME "ant_sense_1" -#define AO_ADC_SQ8 AO_ADC_ANT_SENSE_2 -#define AO_ADC_SQ8_NAME "ant_sense_2" -#define AO_ADC_SQ9 AO_ADC_GYRO_X_1 -#define AO_ADC_SQ9_NAME "gyro_x_1" -#define AO_ADC_SQ10 AO_ADC_GYRO_Z_1 -#define AO_ADC_SQ10_NAME "gyro_z_1" -#define AO_ADC_SQ11 AO_ADC_GYRO_X_2 -#define AO_ADC_SQ11_NAME "gyro_x_2" -#define AO_ADC_SQ12 AO_ADC_GYRO_Z_2 -#define AO_ADC_SQ12_NAME "gyro_z_2" -#define AO_ADC_SQ13 AO_ADC_TEMP -#define AO_ADC_SQ13_NAME "temp" +#define AO_ADC_SQ2 AO_ADC_TX_TEMP +#define AO_ADC_SQ2_NAME "tx_temp" +#define AO_ADC_SQ3 AO_ADC_EXP4_TEMP +#define AO_ADC_SQ3_NAME "expr_temp" +#define AO_ADC_SQ4 AO_ADC_RX_TEMP +#define AO_ADC_SQ4_NAME "rx_temp" +#define AO_ADC_SQ5 AO_ADC_TX_ANALOG_1 +#define AO_ADC_SQ5_NAME "tx_analog_1" +#define AO_ADC_SQ6 AO_ADC_SENSE_BATT +#define AO_ADC_SQ6_NAME "sense_batt" +#define AO_ADC_SQ7 AO_ADC_RX_ANALOG_1 +#define AO_ADC_SQ7_NAME "rx_analog_1" +#define AO_ADC_SQ8 AO_ADC_RX_RSSI +#define AO_ADC_SQ8_NAME "rx_rssi" +#define AO_ADC_SQ9 AO_ADC_RX_CD +#define AO_ADC_SQ9_NAME "rx_cd" +#define AO_ADC_SQ10 AO_ADC_ANT_SENSE_1 +#define AO_ADC_SQ10_NAME "ant_sense_1" +#define AO_ADC_SQ11 AO_ADC_ANT_SENSE_2 +#define AO_ADC_SQ11_NAME "ant_sense_2" +#define AO_ADC_SQ12 AO_ADC_GYRO_X_1 +#define AO_ADC_SQ12_NAME "gyro_x_1" +#define AO_ADC_SQ13 AO_ADC_GYRO_Z_1 +#define AO_ADC_SQ13_NAME "gyro_z_1" +#define AO_ADC_SQ14 AO_ADC_GYRO_VREF_1 +#define AO_ADC_SQ14_NAME "gyro_vref_1" +#define AO_ADC_SQ15 AO_ADC_GYRO_X_2 +#define AO_ADC_SQ15_NAME "gyro_x_2" +#define AO_ADC_SQ16 AO_ADC_GYRO_Z_2 +#define AO_ADC_SQ16_NAME "gyro_z_2" +#define AO_ADC_SQ17 AO_ADC_GYRO_VREF_2 +#define AO_ADC_SQ17_NAME "gyro_vref_2" +#define AO_ADC_SQ18 AO_ADC_TEMP +#define AO_ADC_SQ18_NAME "temp" /* Watchdog timer */ -- cgit v1.2.3 From 6247e0c81084b59365ceca0ced33d2db92a72444 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 1 Feb 2014 00:06:30 -0800 Subject: altos/fox: Enable system timer in flash loader and prod watchdog with it This makes it possible to reflash the board without needing to disable the watchdog. Signed-off-by: Keith Packard --- src/fox1ihu/flash-loader/ao_pins.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) (limited to 'src') diff --git a/src/fox1ihu/flash-loader/ao_pins.h b/src/fox1ihu/flash-loader/ao_pins.h index 3a942c9c..90917a38 100644 --- a/src/fox1ihu/flash-loader/ao_pins.h +++ b/src/fox1ihu/flash-loader/ao_pins.h @@ -21,6 +21,20 @@ /* External crystal at 8MHz */ #define AO_HSE 8000000 +#define AO_WATCHDOG_PORT (&stm_gpiod) +#define AO_WATCHDOG_BIT 3 + +#define AO_FLASH_LOADER_INIT do { \ + ao_enable_output(AO_WATCHDOG_PORT, AO_WATCHDOG_BIT, AO_WATCHDOG, 0); \ + } while (0) + +#define AO_TIMER_HOOK do { \ + static uint8_t watchdog; \ + ao_gpio_set(AO_WATCHDOG_PORT, AO_WATCHDOG_BIT, AO_WATCHDOG_PIN, watchdog); \ + watchdog ^= 1; \ + } while (0) + +#define HAS_TICK 1 #include /* Detatched signal, PD6 */ -- cgit v1.2.3 From ddf70e806f894707613830fd64e21b5bb8f19972 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 27 Apr 2013 00:36:36 -0700 Subject: altos: Build fox1ihu by default Signed-off-by: Keith Packard --- src/Makefile | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/Makefile b/src/Makefile index 8420b376..e8008128 100644 --- a/src/Makefile +++ b/src/Makefile @@ -25,6 +25,7 @@ SDCCDIRS=\ telerepeat-v1.0 ARMM3DIRS=\ + fox1ihu fox1ihu/flash-loader \ easymega-v1.0 easymega-v1.0/flash-loader \ telemega-v0.1 telemega-v0.1/flash-loader \ telemega-v1.0 telemega-v1.0/flash-loader \ -- cgit v1.2.3 From 9003147a89074f8d991d1707f307fd4e41435aa3 Mon Sep 17 00:00:00 2001 From: Bdale Garbee Date: Mon, 5 May 2014 17:32:22 -0600 Subject: move signal that forces flash-loader on to PB8, active high --- src/fox1ihu/flash-loader/ao_pins.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/fox1ihu/flash-loader/ao_pins.h b/src/fox1ihu/flash-loader/ao_pins.h index 90917a38..a7afaabb 100644 --- a/src/fox1ihu/flash-loader/ao_pins.h +++ b/src/fox1ihu/flash-loader/ao_pins.h @@ -37,12 +37,12 @@ #define HAS_TICK 1 #include -/* Detatched signal, PD6 */ +/* Attached signal, PB8 */ #define AO_BOOT_PIN 1 -#define AO_BOOT_APPLICATION_GPIO stm_gpiod -#define AO_BOOT_APPLICATION_PIN 6 -#define AO_BOOT_APPLICATION_VALUE 0 +#define AO_BOOT_APPLICATION_GPIO stm_gpiob +#define AO_BOOT_APPLICATION_PIN 8 +#define AO_BOOT_APPLICATION_VALUE 1 #define AO_BOOT_APPLICATION_MODE 0 #endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From c408c773793b5a5685d95c11ba6f710200505c25 Mon Sep 17 00:00:00 2001 From: Bdale Garbee Date: Tue, 6 May 2014 00:38:33 -0600 Subject: had the sense backwards .. presence of 'attached' should put us in the loader --- src/fox1ihu/flash-loader/ao_pins.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/fox1ihu/flash-loader/ao_pins.h b/src/fox1ihu/flash-loader/ao_pins.h index a7afaabb..31201eb0 100644 --- a/src/fox1ihu/flash-loader/ao_pins.h +++ b/src/fox1ihu/flash-loader/ao_pins.h @@ -42,7 +42,7 @@ #define AO_BOOT_PIN 1 #define AO_BOOT_APPLICATION_GPIO stm_gpiob #define AO_BOOT_APPLICATION_PIN 8 -#define AO_BOOT_APPLICATION_VALUE 1 +#define AO_BOOT_APPLICATION_VALUE 0 #define AO_BOOT_APPLICATION_MODE 0 #endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From 8025b98420f16730e5e6e45114cbdbbf88f7a748 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 7 Aug 2015 20:16:41 -0700 Subject: Remove ao_radio_cal from stm products without radios Products without a radio don't need this value. Signed-off-by: Keith Packard --- src/stm/ao_romconfig.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/stm/ao_romconfig.c b/src/stm/ao_romconfig.c index 9d5fd6fb..63a48bec 100644 --- a/src/stm/ao_romconfig.c +++ b/src/stm/ao_romconfig.c @@ -24,5 +24,6 @@ AO_ROMCONFIG_SYMBOL (0) uint16_t ao_serial_number = 0; #ifndef AO_RADIO_CAL_DEFAULT #define AO_RADIO_CAL_DEFAULT 0x01020304 #endif +#if HAS_RADIO AO_ROMCONFIG_SYMBOL (0) uint32_t ao_radio_cal = AO_RADIO_CAL_DEFAULT; - +#endif -- cgit v1.2.3 From 0dfaecc25a12da57248541a66dd5118161616cce Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 21 Apr 2018 16:16:18 -0700 Subject: altos/fox1ihu: Pull down radio control pin in flash loader Ensures that the radio is powered down while in the flash loader. Signed-off-by: Keith Packard --- src/fox1ihu/flash-loader/ao_pins.h | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/fox1ihu/flash-loader/ao_pins.h b/src/fox1ihu/flash-loader/ao_pins.h index 31201eb0..19f29b08 100644 --- a/src/fox1ihu/flash-loader/ao_pins.h +++ b/src/fox1ihu/flash-loader/ao_pins.h @@ -22,16 +22,27 @@ #define AO_HSE 8000000 #define AO_WATCHDOG_PORT (&stm_gpiod) +#define AO_WATCHDOG_PORT_BIT (1 << STM_RCC_AHBENR_GPIODEN) #define AO_WATCHDOG_BIT 3 - -#define AO_FLASH_LOADER_INIT do { \ - ao_enable_output(AO_WATCHDOG_PORT, AO_WATCHDOG_BIT, AO_WATCHDOG, 0); \ +#define AO_WATCHDOG_VALUE 0 + +#define AO_RADIO_CONTROL_PORT (&stm_gpioe) +#define AO_RADIO_CONTROL_PORT_BIT (1 << STM_RCC_AHBENR_GPIOEEN) +#define AO_RADIO_CONTROL_BIT 12 +#define AO_RADIO_CONTROL_VALUE 1 + +#define AO_FLASH_LOADER_INIT do { \ + stm_rcc.ahbenr |= AO_WATCHDOG_PORT_BIT | AO_RADIO_CONTROL_PORT_BIT; \ + \ + stm_gpio_set(AO_WATCHDOG_PORT, AO_WATCHDOG_BIT, AO_WATCHDOG_VALUE); \ + stm_moder_set(AO_WATCHDOG_PORT, AO_WATCHDOG_BIT, STM_MODER_OUTPUT); \ + \ + stm_gpio_set(AO_RADIO_CONTROL_PORT, AO_RADIO_CONTROL_BIT, AO_RADIO_CONTROL_VALUE); \ + stm_moder_set(AO_RADIO_CONTROL_PORT, AO_RADIO_CONTROL_BIT, STM_MODER_OUTPUT); \ } while (0) - + #define AO_TIMER_HOOK do { \ - static uint8_t watchdog; \ - ao_gpio_set(AO_WATCHDOG_PORT, AO_WATCHDOG_BIT, AO_WATCHDOG_PIN, watchdog); \ - watchdog ^= 1; \ + AO_WATCHDOG_PORT->odr ^= (1 << AO_WATCHDOG_BIT); \ } while (0) #define HAS_TICK 1 -- cgit v1.2.3 From 772b5f1cb625fba1396a57b47498ef805ae1a9a8 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 1 May 2018 23:36:16 -0700 Subject: altos/stmf0: Change tests for AO_BOOT_CHAIN and AO_BOOT_PIN to #if Were #ifdef, which meant that #define AO_BOOT_PIN 0 didn't work right. Signed-off-by: Keith Packard --- src/stmf0/ao_interrupt.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/stmf0/ao_interrupt.c b/src/stmf0/ao_interrupt.c index fcd330f1..0d6f6113 100644 --- a/src/stmf0/ao_interrupt.c +++ b/src/stmf0/ao_interrupt.c @@ -69,9 +69,9 @@ stm_flash_size(void) { void start(void) { -#ifdef AO_BOOT_CHAIN +#if AO_BOOT_CHAIN if (ao_boot_check_chain()) { -#ifdef AO_BOOT_PIN +#if AO_BOOT_PIN ao_boot_check_pin(); #endif } -- cgit v1.2.3 From 980e3dc43ac4712680a370756e5112e9f330aa9a Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 1 May 2018 23:36:57 -0700 Subject: altos/stmf0: whitespace fix Trailing whitespace. Signed-off-by: Keith Packard --- src/stmf0/ao_flash_loader_stm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/stmf0/ao_flash_loader_stm.c b/src/stmf0/ao_flash_loader_stm.c index a8d1701b..18bf272c 100644 --- a/src/stmf0/ao_flash_loader_stm.c +++ b/src/stmf0/ao_flash_loader_stm.c @@ -34,7 +34,7 @@ main(void) #ifdef AO_FLASH_LOADER_INIT AO_FLASH_LOADER_INIT; -#endif +#endif ao_flash_task(); return 0; } -- cgit v1.2.3 From 08e543cd8b761e4cbbfa97404fcd5394f65a0e9e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 1 May 2018 23:44:00 -0700 Subject: altos/stmf0: Set 0x0 mapping to Main Flash for boot loader When DFU finishes loading firmware and jumps to the application, it leaves the mapping of addresses starting at 0x0 set to System flash, which prevents the boot loader from receiving interrupts and requires a power cycle during flash & cal. Signed-off-by: Keith Packard --- src/stmf0/ao_interrupt.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/stmf0/ao_interrupt.c b/src/stmf0/ao_interrupt.c index 0d6f6113..a67f6f1a 100644 --- a/src/stmf0/ao_interrupt.c +++ b/src/stmf0/ao_interrupt.c @@ -76,13 +76,17 @@ void start(void) #endif } #endif -#if RELOCATE_INTERRUPT /* Turn on syscfg */ stm_rcc.apb2enr |= (1 << STM_RCC_APB2ENR_SYSCFGCOMPEN); +#if RELOCATE_INTERRUPT memcpy(&__interrupt_start__, &__interrupt_rom__, &__interrupt_end__ - &__interrupt_start__); stm_syscfg.cfgr1 = (stm_syscfg.cfgr1 & ~(STM_SYSCFG_CFGR1_MEM_MODE_MASK << STM_SYSCFG_CFGR1_MEM_MODE)) | (STM_SYSCFG_CFGR1_MEM_MODE_SRAM << STM_SYSCFG_CFGR1_MEM_MODE); +#else + /* Switch to Main Flash mode (DFU loader leaves us in System mode) */ + stm_syscfg.cfgr1 = (stm_syscfg.cfgr1 & ~(STM_SYSCFG_CFGR1_MEM_MODE_MASK << STM_SYSCFG_CFGR1_MEM_MODE)) | + (STM_SYSCFG_CFGR1_MEM_MODE_MAIN_FLASH << STM_SYSCFG_CFGR1_MEM_MODE); #endif memcpy(&__data_start__, &__text_end__, &__data_end__ - &__data_start__); memset(&__bss_start__, '\0', &__bss_end__ - &__bss_start__); -- cgit v1.2.3 From f282b802d2f5a0da56bb8245169c46a16b2eed71 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 6 May 2018 21:06:31 -0700 Subject: altos/kernel: Define usb IN2/IN3 functions These are putchar and flush functions that are used when sending data to the additional IN2 and IN3 endpoints. Signed-off-by: Keith Packard --- src/kernel/ao_usb.h | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) (limited to 'src') diff --git a/src/kernel/ao_usb.h b/src/kernel/ao_usb.h index 936d939b..40516de1 100644 --- a/src/kernel/ao_usb.h +++ b/src/kernel/ao_usb.h @@ -41,7 +41,23 @@ ao_usb_pollchar(void); void ao_usb_flush(void); +#if AO_USB_HAS_IN2 +void +ao_usb_flush2(void); + +void +ao_usb_putchar2(char c); +#endif + +#if AO_USB_HAS_IN3 +void +ao_usb_flush3(void); + +void +ao_usb_putchar3(char c); +#endif /* Enable the USB controller */ + void ao_usb_enable(void); @@ -107,6 +123,7 @@ extern __code __at (0x00aa) uint8_t ao_usb_descriptors []; #define AO_USB_OUT_EP 4 #define AO_USB_IN_EP 5 #define AO_USB_IN2_EP 6 +#define AO_USB_IN3_EP 7 #endif #ifndef AO_USB_HAS_OUT @@ -125,6 +142,10 @@ extern __code __at (0x00aa) uint8_t ao_usb_descriptors []; #define AO_USB_HAS_IN2 0 #endif +#ifndef AO_USB_HAS_IN3 +#define AO_USB_HAS_IN3 0 +#endif + /* * USB bulk packets can only come in 8, 16, 32 and 64 * byte sizes, so we'll use 64 for everything -- cgit v1.2.3 From 8efe0d40deded973f08f63eb650a036f9e24d2fb Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 6 May 2018 21:17:32 -0700 Subject: altos/kernel: Add USB descriptors for IN3 This adds the necessary descriptor information to support another IN endpoint for applications. Signed-off-by: Keith Packard --- src/kernel/ao_product.c | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/kernel/ao_product.c b/src/kernel/ao_product.c index c4df9f26..4c2d83ef 100644 --- a/src/kernel/ao_product.c +++ b/src/kernel/ao_product.c @@ -55,7 +55,7 @@ const char ao_product[] = AO_iProduct_STRING; #define HEADER_LEN 9 #define CONTROL_CLASS_LEN 35 -#define DATA_LEN (9 + 7 * AO_USB_HAS_OUT + 7 * AO_USB_HAS_IN + 7 * AO_USB_HAS_IN2) +#define DATA_LEN (9 + 7 * AO_USB_HAS_OUT + 7 * AO_USB_HAS_IN + 7 * AO_USB_HAS_IN2 + 7 * AO_USB_HAS_IN3) #define TOTAL_LENGTH (HEADER_LEN + AO_USB_HAS_INT * CONTROL_CLASS_LEN + DATA_LEN) #define NUM_INTERFACES (AO_USB_HAS_INT + 1) @@ -141,7 +141,7 @@ AO_ROMCONFIG_SYMBOL(0x00aa) uint8_t ao_usb_descriptors [] = AO_USB_DESC_INTERFACE, AO_USB_HAS_INT, /* bInterfaceNumber */ 0x00, /* bAlternateSetting */ - AO_USB_HAS_OUT + AO_USB_HAS_IN + AO_USB_HAS_IN2, /* bNumEndPoints */ + AO_USB_HAS_OUT + AO_USB_HAS_IN + AO_USB_HAS_IN2 + AO_USB_HAS_IN3, /* bNumEndPoints */ AO_USB_INTERFACE_CLASS_DATA, /* bInterfaceClass = data */ 0x00, /* bInterfaceSubClass */ 0x00, /* bInterfaceProtocol */ @@ -177,6 +177,16 @@ AO_ROMCONFIG_SYMBOL(0x00aa) uint8_t ao_usb_descriptors [] = 0x00, /* bInterval */ #endif +#if AO_USB_HAS_IN3 + /* Data EP in 3 */ + 0x07, + AO_USB_DESC_ENDPOINT, + AO_USB_IN3_EP|0x80, /* bEndpointAddress */ + 0x02, /* bmAttributes = bulk */ + LE_WORD(AO_USB_IN_SIZE),/* wMaxPacketSize */ + 0x00, /* bInterval */ +#endif + /* String descriptors */ 0x04, AO_USB_DESC_STRING, -- cgit v1.2.3 From b15549d8c5277ba3aa425e232473a17dc136e5a4 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 6 May 2018 21:09:22 -0700 Subject: altos/stmf0: Toggle IN2 SW_BUF bit when sending data This tells the hardware we're done writing data to the second input buffer and allows it to be switched from NAK to VALID. Signed-off-by: Keith Packard --- src/stmf0/ao_usb_stm.c | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src') diff --git a/src/stmf0/ao_usb_stm.c b/src/stmf0/ao_usb_stm.c index 483d2419..3d227f20 100644 --- a/src/stmf0/ao_usb_stm.c +++ b/src/stmf0/ao_usb_stm.c @@ -1132,6 +1132,9 @@ _ao_usb_in2_send(void) /* Toggle our usage */ ao_usb_in_tx2_which = 1 - ao_usb_in_tx2_which; + /* Toggle the SW_BUF flag */ + _ao_usb_toggle_dtog(AO_USB_IN2_EPR, 1, 0); + /* Mark the outgoing buffer as valid */ _ao_usb_set_stat_tx(AO_USB_IN2_EPR, STM_USB_EPR_STAT_TX_VALID); -- cgit v1.2.3 From a06c283c358455008cd1e5376ccc0b6f72c7ac87 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 6 May 2018 21:11:02 -0700 Subject: altos/stmf0: Add IN3 alternate endpoint support This adds the code necessary to drive another IN endpoint. Signed-off-by: Keith Packard --- src/stmf0/ao_usb_stm.c | 145 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 145 insertions(+) (limited to 'src') diff --git a/src/stmf0/ao_usb_stm.c b/src/stmf0/ao_usb_stm.c index 3d227f20..a99b4cff 100644 --- a/src/stmf0/ao_usb_stm.c +++ b/src/stmf0/ao_usb_stm.c @@ -113,6 +113,12 @@ static uint8_t ao_usb_in_tx2_which; static uint8_t ao_usb_tx2_count; #endif +#if AO_USB_HAS_IN3 +static uint16_t ao_usb_in_tx3_offset; +static uint8_t ao_usb_in_tx3_which; +static uint8_t ao_usb_tx3_count; +#endif + /* * End point register indices */ @@ -122,6 +128,7 @@ static uint8_t ao_usb_tx2_count; #define AO_USB_OUT_EPR 2 #define AO_USB_IN_EPR 3 #define AO_USB_IN2_EPR 4 +#define AO_USB_IN3_EPR 5 /* Marks when we don't need to send an IN packet. * This happens only when the last IN packet is not full, @@ -146,6 +153,16 @@ static uint16_t in2_count; static uint8_t ao_usb_in2_flushed; #endif +#if AO_USB_HAS_IN3 +/* 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_in3_pending; +static uint16_t in3_count; +static uint8_t ao_usb_in3_flushed; +#endif + /* Marks when an OUT packet has been received by the hardware * but not pulled to the shadow buffer. */ @@ -423,6 +440,11 @@ ao_usb_alloc_buffers(void) ao_usb_in_tx2_offset = sram_addr; sram_addr += AO_USB_IN_SIZE * 2; #endif +#if AO_USB_HAS_IN3 + sram_addr += (sram_addr & 1); + ao_usb_in_tx3_offset = sram_addr; + sram_addr += AO_USB_IN_SIZE * 2; +#endif } static void @@ -558,6 +580,25 @@ ao_usb_set_configuration(void) ao_usb_in_tx2_which = 0; #endif +#if AO_USB_HAS_IN3 + /* Set up the IN3 end point */ + stm_usb_bdt[AO_USB_IN3_EPR].double_tx[0].addr = ao_usb_in_tx3_offset; + stm_usb_bdt[AO_USB_IN3_EPR].double_tx[0].count = 0; + stm_usb_bdt[AO_USB_IN3_EPR].double_tx[1].addr = ao_usb_in_tx3_offset + AO_USB_IN_SIZE; + stm_usb_bdt[AO_USB_IN3_EPR].double_tx[1].count = 0; + + ao_usb_init_ep(AO_USB_IN3_EPR, + AO_USB_IN3_EP, + STM_USB_EPR_EP_TYPE_BULK, + STM_USB_EPR_STAT_RX_DISABLED, + STM_USB_EPR_STAT_TX_NAK, + STM_USB_EPR_EP_KIND_DBL_BUF, + 0, 1); + + /* First transmit data goes to buffer 0 */ + ao_usb_in_tx3_which = 0; +#endif + ao_usb_in_flushed = 0; ao_usb_in_pending = 0; ao_wakeup(&ao_usb_in_pending); @@ -567,6 +608,12 @@ ao_usb_set_configuration(void) ao_wakeup(&ao_usb_in2_pending); #endif +#if AO_USB_HAS_IN3 + ao_usb_in3_flushed = 0; + ao_usb_in3_pending = 0; + ao_wakeup(&ao_usb_in3_pending); +#endif + ao_usb_out_avail = 0; ao_usb_configuration = 0; @@ -995,6 +1042,16 @@ stm_usb_isr(void) ao_wakeup(&ao_usb_in2_pending); } break; +#endif +#if AO_USB_HAS_IN3 + case AO_USB_IN3_EPR: + ++in3_count; + _tx_dbg1("TX3 ISR", epr); + if (ao_usb_epr_ctr_tx(epr)) { + ao_usb_in3_pending = 0; + ao_wakeup(&ao_usb_in3_pending); + } + break; #endif case AO_USB_INT_EPR: #if USB_STATUS @@ -1202,6 +1259,94 @@ ao_usb_putchar2(char c) } #endif +#if AO_USB_HAS_IN3 +/* Queue the current IN buffer for transmission */ +static void +_ao_usb_in3_send(void) +{ + _tx_dbg0("in3_send start"); + debug ("send3 %d\n", ao_usb_tx3_count); + while (ao_usb_in3_pending) + ao_sleep(&ao_usb_in3_pending); + ao_usb_in3_pending = 1; + if (ao_usb_tx3_count != AO_USB_IN_SIZE) + ao_usb_in3_flushed = 1; + stm_usb_bdt[AO_USB_IN3_EPR].double_tx[ao_usb_in_tx3_which].count = ao_usb_tx3_count; + ao_usb_tx3_count = 0; + + /* Toggle our usage */ + ao_usb_in_tx3_which = 1 - ao_usb_in_tx3_which; + + /* Toggle the SW_BUF flag */ + _ao_usb_toggle_dtog(AO_USB_IN3_EPR, 1, 0); + + /* Mark the outgoing buffer as valid */ + _ao_usb_set_stat_tx(AO_USB_IN3_EPR, STM_USB_EPR_STAT_TX_VALID); + + _tx_dbg0("in3_send end"); +} + +/* Wait for a free IN buffer. Interrupts are blocked */ +static void +_ao_usb_in3_wait(void) +{ + for (;;) { + /* Check if the current buffer is writable */ + if (ao_usb_tx3_count < AO_USB_IN_SIZE) + break; + + _tx_dbg0("in3_wait top"); + /* Wait for an IN buffer to be ready */ + while (ao_usb_in3_pending) + ao_sleep(&ao_usb_in3_pending); + _tx_dbg0("in_wait bottom"); + } +} + +void +ao_usb_flush3(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_in3_flushed) { + _tx_dbg0("flush3 top"); + _ao_usb_in3_send(); + _tx_dbg0("flush3 end"); + } + ao_arch_release_interrupts(); +} + +void +ao_usb_putchar3(char c) +{ + if (!ao_usb_running) + return; + + ao_arch_block_interrupts(); + _ao_usb_in3_wait(); + + ao_usb_in3_flushed = 0; + ao_usb_tx_byte(ao_usb_in_tx3_offset + AO_USB_IN_SIZE * ao_usb_in_tx3_which + ao_usb_tx3_count++, c); + + /* Send the packet when full */ + if (ao_usb_tx3_count == AO_USB_IN_SIZE) { + _tx_dbg0("putchar3 full"); + _ao_usb_in3_send(); + _tx_dbg0("putchar3 flushed"); + } + ao_arch_release_interrupts(); +} +#endif + #if AO_USB_HAS_OUT static void _ao_usb_out_recv(void) -- cgit v1.2.3 From 4451f7b6bade66775a197b93c6e70ba15f1826ce Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 6 May 2018 21:11:48 -0700 Subject: altos/stmf0: Fix up USB debug code At least make it compile. In this configuration, it's dumping out IN3 endpoint register values. Signed-off-by: Keith Packard --- src/stmf0/ao_usb_stm.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/stmf0/ao_usb_stm.c b/src/stmf0/ao_usb_stm.c index a99b4cff..c4860d8e 100644 --- a/src/stmf0/ao_usb_stm.c +++ b/src/stmf0/ao_usb_stm.c @@ -1689,9 +1689,9 @@ struct ao_usb_dbg { #endif }; -#define NUM_USB_DBG 128 +#define NUM_USB_DBG 16 -struct ao_usb_dbg dbg[128]; +struct ao_usb_dbg dbg[NUM_USB_DBG]; int dbg_i; static void _dbg(int line, char *msg, uint32_t value) @@ -1703,11 +1703,11 @@ static void _dbg(int line, char *msg, uint32_t 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_epr = stm_usb.epr[AO_USB_IN_EPR]; - 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; + dbg[dbg_i].in_count = in3_count; + dbg[dbg_i].in_epr = stm_usb.epr[AO_USB_IN3_EPR].r; + dbg[dbg_i].in_pending = ao_usb_in3_pending; + dbg[dbg_i].tx_count = ao_usb_tx3_count; + dbg[dbg_i].in_flushed = ao_usb_in3_flushed; #endif #if RX_DBG dbg[dbg_i].rx_count = ao_usb_rx_count; -- cgit v1.2.3 From bea42e45952df85d61428662caefbb100465a585 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 6 May 2018 21:13:02 -0700 Subject: altos/chaoskey-v1.0: Add endpoint for reading flash contents This creates another IN endpoint which provides the contents of flash for validation of the firmware load on the host. Signed-off-by: Keith Packard --- src/chaoskey-v1.0/Makefile | 4 +++- src/chaoskey-v1.0/ao_chaoskey.c | 2 ++ src/chaoskey-v1.0/ao_pins.h | 4 ++++ src/kernel/ao_flash_readout.c | 50 +++++++++++++++++++++++++++++++++++++++++ src/kernel/ao_flash_readout.h | 20 +++++++++++++++++ 5 files changed, 79 insertions(+), 1 deletion(-) create mode 100644 src/kernel/ao_flash_readout.c create mode 100644 src/kernel/ao_flash_readout.h (limited to 'src') diff --git a/src/chaoskey-v1.0/Makefile b/src/chaoskey-v1.0/Makefile index dea5b483..c6cf45bd 100644 --- a/src/chaoskey-v1.0/Makefile +++ b/src/chaoskey-v1.0/Makefile @@ -14,6 +14,7 @@ INC = \ ao_task.h \ ao_adc_fast.h \ ao_power.h \ + ao_flash_readout.h \ ao_crc.h \ stm32f0.h @@ -34,6 +35,7 @@ ALTOS_SRC = \ ao_boot_chain.c \ ao_usb_stm.c \ ao_trng_send.c \ + ao_flash_readout.c \ ao_task.c \ ao_power.c \ ao_gpio.c \ @@ -84,7 +86,7 @@ check: $(METAINFO) distclean: clean clean: - rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx + rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx *.bin rm -f ao_product.h rm -f *.cab diff --git a/src/chaoskey-v1.0/ao_chaoskey.c b/src/chaoskey-v1.0/ao_chaoskey.c index c3acd441..1165e454 100644 --- a/src/chaoskey-v1.0/ao_chaoskey.c +++ b/src/chaoskey-v1.0/ao_chaoskey.c @@ -20,6 +20,7 @@ #include #include #include +#include void main(void) { @@ -30,6 +31,7 @@ void main(void) ao_dma_init(); ao_adc_init(); ao_crc_init(); + ao_flash_readout_init(); ao_usb_init(); diff --git a/src/chaoskey-v1.0/ao_pins.h b/src/chaoskey-v1.0/ao_pins.h index f2c46d8b..22861d9d 100644 --- a/src/chaoskey-v1.0/ao_pins.h +++ b/src/chaoskey-v1.0/ao_pins.h @@ -50,6 +50,7 @@ #define AO_USB_HAS_OUT 0 #define AO_USB_HAS_IN 1 #define AO_USB_HAS_IN2 1 +#define AO_USB_HAS_IN3 1 #define AO_USB_HAS_INT 0 #define AO_USB_SELF_POWER 0 #define AO_USB_DEVICE_ID_SERIAL 1 @@ -58,6 +59,9 @@ #define IS_FLASH_LOADER 0 +#define AO_FLASH_READOUT 1 +#define ao_flash_readout_putchar(c) ao_usb_putchar3(c) + /* ADC */ #define AO_ADC_PIN0_PORT (&stm_gpioa) diff --git a/src/kernel/ao_flash_readout.c b/src/kernel/ao_flash_readout.c new file mode 100644 index 00000000..46b5ba7a --- /dev/null +++ b/src/kernel/ao_flash_readout.c @@ -0,0 +1,50 @@ +/* + * Copyright © 2018 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + */ + +#include +#include +#include + +#ifndef AO_FLASH_READOUT_BASE +#define AO_FLASH_READOUT_BASE AO_BOOT_LOADER_BASE +#define AO_FLASH_READOUT_BOUND AO_BOOT_APPLICATION_BOUND +#endif + +static void +ao_flash_readout(void) +{ + uint8_t *base = (uint8_t *) AO_FLASH_READOUT_BASE; + uint8_t *bound = (uint8_t *) AO_FLASH_READOUT_BOUND; + uint8_t *p = base; + + for (;;) { + ao_arch_block_interrupts(); + while (!ao_usb_running) { + p = base; + ao_sleep(&ao_usb_running); + } + ao_arch_release_interrupts(); + ao_flash_readout_putchar(*p++); + if (p == bound) + p = base; + } +} + +static struct ao_task ao_flash_readout_task; + +void +ao_flash_readout_init(void) +{ + ao_add_task(&ao_flash_readout_task, ao_flash_readout, "flash_readout"); +} diff --git a/src/kernel/ao_flash_readout.h b/src/kernel/ao_flash_readout.h new file mode 100644 index 00000000..5eee53cc --- /dev/null +++ b/src/kernel/ao_flash_readout.h @@ -0,0 +1,20 @@ +/* + * Copyright © 2018 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + */ + +#ifndef _AO_FLASH_READOUT_H_ +#define _AO_FLASH_READOUT_H_ + +void ao_flash_readout_init(void); + +#endif /* _AO_FLASH_READOUT_H_ */ -- cgit v1.2.3 From cc83d57454ed07e4828b4413e5af6ae2ecfe2e5a Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 7 May 2018 08:51:36 -0700 Subject: altos: Eliminate height requirement for coast detect We had required a minimum altitude of 100m to transition from boost to coast. With small motors in a heavy multi-staged rocket, this can fail to detect coast in time to light the second motor. Also, this would fail to deploy recovery systems if the flight failed before reaching 100m. Signed-off-by: Keith Packard --- src/kernel/ao_flight.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index cb02c454..7b3cb9fa 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -233,7 +233,7 @@ ao_flight(void) * deceleration, or by waiting until the maximum burn duration * (15 seconds) has past. */ - if ((ao_accel < AO_MSS_TO_ACCEL(-2.5) && ao_height > AO_M_TO_HEIGHT(100)) || + if ((ao_accel < AO_MSS_TO_ACCEL(-2.5)) || (int16_t) (ao_sample_tick - ao_boost_tick) > BOOST_TICKS_MAX) { #if HAS_ACCEL @@ -310,7 +310,7 @@ ao_flight(void) #if HAS_ACCEL else { check_re_boost: - ao_coast_avg_accel = ao_coast_avg_accel - (ao_coast_avg_accel >> 6) + (ao_accel >> 6); + ao_coast_avg_accel = ao_coast_avg_accel + ((ao_accel - ao_coast_avg_accel) >> 5); if (ao_coast_avg_accel > AO_MSS_TO_ACCEL(20)) { ao_boost_tick = ao_sample_tick; ao_flight_state = ao_flight_boost; -- cgit v1.2.3 From 7b11a34bb031035883bac97952e5ca6db0684e33 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 7 May 2018 08:56:32 -0700 Subject: altos/test: Replace state name with 10* state value in test log. Fix raw speed Using a state value means we can plot state changes along with the rest of the graph. Raw speed (simple integrated acceleration) was busted; mostly needing to skip the first accel sample. Signed-off-by: Keith Packard --- src/test/ao_flight_test.c | 13 +++++++++---- src/test/plottest | 4 +++- 2 files changed, 12 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 2d862f82..8fe3b5df 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -504,7 +504,7 @@ ao_insert(void) ao_data_ring[ao_data_head] = ao_data_static; if (ao_flight_state != ao_flight_startup) { #if HAS_ACCEL - double accel = ((ao_flight_ground_accel - ao_data_accel_cook(&ao_data_static)) * GRAVITY * 2.0) / + double accel = ((ao_flight_ground_accel - ao_data_accel(&ao_data_static)) * GRAVITY * 2.0) / (ao_config.accel_minus_g - ao_config.accel_plus_g); #else double accel = 0.0; @@ -515,7 +515,12 @@ ao_insert(void) tick_offset = -ao_data_static.tick; if ((prev_tick - ao_data_static.tick) > 0x400) tick_offset += 65536; - simple_speed += accel * (ao_data_static.tick - prev_tick) / 100.0; + if (prev_tick) { + int ticks = ao_data_static.tick - prev_tick; + if (ticks < 0) + ticks += 65536; + simple_speed += accel * ticks / 100.0; + } prev_tick = ao_data_static.tick; time = (double) (ao_data_static.tick + tick_offset) / 100; @@ -653,7 +658,7 @@ ao_insert(void) #if 1 printf("%7.2f height %8.2f accel %8.3f accel_speed %8.3f " - "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d" + "state %d k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d" #if TELEMEGA " 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 " @@ -663,7 +668,7 @@ ao_insert(void) height, accel, simple_speed > -100.0 ? simple_speed : -100.0, - ao_state_names[ao_flight_state], + ao_flight_state * 10, ao_k_height / 65536.0, ao_k_speed / 65536.0 / 16.0, ao_k_accel / 65536.0 / 16.0, diff --git a/src/test/plottest b/src/test/plottest index 95337f10..e427604a 100755 --- a/src/test/plottest +++ b/src/test/plottest @@ -1,4 +1,5 @@ gnuplot -persist << EOF +set title "$1" set ylabel "altitude (m)" set y2label "velocity (m/s), acceleration(m/s²)" set xlabel "time (s)" @@ -13,5 +14,6 @@ plot "$1" using 1:3 with lines axes x1y1 title "raw height",\ "$1" using 1:15 with lines axes x1y2 title "accel",\ "$1" using 1:19 with lines axes x1y1 title "drogue",\ "$1" using 1:21 with lines axes x1y1 title "main",\ -"$1" using 1:23 with lines axes x1y1 title "error" +"$1" using 1:23 with lines axes x1y1 title "error",\ +"$1" using 1:9 with lines axes x1y2 title "state" EOF -- cgit v1.2.3 From 5ddd4e10bd8ddb4a00a0ccd8982db3311ec5a9e7 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 20 Jan 2016 00:00:07 -0800 Subject: altos: Add µPusb v3.0 project MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This replaces the ft230 with a stm32f04, saving a bit of BOM and giving us control over the firmware. Signed-off-by: Keith Packard --- src/mpusb-v3.0/.gitignore | 2 + src/mpusb-v3.0/Makefile | 67 ++++++++++++++++++++++++++++++++++ src/mpusb-v3.0/ao_mpusb.c | 61 +++++++++++++++++++++++++++++++ src/mpusb-v3.0/ao_pins.h | 65 +++++++++++++++++++++++++++++++++ src/mpusb-v3.0/flash-loader/.gitignore | 2 + src/mpusb-v3.0/flash-loader/Makefile | 7 ++++ src/mpusb-v3.0/flash-loader/ao_pins.h | 34 +++++++++++++++++ 7 files changed, 238 insertions(+) create mode 100644 src/mpusb-v3.0/.gitignore create mode 100644 src/mpusb-v3.0/Makefile create mode 100644 src/mpusb-v3.0/ao_mpusb.c create mode 100644 src/mpusb-v3.0/ao_pins.h create mode 100644 src/mpusb-v3.0/flash-loader/.gitignore create mode 100644 src/mpusb-v3.0/flash-loader/Makefile create mode 100644 src/mpusb-v3.0/flash-loader/ao_pins.h (limited to 'src') diff --git a/src/mpusb-v3.0/.gitignore b/src/mpusb-v3.0/.gitignore new file mode 100644 index 00000000..7c1cb599 --- /dev/null +++ b/src/mpusb-v3.0/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +mpusb-v3.0-* diff --git a/src/mpusb-v3.0/Makefile b/src/mpusb-v3.0/Makefile new file mode 100644 index 00000000..5d42b53b --- /dev/null +++ b/src/mpusb-v3.0/Makefile @@ -0,0 +1,67 @@ +# +# AltOS build +# +# + +include ../stmf0/Makefile.defs + +INC = \ + ao.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_boot.h \ + ao_pins.h \ + ao_product.h \ + ao_task.h \ + ao_whiten.h \ + stm32f0.h \ + Makefile + +ALTOS_SRC = \ + ao_boot_chain.c \ + ao_interrupt.c \ + ao_product.c \ + ao_romconfig.c \ + ao_cmd.c \ + ao_dma_stm.c \ + ao_task.c \ + ao_stdio.c \ + ao_panic.c \ + ao_timer.c \ + ao_mutex.c \ + ao_usb_stm.c \ + ao_power.c \ + ao_serial_stm.c + +PRODUCT=MicroPeakUSB-v3.0 +PRODUCT_DEF=-DMPUSB +IDPRODUCT=0x002b + +CFLAGS = $(PRODUCT_DEF) $(STMF0_CFLAGS) -Os -g + +PROGNAME=mpusb-v3.0 +PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx + +SRC=$(ALTOS_SRC) ao_mpusb.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) + +distclean: clean + +clean: + rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx + rm -f ao_product.h + +install: + +uninstall: diff --git a/src/mpusb-v3.0/ao_mpusb.c b/src/mpusb-v3.0/ao_mpusb.c new file mode 100644 index 00000000..dc30e197 --- /dev/null +++ b/src/mpusb-v3.0/ao_mpusb.c @@ -0,0 +1,61 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include + +static struct ao_task ao_report_task; + +static void +ao_report(void) +{ + int c; + ao_up_set_speed(AO_SERIAL_SPEED_9600); + for (;;) { + ao_arch_block_interrupts(); + c = _ao_up_pollchar(); + ao_arch_release_interrupts(); + if (c == AO_READ_AGAIN) { + flush(); + c = ao_up_getchar(); + } + putchar(c); + } +} + +int +main(void) +{ + ao_clock_init(); + + ao_task_init(); + + ao_timer_init(); + + ao_serial_init(); + + ao_dma_init(); + + ao_cmd_init(); + + ao_usb_init(); + + ao_add_task(&ao_report_task, ao_report, "report"); + + ao_start_scheduler(); + return 0; +} diff --git a/src/mpusb-v3.0/ao_pins.h b/src/mpusb-v3.0/ao_pins.h new file mode 100644 index 00000000..0c211861 --- /dev/null +++ b/src/mpusb-v3.0/ao_pins.h @@ -0,0 +1,65 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +/* Using TeleMetrum v1.9 board */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +#define HAS_TASK_QUEUE 1 + +#define IS_FLASH_LOADER 0 + +#define AO_POWER_MANAGEMENT 1 + +/* 48MHz clock based on USB */ +#define AO_HSI48 1 + +/* HCLK = 48MHz */ +#define AO_AHB_PRESCALER 1 +#define AO_RCC_CFGR_HPRE_DIV STM_RCC_CFGR_HPRE_DIV_1 + +/* APB = 48MHz */ +#define AO_APB_PRESCALER 1 +#define AO_RCC_CFGR_PPRE_DIV STM_RCC_CFGR_PPRE_DIV_1 + +#define HAS_EEPROM 0 +#define USE_INTERNAL_FLASH 0 +#define USE_STORAGE_CONFIG 0 +#define USE_EEPROM_CONFIG 0 + +#define HAS_BEEP 0 + +#define HAS_USB 1 +#define AO_USB_DIRECTIO 0 +#define AO_PA11_PA12_RMP 1 + +#define HAS_SPI_0 0 + +#define LOW_LEVEL_DEBUG 0 + +#define HAS_SERIAL_2 1 +#define USE_SERIAL_2_STDIN 0 +#define SERIAL_2_PA2_PA3 1 +#define SERIAL_2_SWAP 0 + +#define ao_up_getchar ao_serial2_getchar +#define ao_up_putchar ao_serial2_putchar +#define _ao_up_pollchar _ao_serial2_pollchar +#define ao_up_set_speed ao_serial2_set_speed + +#endif /* _AO_PINS_H_ */ diff --git a/src/mpusb-v3.0/flash-loader/.gitignore b/src/mpusb-v3.0/flash-loader/.gitignore new file mode 100644 index 00000000..1f8b3545 --- /dev/null +++ b/src/mpusb-v3.0/flash-loader/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +mpusb* diff --git a/src/mpusb-v3.0/flash-loader/Makefile b/src/mpusb-v3.0/flash-loader/Makefile new file mode 100644 index 00000000..8d0f9e42 --- /dev/null +++ b/src/mpusb-v3.0/flash-loader/Makefile @@ -0,0 +1,7 @@ +# +# AltOS flash loader build +# + +TOPDIR=../.. +HARDWARE=mpusb-v3.0 +include $(TOPDIR)/stmf0/Makefile-flash.defs diff --git a/src/mpusb-v3.0/flash-loader/ao_pins.h b/src/mpusb-v3.0/flash-loader/ao_pins.h new file mode 100644 index 00000000..7feb9362 --- /dev/null +++ b/src/mpusb-v3.0/flash-loader/ao_pins.h @@ -0,0 +1,34 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +#include + +/* Debug port PA15 (pin 23) */ + +#define AO_BOOT_PIN 1 +#define AO_BOOT_APPLICATION_GPIO (stm_gpioa) +#define AO_BOOT_APPLICATION_PIN 15 +#define AO_BOOT_APPLICATION_VALUE 1 +#define AO_BOOT_APPLICATION_MODE AO_EXTI_MODE_PULL_UP + +#define AO_USB_DIRECTIO 0 +#define AO_PA11_PA12_RMP 1 + +#endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From bc70f92966221f941b96177b401744a7aca24814 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 7 May 2018 09:42:28 -0700 Subject: altos/stmf0: leave ao_power_gpio names undefined without power management Should make it more obvious at compile time that you've done something wrong. Signed-off-by: Keith Packard --- src/stmf0/ao_arch_funcs.h | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/stmf0/ao_arch_funcs.h b/src/stmf0/ao_arch_funcs.h index 56a3bc75..96c033f9 100644 --- a/src/stmf0/ao_arch_funcs.h +++ b/src/stmf0/ao_arch_funcs.h @@ -171,10 +171,12 @@ ao_spi_try_get_mask(struct stm_gpio *reg, uint16_t mask, uint8_t bus, uint32_t s #define ao_spi_get_bit(reg,bit,pin,bus,speed) ao_spi_get_mask(reg,(1< Date: Mon, 7 May 2018 09:43:27 -0700 Subject: altos/mpusb-v3.0: Make it compile again A few things have changed since this code was written. Signed-off-by: Keith Packard --- src/mpusb-v3.0/Makefile | 1 + src/mpusb-v3.0/ao_pins.h | 2 ++ 2 files changed, 3 insertions(+) (limited to 'src') diff --git a/src/mpusb-v3.0/Makefile b/src/mpusb-v3.0/Makefile index 5d42b53b..96156741 100644 --- a/src/mpusb-v3.0/Makefile +++ b/src/mpusb-v3.0/Makefile @@ -31,6 +31,7 @@ ALTOS_SRC = \ ao_mutex.c \ ao_usb_stm.c \ ao_power.c \ + ao_gpio.c \ ao_serial_stm.c PRODUCT=MicroPeakUSB-v3.0 diff --git a/src/mpusb-v3.0/ao_pins.h b/src/mpusb-v3.0/ao_pins.h index 0c211861..3e79aad3 100644 --- a/src/mpusb-v3.0/ao_pins.h +++ b/src/mpusb-v3.0/ao_pins.h @@ -56,6 +56,8 @@ #define USE_SERIAL_2_STDIN 0 #define SERIAL_2_PA2_PA3 1 #define SERIAL_2_SWAP 0 +#define USE_SERIAL_2_FLOW 0 +#define USE_SERIAL_2_SW_FLOW 0 #define ao_up_getchar ao_serial2_getchar #define ao_up_putchar ao_serial2_putchar -- cgit v1.2.3 From 06dac6551418ba817798c187f198b9b00c1dda74 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 7 May 2018 11:26:42 -0700 Subject: altos: add 'vidtime', a simple brightness monitor This is a testing tool for video refresh tracking. It uses the mpusb v3.0 hardware and monitors the phototransistor value, sending 0 or 1 to the USB port when the value changes. Signed-off-by: Keith Packard --- src/vidtime/.gitignore | 2 + src/vidtime/Makefile | 65 ++++++++++++++++++++++ src/vidtime/ao_pins.h | 56 +++++++++++++++++++ src/vidtime/ao_vidtime.c | 105 ++++++++++++++++++++++++++++++++++++ src/vidtime/flash-loader/.gitignore | 2 + src/vidtime/flash-loader/Makefile | 7 +++ src/vidtime/flash-loader/ao_pins.h | 34 ++++++++++++ 7 files changed, 271 insertions(+) create mode 100644 src/vidtime/.gitignore create mode 100644 src/vidtime/Makefile create mode 100644 src/vidtime/ao_pins.h create mode 100644 src/vidtime/ao_vidtime.c create mode 100644 src/vidtime/flash-loader/.gitignore create mode 100644 src/vidtime/flash-loader/Makefile create mode 100644 src/vidtime/flash-loader/ao_pins.h (limited to 'src') diff --git a/src/vidtime/.gitignore b/src/vidtime/.gitignore new file mode 100644 index 00000000..471b7fd9 --- /dev/null +++ b/src/vidtime/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +vidtime-* diff --git a/src/vidtime/Makefile b/src/vidtime/Makefile new file mode 100644 index 00000000..327614bc --- /dev/null +++ b/src/vidtime/Makefile @@ -0,0 +1,65 @@ +# +# AltOS build +# +# + +include ../stmf0/Makefile.defs + +INC = \ + ao.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_boot.h \ + ao_pins.h \ + ao_product.h \ + ao_task.h \ + stm32f0.h \ + Makefile + +ALTOS_SRC = \ + ao_boot_chain.c \ + ao_interrupt.c \ + ao_product.c \ + ao_romconfig.c \ + ao_cmd.c \ + ao_dma_stm.c \ + ao_exti_stm.c \ + ao_task.c \ + ao_stdio.c \ + ao_panic.c \ + ao_timer.c \ + ao_mutex.c \ + ao_usb_stm.c + +PRODUCT=VidTime +PRODUCT_DEF=-DVIDTIME +IDPRODUCT=0x002b + +CFLAGS = $(PRODUCT_DEF) $(STMF0_CFLAGS) -Os -g + +PROGNAME=vidtime-v1.0 +PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx + +SRC=$(ALTOS_SRC) ao_vidtime.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) + +distclean: clean + +clean: + rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx + rm -f ao_product.h + +install: + +uninstall: diff --git a/src/vidtime/ao_pins.h b/src/vidtime/ao_pins.h new file mode 100644 index 00000000..fe9d3879 --- /dev/null +++ b/src/vidtime/ao_pins.h @@ -0,0 +1,56 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +#define HAS_TASK_QUEUE 1 + +#define IS_FLASH_LOADER 0 + +#define AO_POWER_MANAGEMENT 0 + +/* 48MHz clock based on USB */ +#define AO_HSI48 1 + +/* HCLK = 48MHz */ +#define AO_AHB_PRESCALER 1 +#define AO_RCC_CFGR_HPRE_DIV STM_RCC_CFGR_HPRE_DIV_1 + +/* APB = 48MHz */ +#define AO_APB_PRESCALER 1 +#define AO_RCC_CFGR_PPRE_DIV STM_RCC_CFGR_PPRE_DIV_1 + +#define HAS_EEPROM 0 +#define USE_INTERNAL_FLASH 0 +#define USE_STORAGE_CONFIG 0 +#define USE_EEPROM_CONFIG 0 + +#define HAS_BEEP 0 + +#define HAS_USB 1 +#define AO_USB_DIRECTIO 0 +#define AO_PA11_PA12_RMP 1 + +#define HAS_SPI_0 0 + +#define AO_SENSOR_PORT (&stm_gpioa) +#define AO_SENSOR_PIN 3 + +#define LOW_LEVEL_DEBUG 0 + +#endif /* _AO_PINS_H_ */ diff --git a/src/vidtime/ao_vidtime.c b/src/vidtime/ao_vidtime.c new file mode 100644 index 00000000..e7f2c219 --- /dev/null +++ b/src/vidtime/ao_vidtime.c @@ -0,0 +1,105 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include + +static uint8_t sensor_value; +static uint8_t vidtime_monitor; + +static void +vidtime(void) +{ + ao_exti_enable(AO_SENSOR_PORT, AO_SENSOR_PIN); + for (;;) { + while (!vidtime_monitor) + ao_sleep(&vidtime_monitor); + ao_sleep(&sensor_value); + printf("%d\n", sensor_value); + flush(); + } +} + +static void +sensor_interrupt(void) +{ + uint8_t new = ao_gpio_get(AO_SENSOR_PORT, AO_SENSOR_PIN, foo); + +#if 0 + if (new) + ao_exti_set_mode(AO_SENSOR_PORT, AO_SENSOR_PIN, + AO_EXTI_MODE_FALLING); + else + ao_exti_set_mode(AO_SENSOR_PORT, AO_SENSOR_PIN, + AO_EXTI_MODE_RISING); +#endif + if (new != sensor_value) { + sensor_value = new; + ao_wakeup(&sensor_value); + } +} + +static struct ao_task vidtime_task; + +static void +ao_init_vidtime(void) +{ + ao_enable_port(AO_SENSOR_PORT); + ao_exti_setup(AO_SENSOR_PORT, AO_SENSOR_PIN, + AO_EXTI_MODE_RISING| + AO_EXTI_MODE_FALLING| + AO_EXTI_MODE_PULL_NONE| + AO_EXTI_PRIORITY_MED, + sensor_interrupt); + ao_add_task(&vidtime_task, vidtime, "vidtime"); +} + +static void +ao_set_vidtime(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status == ao_cmd_success) { + vidtime_monitor = ao_cmd_lex_i != 0; + ao_wakeup(&vidtime_monitor); + } +} + +const struct ao_cmds ao_vidtime_cmds[] = { + { ao_set_vidtime, "V <0 off, 1 on>\0Enable/disable timing monitor" }, + { 0, NULL } +}; + +void main(void) +{ + ao_clock_init(); + + ao_task_init(); + + ao_timer_init(); + + ao_dma_init(); + + ao_init_vidtime(); + + ao_usb_init(); + + ao_cmd_init(); + + ao_cmd_register(&ao_vidtime_cmds[0]); + + ao_start_scheduler(); +} diff --git a/src/vidtime/flash-loader/.gitignore b/src/vidtime/flash-loader/.gitignore new file mode 100644 index 00000000..05f1a4dc --- /dev/null +++ b/src/vidtime/flash-loader/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +vidtime* diff --git a/src/vidtime/flash-loader/Makefile b/src/vidtime/flash-loader/Makefile new file mode 100644 index 00000000..57c65b5e --- /dev/null +++ b/src/vidtime/flash-loader/Makefile @@ -0,0 +1,7 @@ +# +# AltOS flash loader build +# + +TOPDIR=../.. +HARDWARE=vidtime-v1.0 +include $(TOPDIR)/stmf0/Makefile-flash.defs diff --git a/src/vidtime/flash-loader/ao_pins.h b/src/vidtime/flash-loader/ao_pins.h new file mode 100644 index 00000000..7feb9362 --- /dev/null +++ b/src/vidtime/flash-loader/ao_pins.h @@ -0,0 +1,34 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; version 2 of the License. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +#include + +/* Debug port PA15 (pin 23) */ + +#define AO_BOOT_PIN 1 +#define AO_BOOT_APPLICATION_GPIO (stm_gpioa) +#define AO_BOOT_APPLICATION_PIN 15 +#define AO_BOOT_APPLICATION_VALUE 1 +#define AO_BOOT_APPLICATION_MODE AO_EXTI_MODE_PULL_UP + +#define AO_USB_DIRECTIO 0 +#define AO_PA11_PA12_RMP 1 + +#endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From 5a26df7db9453bf0596f729a23efb90e5e8a63c7 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 16 May 2018 22:05:26 -0700 Subject: altos/vidtime: Check for value change in normal code, not irq This makes sure each value change is reported to the user. Signed-off-by: Keith Packard --- src/vidtime/ao_vidtime.c | 24 ++++++++---------------- 1 file changed, 8 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/vidtime/ao_vidtime.c b/src/vidtime/ao_vidtime.c index e7f2c219..1b9b9e1c 100644 --- a/src/vidtime/ao_vidtime.c +++ b/src/vidtime/ao_vidtime.c @@ -24,33 +24,25 @@ static uint8_t vidtime_monitor; static void vidtime(void) { + uint8_t old = 0, got; + ao_exti_enable(AO_SENSOR_PORT, AO_SENSOR_PIN); for (;;) { while (!vidtime_monitor) ao_sleep(&vidtime_monitor); - ao_sleep(&sensor_value); - printf("%d\n", sensor_value); + while ((got = sensor_value) == old) + ao_sleep(&sensor_value); + printf("%d\n", got); flush(); + old = got; } } static void sensor_interrupt(void) { - uint8_t new = ao_gpio_get(AO_SENSOR_PORT, AO_SENSOR_PIN, foo); - -#if 0 - if (new) - ao_exti_set_mode(AO_SENSOR_PORT, AO_SENSOR_PIN, - AO_EXTI_MODE_FALLING); - else - ao_exti_set_mode(AO_SENSOR_PORT, AO_SENSOR_PIN, - AO_EXTI_MODE_RISING); -#endif - if (new != sensor_value) { - sensor_value = new; - ao_wakeup(&sensor_value); - } + sensor_value = ao_gpio_get(AO_SENSOR_PORT, AO_SENSOR_PIN, foo); + ao_wakeup(&sensor_value); } static struct ao_task vidtime_task; -- cgit v1.2.3 From 96ee5257068a988db10097af8df72b0008bce978 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 16 May 2018 22:02:11 -0700 Subject: altos: Add TeleLCO v2.0 product New LCO design with a single rotary knob and lots more buttons. Signed-off-by: Keith Packard --- src/telelco-v2.0/.gitignore | 2 + src/telelco-v2.0/Makefile | 103 +++++ src/telelco-v2.0/ao_lco_v2.c | 713 +++++++++++++++++++++++++++++++ src/telelco-v2.0/ao_pins.h | 358 ++++++++++++++++ src/telelco-v2.0/ao_telelco.c | 71 +++ src/telelco-v2.0/flash-loader/.gitignore | 2 + src/telelco-v2.0/flash-loader/Makefile | 8 + src/telelco-v2.0/flash-loader/ao_pins.h | 35 ++ 8 files changed, 1292 insertions(+) create mode 100644 src/telelco-v2.0/.gitignore create mode 100644 src/telelco-v2.0/Makefile create mode 100644 src/telelco-v2.0/ao_lco_v2.c create mode 100644 src/telelco-v2.0/ao_pins.h create mode 100644 src/telelco-v2.0/ao_telelco.c create mode 100644 src/telelco-v2.0/flash-loader/.gitignore create mode 100644 src/telelco-v2.0/flash-loader/Makefile create mode 100644 src/telelco-v2.0/flash-loader/ao_pins.h (limited to 'src') diff --git a/src/telelco-v2.0/.gitignore b/src/telelco-v2.0/.gitignore new file mode 100644 index 00000000..a32ec26e --- /dev/null +++ b/src/telelco-v2.0/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +telelco*.elf diff --git a/src/telelco-v2.0/Makefile b/src/telelco-v2.0/Makefile new file mode 100644 index 00000000..75c66abf --- /dev/null +++ b/src/telelco-v2.0/Makefile @@ -0,0 +1,103 @@ +# +# AltOS build for TeleLCO v2.0 +# +# + +include ../stm/Makefile.defs + +INC = \ + ao.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_boot.h \ + ao_companion.h \ + ao_data.h \ + ao_sample.h \ + ao_pins.h \ + ao_product.h \ + ao_seven_segment.h \ + ao_lco.h \ + ao_lco_cmd.h \ + ao_lco_func.h \ + ao_radio_spi.h \ + ao_radio_cmac.h \ + ao_cc1200_CC1200.h \ + ao_cc1200.h \ + stm32l.h + +# +# Common AltOS sources +# + +#PROFILE=ao_profile.c +#PROFILE_DEF=-DAO_PROFILE=1 + +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_freq.c \ + ao_dma_stm.c \ + ao_spi_stm.c \ + ao_beep_stm.c \ + ao_eeprom_stm.c \ + ao_fast_timer.c \ + ao_lcd_stm.c \ + ao_usb_stm.c \ + ao_exti_stm.c \ + ao_cc1200.c \ + ao_radio_cmac.c \ + ao_aes.c \ + ao_aes_tables.c \ + ao_fec_tx.c \ + ao_fec_rx.c \ + ao_seven_segment.c \ + ao_quadrature.c \ + ao_button.c \ + ao_event.c \ + ao_lco_v2.c \ + ao_lco_cmd.c \ + ao_lco_func.c \ + ao_radio_cmac_cmd.c + +PRODUCT=TeleLCO-v2.0 +PRODUCT_DEF=-DTELELCO +IDPRODUCT=0x0023 + +CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) -Os -g + +PROGNAME=telelco-v2.0 +PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx + +SRC=$(ALTOS_SRC) ao_telelco.c +OBJ=$(SRC:.c=.o) + +all: $(PROG) $(HEX) + +$(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 $(PROGNAME)-*.elf $(PROGNAME)-*.ihx + rm -f ao_product.h + +install: + +uninstall: diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c new file mode 100644 index 00000000..6f2b618a --- /dev/null +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -0,0 +1,713 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include +#include +#include +#include +#include + +#define DEBUG 1 + +#if DEBUG +static uint8_t ao_lco_debug; +#define PRINTD(...) do { if (!ao_lco_debug) break; printf ("\r%5u %s: ", ao_tick_count, __func__); printf(__VA_ARGS__); flush(); } while(0) +#else +#define PRINTD(...) +#endif + +#define AO_LCO_PAD_DIGIT 0 +#define AO_LCO_BOX_DIGIT_1 1 +#define AO_LCO_BOX_DIGIT_10 2 + +#define AO_LCO_DRAG_RACE_START_TIME AO_SEC_TO_TICKS(5) +#define AO_LCO_DRAG_RACE_STOP_TIME AO_SEC_TO_TICKS(2) + +#define AO_LCO_VALID_LAST 1 +#define AO_LCO_VALID_EVER 2 + +static uint8_t ao_lco_min_box, ao_lco_max_box; +static uint8_t ao_lco_selected[AO_PAD_MAX_BOXES]; +static uint8_t ao_lco_valid[AO_PAD_MAX_BOXES]; +static uint8_t ao_lco_channels[AO_PAD_MAX_BOXES]; +static uint16_t ao_lco_tick_offset[AO_PAD_MAX_BOXES]; + +/* UI values */ +static uint8_t ao_lco_armed; +static uint8_t ao_lco_firing; +static uint16_t ao_lco_fire_tick; +static uint8_t ao_lco_fire_down; +static uint8_t ao_lco_drag_race; +static uint8_t ao_lco_pad; +static int16_t ao_lco_box; +static uint8_t ao_lco_select_mode; +#define AO_LCO_SELECT_PAD 0 +#define AO_LCO_SELECT_BOX 1 +#define AO_LCO_SELECT_NONE 2 + +static struct ao_pad_query ao_pad_query; + +static uint8_t ao_lco_display_mutex; + +static void +ao_lco_set_pad(uint8_t pad) +{ + ao_mutex_get(&ao_lco_display_mutex); + ao_seven_segment_set(AO_LCO_PAD_DIGIT, pad | (ao_lco_drag_race << 4)); + ao_mutex_put(&ao_lco_display_mutex); +} + +#define SEVEN_SEGMENT_d ((0 << 0) | \ + (0 << 1) | \ + (1 << 2) | \ + (1 << 3) | \ + (1 << 4) | \ + (1 << 5) | \ + (1 << 6)) + + +#define SEVEN_SEGMENT_r ((0 << 0) | \ + (0 << 1) | \ + (0 << 2) | \ + (1 << 3) | \ + (1 << 4) | \ + (0 << 5) | \ + (0 << 6)) + +static void +ao_lco_set_box(uint16_t box) +{ + ao_mutex_get(&ao_lco_display_mutex); + ao_seven_segment_set(AO_LCO_BOX_DIGIT_1, box % 10 | (ao_lco_drag_race << 4)); + ao_seven_segment_set(AO_LCO_BOX_DIGIT_10, box / 10 | (ao_lco_drag_race << 4)); + ao_mutex_put(&ao_lco_display_mutex); +} + +static void +ao_lco_set_voltage(uint16_t decivolts) +{ + uint8_t tens, ones, tenths; + + tenths = decivolts % 10; + ones = (decivolts / 10) % 10; + tens = (decivolts / 100) % 10; + ao_mutex_get(&ao_lco_display_mutex); + ao_seven_segment_set(AO_LCO_PAD_DIGIT, tenths); + ao_seven_segment_set(AO_LCO_BOX_DIGIT_1, ones | 0x10); + ao_seven_segment_set(AO_LCO_BOX_DIGIT_10, tens); + ao_mutex_put(&ao_lco_display_mutex); +} + +static void +ao_lco_set_display(void) +{ + if (ao_lco_pad == 0) { + ao_lco_set_voltage(ao_pad_query.battery); + } else { + ao_lco_set_pad(ao_lco_pad); + ao_lco_set_box(ao_lco_box); + } +} + +#define MASK_SIZE(n) (((n) + 7) >> 3) +#define MASK_ID(n) ((n) >> 3) +#define MASK_SHIFT(n) ((n) & 7) + +static uint8_t ao_lco_box_mask[MASK_SIZE(AO_PAD_MAX_BOXES)]; + +static uint8_t +ao_lco_box_present(uint16_t box) +{ + if (box >= AO_PAD_MAX_BOXES) + return 0; + return (ao_lco_box_mask[MASK_ID(box)] >> MASK_SHIFT(box)) & 1; +} + +static uint8_t +ao_lco_pad_present(uint8_t box, uint8_t pad) +{ + /* voltage measurement is always valid */ + if (pad == 0) + return 1; + if (!ao_lco_channels[box]) + return 0; + if (pad > AO_PAD_MAX_CHANNELS) + return 0; + return (ao_lco_channels[box] >> (pad - 1)) & 1; +} + +static uint8_t +ao_lco_pad_first(uint8_t box) +{ + uint8_t pad; + + for (pad = 1; pad <= AO_PAD_MAX_CHANNELS; pad++) + if (ao_lco_pad_present(box, pad)) + return pad; + return 0; +} + +static void +ao_lco_set_select(void) +{ + switch (ao_lco_select_mode) { + case AO_LCO_SELECT_PAD: + ao_led_off(AO_LED_BOX); + ao_led_on(AO_LED_PAD); + break; + case AO_LCO_SELECT_BOX: + ao_led_off(AO_LED_PAD); + ao_led_on(AO_LED_BOX); + break; + default: + ao_led_off(AO_LED_PAD); + ao_led_off(AO_LED_BOX); + break; + } +} + +static struct ao_task ao_lco_drag_task; +static uint8_t ao_lco_drag_active; +static uint8_t ao_lco_drag_beep_count; +static uint8_t ao_lco_drag_beep_on; +static uint16_t ao_lco_drag_beep_time; +static uint16_t ao_lco_drag_warn_time; + +#define AO_LCO_DRAG_BEEP_TIME AO_MS_TO_TICKS(50) +#define AO_LCO_DRAG_WARN_TIME AO_SEC_TO_TICKS(5) + +static void +ao_lco_drag_beep_start(void) +{ + ao_beep(AO_BEEP_HIGH); + PRINTD("beep start\n"); + ao_lco_drag_beep_on = 1; + ao_lco_drag_beep_time = ao_time() + AO_LCO_DRAG_BEEP_TIME; +} + +static void +ao_lco_drag_beep_stop(void) +{ + ao_beep(0); + PRINTD("beep stop\n"); + ao_lco_drag_beep_on = 0; + if (ao_lco_drag_beep_count) { + --ao_lco_drag_beep_count; + if (ao_lco_drag_beep_count) + ao_lco_drag_beep_time = ao_time() + AO_LCO_DRAG_BEEP_TIME; + } +} + +static void +ao_lco_drag_beep(uint8_t beeps) +{ + PRINTD("beep %d\n", beeps); + if (!ao_lco_drag_beep_count) + ao_lco_drag_beep_start(); + ao_lco_drag_beep_count += beeps; +} + +static uint16_t +ao_lco_drag_beep_check(uint16_t now, uint16_t delay) +{ + PRINTD("beep check count %d delta %d\n", + ao_lco_drag_beep_count, + (int16_t) (now - ao_lco_drag_beep_time)); + if (ao_lco_drag_beep_count) { + if ((int16_t) (now - ao_lco_drag_beep_time) >= 0) { + if (ao_lco_drag_beep_on) + ao_lco_drag_beep_stop(); + else + ao_lco_drag_beep_start(); + } + } + + if (ao_lco_drag_beep_count) { + if (delay > AO_LCO_DRAG_BEEP_TIME) + delay = AO_LCO_DRAG_BEEP_TIME; + } + return delay; +} + +static void +ao_lco_drag_enable(void) +{ + PRINTD("Drag enable\n"); + ao_lco_drag_race = 1; + memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); + ao_lco_drag_beep(5); + ao_lco_set_display(); + ao_lco_fire_down = 0; +} + +static void +ao_lco_drag_disable(void) +{ + PRINTD("Drag disable\n"); + ao_lco_drag_race = 0; + memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); + ao_lco_drag_beep(2); + ao_lco_set_display(); + ao_lco_fire_down = 0; +} + +static uint16_t +ao_lco_drag_button_check(uint16_t now, uint16_t delay) +{ + uint16_t button_delay = ~0; + + /* + * Check to see if the button has been held down long enough + * to switch in/out of drag race mode + */ + if (ao_lco_fire_down) { + if (ao_lco_drag_race) { + if ((int16_t) (now - ao_lco_fire_tick) >= AO_LCO_DRAG_RACE_STOP_TIME) + ao_lco_drag_disable(); + else + button_delay = ao_lco_fire_tick + AO_LCO_DRAG_RACE_STOP_TIME - now; + } else { + if ((int16_t) (now - ao_lco_fire_tick) >= AO_LCO_DRAG_RACE_START_TIME) + ao_lco_drag_enable(); + else + button_delay = ao_lco_fire_tick + AO_LCO_DRAG_RACE_START_TIME - now; + } + if (delay > button_delay) + delay = button_delay; + } + return delay; +} + +static uint16_t +ao_lco_drag_warn_check(uint16_t now, uint16_t delay) +{ + uint16_t warn_delay = ~0; + + if (ao_lco_drag_race) { + if ((int16_t) (now - ao_lco_drag_warn_time) >= 0) { + ao_lco_drag_beep(1); + ao_lco_drag_warn_time = now + AO_LCO_DRAG_WARN_TIME; + } + warn_delay = ao_lco_drag_warn_time - now; + } + if (delay > warn_delay) + delay = warn_delay; + return delay; +} + +static void +ao_lco_drag_monitor(void) +{ + uint16_t delay = ~0; + uint16_t now; + + for (;;) { + PRINTD("Drag monitor active %d delay %d\n", ao_lco_drag_active, delay); + if (delay == (uint16_t) ~0) + ao_sleep(&ao_lco_drag_active); + else + ao_sleep_for(&ao_lco_drag_active, delay); + + delay = ~0; + if (!ao_lco_drag_active) + continue; + + now = ao_time(); + delay = ao_lco_drag_button_check(now, delay); + delay = ao_lco_drag_warn_check(now, delay); + delay = ao_lco_drag_beep_check(now, delay); + + /* check to see if there's anything left to do here */ + if (!ao_lco_fire_down && !ao_lco_drag_race && !ao_lco_drag_beep_count) { + delay = ~0; + ao_lco_drag_active = 0; + } + } +} + +static void +ao_lco_input(void) +{ + static struct ao_event event; + int8_t dir, new_pad; + int16_t new_box; + + ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200)); + for (;;) { + ao_event_get(&event); + PRINTD("event type %d unit %d value %d\n", + event.type, event.unit, event.value); + switch (event.type) { + case AO_EVENT_QUADRATURE: + switch (event.unit) { + case AO_QUADRATURE_SELECT: + if (!ao_lco_armed) { + switch (ao_lco_select_mode) { + case AO_LCO_SELECT_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; + if (new_pad < 0) + new_pad = AO_PAD_MAX_CHANNELS; + if (new_pad == ao_lco_pad) + break; + } while (!ao_lco_pad_present(ao_lco_box, new_pad)); + if (new_pad != ao_lco_pad) { + ao_lco_pad = new_pad; + ao_lco_set_display(); + } + break; + case AO_LCO_SELECT_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_max_box; + if (new_box == ao_lco_box) + break; + } while (!ao_lco_box_present(new_box)); + if (ao_lco_box != new_box) { + ao_lco_box = new_box; + ao_lco_pad = 1; + ao_lco_channels[ao_lco_box] = 0; + ao_lco_set_display(); + } + break; + default: + break; + } + } + break; + } + break; + case AO_EVENT_BUTTON: + switch (event.unit) { + case AO_BUTTON_ARM: + ao_lco_armed = event.value; + PRINTD("Armed %d\n", ao_lco_armed); + if (ao_lco_armed) { + if (ao_lco_drag_race) { + uint8_t box; + + for (box = ao_lco_min_box; box <= ao_lco_max_box; box++) { + if (ao_lco_selected[box]) { + ao_wakeup(&ao_lco_armed); + break; + } + } + } else { + memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); + if (ao_lco_pad != 0) + ao_lco_selected[ao_lco_box] = (1 << (ao_lco_pad - 1)); + else + ao_lco_armed = 0; + } + } + ao_wakeup(&ao_lco_armed); + break; + case AO_BUTTON_FIRE: + if (ao_lco_armed) { + ao_lco_fire_down = 0; + ao_lco_firing = event.value; + PRINTD("Firing %d\n", ao_lco_firing); + ao_wakeup(&ao_lco_armed); + } + break; + case AO_BUTTON_DRAG_SELECT: + if (ao_lco_drag_race) { + if (ao_lco_pad != 0) { + ao_lco_selected[ao_lco_box] ^= (1 << (ao_lco_pad - 1)); + PRINTD("Toggle box %d pad %d (pads now %x) to drag race\n", + ao_lco_pad, ao_lco_box, ao_lco_selected[ao_lco_box]); + ao_lco_drag_beep(ao_lco_pad); + } + } + break; + case AO_BUTTON_MODE_SELECT: + if (event.value) + ao_lco_drag_enable(); + else + ao_lco_drag_disable(); + break; + case AO_BUTTON_SELECT: + ao_lco_select_mode++; + if (ao_lco_select_mode > AO_LCO_SELECT_NONE) + ao_lco_select_mode = AO_LCO_SELECT_PAD; + ao_lco_set_select(); + break; + } + break; + } + } +} + +static AO_LED_TYPE continuity_led[AO_LED_CONTINUITY_NUM] = { +#ifdef AO_LED_CONTINUITY_0 + AO_LED_CONTINUITY_0, +#endif +#ifdef AO_LED_CONTINUITY_1 + AO_LED_CONTINUITY_1, +#endif +#ifdef AO_LED_CONTINUITY_2 + AO_LED_CONTINUITY_2, +#endif +#ifdef AO_LED_CONTINUITY_3 + AO_LED_CONTINUITY_3, +#endif +#ifdef AO_LED_CONTINUITY_4 + AO_LED_CONTINUITY_4, +#endif +#ifdef AO_LED_CONTINUITY_5 + AO_LED_CONTINUITY_5, +#endif +#ifdef AO_LED_CONTINUITY_6 + AO_LED_CONTINUITY_6, +#endif +#ifdef AO_LED_CONTINUITY_7 + AO_LED_CONTINUITY_7, +#endif +}; + +static uint8_t +ao_lco_get_channels(uint8_t box, struct ao_pad_query *query) +{ + int8_t r; + + r = ao_lco_query(box, query, &ao_lco_tick_offset[box]); + if (r == AO_RADIO_CMAC_OK) { + ao_lco_channels[box] = query->channels; + ao_lco_valid[box] = AO_LCO_VALID_LAST | AO_LCO_VALID_EVER; + } else + ao_lco_valid[box] &= ~AO_LCO_VALID_LAST; + PRINTD("ao_lco_get_channels(%d) rssi %d valid %d ret %d offset %d\n", box, ao_radio_cmac_rssi, ao_lco_valid[box], r, ao_lco_tick_offset[box]); + ao_wakeup(&ao_pad_query); + return ao_lco_valid[box]; +} + +static void +ao_lco_update(void) +{ + uint8_t previous_valid = ao_lco_valid[ao_lco_box]; + + if (ao_lco_get_channels(ao_lco_box, &ao_pad_query) & AO_LCO_VALID_LAST) { + if (!(previous_valid & AO_LCO_VALID_EVER)) { + if (ao_lco_pad != 0) + ao_lco_pad = ao_lco_pad_first(ao_lco_box); + ao_lco_set_display(); + } + if (ao_lco_pad == 0) + ao_lco_set_display(); + } +} + +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); +} + +static void +ao_lco_search(void) +{ + int8_t r; + int8_t try; + uint8_t box; + uint8_t boxes = 0; + + ao_lco_box_reset_present(); + ao_lco_set_pad(0); + for (box = 0; box < AO_PAD_MAX_BOXES; box++) { + if ((box % 10) == 0) + ao_lco_set_box(box); + for (try = 0; try < 3; try++) { + ao_lco_tick_offset[box] = 0; + r = ao_lco_query(box, &ao_pad_query, &ao_lco_tick_offset[box]); + PRINTD("box %d result %d offset %d\n", box, r, ao_lco_tick_offset[box]); + if (r == AO_RADIO_CMAC_OK) { + ++boxes; + ao_lco_box_set_present(box); + ao_lco_set_pad(boxes % 10); + ao_delay(AO_MS_TO_TICKS(30)); + break; + } + } + } + if (ao_lco_min_box <= ao_lco_max_box) + ao_lco_box = ao_lco_min_box; + else + ao_lco_min_box = ao_lco_max_box = ao_lco_box = 0; + memset(ao_lco_valid, 0, sizeof (ao_lco_valid)); + memset(ao_lco_channels, 0, sizeof (ao_lco_channels)); + ao_lco_pad = 1; + ao_lco_set_display(); +} + +static void +ao_lco_igniter_status(void) +{ + uint8_t c; + uint8_t t = 0; + + for (;;) { + ao_sleep(&ao_pad_query); + PRINTD("RSSI %d VALID %d\n", ao_radio_cmac_rssi, ao_lco_valid[ao_lco_box]); + if (!(ao_lco_valid[ao_lco_box] & AO_LCO_VALID_LAST)) { + ao_led_on(AO_LED_RED); + ao_led_off(AO_LED_GREEN|AO_LED_AMBER); + continue; + } + if (ao_radio_cmac_rssi < -90) { + ao_led_on(AO_LED_AMBER); + ao_led_off(AO_LED_RED|AO_LED_GREEN); + } else { + ao_led_on(AO_LED_GREEN); + ao_led_off(AO_LED_RED|AO_LED_AMBER); + } + if (ao_pad_query.arm_status) + ao_led_on(AO_LED_REMOTE_ARM); + else + ao_led_off(AO_LED_REMOTE_ARM); + + for (c = 0; c < AO_LED_CONTINUITY_NUM; c++) { + uint8_t status; + + if (ao_lco_drag_race) { + if (ao_lco_selected[ao_lco_box] & (1 << c) && t) + ao_led_on(continuity_led[c]); + else + ao_led_off(continuity_led[c]); + } else { + if (ao_pad_query.channels & (1 << c)) + status = ao_pad_query.igniter_status[c]; + else + status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN; + if (status == AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN) + ao_led_on(continuity_led[c]); + else + ao_led_off(continuity_led[c]); + } + } + t = 1-t; + } +} + +static void +ao_lco_arm_warn(void) +{ + for (;;) { + while (!ao_lco_armed) { + ao_led_off(AO_LED_FIRE); + ao_sleep(&ao_lco_armed); + } + ao_led_on(AO_LED_FIRE); + ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200)); + ao_delay(AO_MS_TO_TICKS(200)); + } +} + +static struct ao_task ao_lco_input_task; +static struct ao_task ao_lco_monitor_task; +static struct ao_task ao_lco_arm_warn_task; +static struct ao_task ao_lco_igniter_status_task; + +static void +ao_lco_monitor(void) +{ + uint16_t delay; + uint8_t box; + + ao_lco_search(); + ao_add_task(&ao_lco_input_task, ao_lco_input, "lco input"); + ao_add_task(&ao_lco_arm_warn_task, ao_lco_arm_warn, "lco arm warn"); + ao_add_task(&ao_lco_igniter_status_task, ao_lco_igniter_status, "lco igniter status"); + ao_add_task(&ao_lco_drag_task, ao_lco_drag_monitor, "drag race"); + for (;;) { + PRINTD("monitor armed %d firing %d\n", + ao_lco_armed, ao_lco_firing); + + if (ao_lco_armed && ao_lco_firing) { + ao_lco_ignite(AO_PAD_FIRE); + } else { + ao_lco_update(); + if (ao_lco_armed) { + for (box = ao_lco_min_box; box <= ao_lco_max_box; box++) { + if (ao_lco_selected[box]) { + PRINTD("Arming box %d pads %x\n", + box, ao_lco_selected[box]); + if (ao_lco_valid[box] & AO_LCO_VALID_EVER) { + ao_lco_arm(box, ao_lco_selected[box], ao_lco_tick_offset[box]); + ao_delay(AO_MS_TO_TICKS(10)); + } + } + } + } + } + if (ao_lco_armed && ao_lco_firing) + delay = AO_MS_TO_TICKS(100); + else + delay = AO_SEC_TO_TICKS(1); + ao_sleep_for(&ao_lco_armed, delay); + } +} + +#if DEBUG +void +ao_lco_set_debug(void) +{ + ao_cmd_decimal(); + if (ao_cmd_status == ao_cmd_success) + ao_lco_debug = ao_cmd_lex_i != 0; +} + +__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 + +void +ao_lco_init(void) +{ + ao_add_task(&ao_lco_monitor_task, ao_lco_monitor, "lco monitor"); +#if DEBUG + ao_cmd_register(&ao_lco_cmds[0]); +#endif +} diff --git a/src/telelco-v2.0/ao_pins.h b/src/telelco-v2.0/ao_pins.h new file mode 100644 index 00000000..d8cf4dec --- /dev/null +++ b/src/telelco-v2.0/ao_pins.h @@ -0,0 +1,358 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +/* 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) + +#define AO_CC1200_FOSC 40000000 + +/* 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_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 +#define HAS_RADIO_RATE 1 +#define HAS_TELEMETRY 0 +#define HAS_AES 1 + +#define HAS_SPI_1 0 +#define SPI_1_PA5_PA6_PA7 0 +#define SPI_1_PB3_PB4_PB5 0 +#define SPI_1_PE13_PE14_PE15 0 + +#define HAS_SPI_2 1 /* CC1200 */ +#define SPI_2_PB13_PB14_PB15 0 +#define SPI_2_PD1_PD3_PD4 1 +#define SPI_2_GPIO (&stm_gpiod) +#define SPI_2_SCK 1 +#define SPI_2_MISO 3 +#define SPI_2_MOSI 4 +#define SPI_2_OSPEEDR STM_OSPEEDR_10MHz + +#define HAS_I2C_1 0 + +#define HAS_I2C_2 0 + +#define PACKET_HAS_SLAVE 0 +#define PACKET_HAS_MASTER 0 + +#define FAST_TIMER_FREQ 10000 /* .1ms for debouncing */ + +/* + * Radio is a cc1200 connected via SPI + */ + +#define AO_RADIO_CAL_DEFAULT 5695733 + +#define AO_CC1200_SPI_CS_PORT (&stm_gpiod) +#define AO_CC1200_SPI_CS_PIN 0 +#define AO_CC1200_SPI_BUS AO_SPI_2_PD1_PD3_PD4 +#define AO_CC1200_SPI stm_spi2 +#define AO_CC1200_SPI_SPEED AO_SPI_SPEED_FAST + +#define AO_CC1200_INT_PORT (&stm_gpiod) +#define AO_CC1200_INT_PIN (5) + +#define AO_CC1200_INT_GPIO 2 +#define AO_CC1200_INT_GPIO_IOCFG CC1200_IOCFG2 + +#define LOW_LEVEL_DEBUG 0 + +#define LED_PER_LED 1 +#define LED_TYPE uint16_t + +#define LED_ENABLE_BITS ((1 << STM_RCC_AHBENR_GPIOAEN) | \ + (1 << STM_RCC_AHBENR_GPIOCEN) | \ + (1 << STM_RCC_AHBENR_GPIODEN) | \ + (1 << STM_RCC_AHBENR_GPIOEEN)) + +/* PC7 - PC9, LED 0 - 2 */ +#define LED_0_PORT (&stm_gpioc) +#define LED_0_PIN 7 +#define LED_1_PORT (&stm_gpioc) +#define LED_1_PIN 8 +#define LED_2_PORT (&stm_gpioc) +#define LED_2_PIN 9 + +/* PD8 - PD10, LED 3 - 5 */ +#define LED_3_PORT (&stm_gpiod) +#define LED_3_PIN 8 +#define LED_4_PORT (&stm_gpiod) +#define LED_4_PIN 9 +#define LED_5_PORT (&stm_gpiod) +#define LED_5_PIN 10 + +/* PE2 - PE11 (not PE6), LED 6-14 */ +#define LED_6_PORT (&stm_gpioe) +#define LED_6_PIN 2 +#define LED_7_PORT (&stm_gpioe) +#define LED_7_PIN 3 +#define LED_8_PORT (&stm_gpioe) +#define LED_8_PIN 4 +#define LED_9_PORT (&stm_gpioe) +#define LED_9_PIN 5 +#define LED_10_PORT (&stm_gpioe) +#define LED_10_PIN 7 +#define LED_11_PORT (&stm_gpioe) +#define LED_11_PIN 8 +#define LED_12_PORT (&stm_gpioe) +#define LED_12_PIN 9 +#define LED_13_PORT (&stm_gpioe) +#define LED_13_PIN 10 +#define LED_14_PORT (&stm_gpioe) +#define LED_14_PIN 11 + +/* PA5, LED 15 */ +#define LED_15_PORT (&stm_gpioa) +#define LED_15_PIN 5 + +#define LED_PIN_RED 0 +#define LED_PIN_AMBER 1 +#define LED_PIN_GREEN 2 +#define LED_PIN_BOX 3 +#define LED_PIN_PAD 4 +#define LED_PIN_DRAG 5 +#define LED_PIN_CONTINUITY_7 6 +#define LED_PIN_CONTINUITY_6 7 +#define LED_PIN_CONTINUITY_5 8 +#define LED_PIN_CONTINUITY_4 9 +#define LED_PIN_CONTINUITY_3 10 +#define LED_PIN_CONTINUITY_2 11 +#define LED_PIN_CONTINUITY_1 12 +#define LED_PIN_CONTINUITY_0 13 +#define LED_PIN_REMOTE_ARM 14 +#define LED_PIN_FIRE 15 +#define AO_LED_RED (1 << LED_PIN_RED) +#define AO_LED_AMBER (1 << LED_PIN_AMBER) +#define AO_LED_GREEN (1 << LED_PIN_GREEN) +#define AO_LED_BOX (1 << LED_PIN_BOX) +#define AO_LED_PAD (1 << LED_PIN_PAD) +#define AO_LED_DRAG (1 << LED_PIN_DRAG) +#define AO_LED_CONTINUITY_7 (1 << LED_PIN_CONTINUITY_7) +#define AO_LED_CONTINUITY_6 (1 << LED_PIN_CONTINUITY_6) +#define AO_LED_CONTINUITY_5 (1 << LED_PIN_CONTINUITY_5) +#define AO_LED_CONTINUITY_4 (1 << LED_PIN_CONTINUITY_4) +#define AO_LED_CONTINUITY_3 (1 << LED_PIN_CONTINUITY_3) +#define AO_LED_CONTINUITY_2 (1 << LED_PIN_CONTINUITY_2) +#define AO_LED_CONTINUITY_1 (1 << LED_PIN_CONTINUITY_1) +#define AO_LED_CONTINUITY_0 (1 << LED_PIN_CONTINUITY_0) +#define AO_LED_CONTINUITY_NUM 8 +#define AO_LED_REMOTE_ARM (1 << LED_PIN_REMOTE_ARM) +#define AO_LED_FIRE (1 << LED_PIN_FIRE) + +#define LEDS_AVAILABLE (AO_LED_RED | \ + AO_LED_AMBER | \ + AO_LED_GREEN | \ + AO_LED_BOX | \ + AO_LED_PAD | \ + AO_LED_DRAG | \ + AO_LED_CONTINUITY_7 | \ + AO_LED_CONTINUITY_6 | \ + AO_LED_CONTINUITY_5 | \ + AO_LED_CONTINUITY_4 | \ + AO_LED_CONTINUITY_3 | \ + AO_LED_CONTINUITY_2 | \ + AO_LED_CONTINUITY_1 | \ + AO_LED_CONTINUITY_0 | \ + AO_LED_REMOTE_ARM | \ + AO_LED_FIRE) + +/* LCD displays */ + +#define LCD_DEBUG 0 +#define SEVEN_SEGMENT_DEBUG 0 + +#define AO_LCD_STM_SEG_ENABLED_0 ( \ + (1 << 0) | /* PA1 */ \ + (1 << 1) | /* PA2 */ \ + (1 << 2) | /* PA3 */ \ + (1 << 3) | /* PA6 */ \ + (1 << 4) | /* PA7 */ \ + (1 << 5) | /* PB0 */ \ + (1 << 6) | /* PB1 */ \ + (1 << 7) | /* PB3 */ \ + (1 << 8) | /* PB4 */ \ + (1 << 9) | /* PB5 */ \ + (1 << 10) | /* PB10 */ \ + (1 << 11) | /* PB11 */ \ + (1 << 12) | /* PB12 */ \ + (1 << 13) | /* PB13 */ \ + (1 << 14) | /* PB14 */ \ + (1 << 15) | /* PB15 */ \ + (1 << 16) | /* PB8 */ \ + (1 << 17) | /* PA15 */ \ + (1 << 18) | /* PC0 */ \ + (1 << 19) | /* PC1 */ \ + (1 << 20) | /* PC2 */ \ + (1 << 21) | /* PC3 */ \ + (1 << 22) | /* PC4 */ \ + (1 << 23) | /* PC5 */ \ + (0 << 24) | /* PC6 */ \ + (0 << 25) | /* PC7 */ \ + (0 << 26) | /* PC8 */ \ + (0 << 27) | /* PC9 */ \ + (0 << 28) | /* PC10 or PD8 */ \ + (0 << 29) | /* PC11 or PD9 */ \ + (0 << 30) | /* PC12 or PD10 */ \ + (0 << 31)) /* PD2 or PD11 */ + +#define AO_LCD_STM_SEG_ENABLED_1 ( \ + (0 << 0) | /* PD12 */ \ + (0 << 1) | /* PD13 */ \ + (0 << 2) | /* PD14 */ \ + (0 << 3) | /* PD15 */ \ + (0 << 4) | /* PE0 */ \ + (0 << 5) | /* PE1 */ \ + (0 << 6) | /* PE2 */ \ + (0 << 7)) /* PE3 */ + +#define AO_LCD_STM_COM_ENABLED ( \ + (1 << 0) | /* PA8 */ \ + (0 << 1) | /* PA9 */ \ + (0 << 2) | /* PA10 */ \ + (0 << 3) | /* PB9 */ \ + (0 << 4) | /* PC10 */ \ + (0 << 5) | /* PC11 */ \ + (0 << 6)) /* PC12 */ + +#define AO_LCD_28_ON_C 0 + +#define AO_LCD_DUTY STM_LCD_CR_DUTY_STATIC + +#define AO_LCD_PER_DIGIT 1 + +#define AO_LCD_DIGITS 3 +#define AO_LCD_SEGMENTS 8 + +#define AO_SEGMENT_MAP { \ + /* pad segments */ \ + { 0, 14 }, \ + { 0, 13 }, \ + { 0, 15 }, \ + { 0, 17 }, \ + { 0, 16 }, \ + { 0, 8 }, \ + { 0, 9 }, \ + { 0, 7 }, \ + /* box1 segments */ \ + { 0, 10 }, \ + { 0, 6 }, \ + { 0, 11 }, \ + { 0, 12 }, \ + { 0, 21 }, \ + { 0, 19 }, \ + { 0, 20 }, \ + { 0, 18 }, \ + /* box0 segments */ \ + { 0, 22 }, \ + { 0, 4 }, \ + { 0, 23 }, \ + { 0, 5 }, \ + { 0, 3 }, \ + { 0, 1 }, \ + { 0, 2 }, \ + { 0, 0 }, \ +} + +/* + * Use event queue for input devices + */ + +#define AO_EVENT 1 + +/* + * Knobs + */ + +#define AO_QUADRATURE_COUNT 1 + +#define AO_QUADRATURE_0_PORT &stm_gpioe +#define AO_QUADRATURE_0_A 15 +#define AO_QUADRATURE_0_B 14 + +#define AO_QUADRATURE_SELECT 0 + +/* + * Buttons + */ + +#define AO_BUTTON_COUNT 9 +#define AO_BUTTON_MODE AO_EXTI_MODE_PULL_UP + +#define AO_BUTTON_MODE_SELECT 0 +#define AO_BUTTON_0_PORT &stm_gpioe +#define AO_BUTTON_0 1 + +#define AO_BUTTON_DRAG_SELECT 1 +#define AO_BUTTON_1_PORT &stm_gpioe +#define AO_BUTTON_1 1 + +#define AO_BUTTON_SPARE1 2 +#define AO_BUTTON_2_PORT &stm_gpiob +#define AO_BUTTON_2 9 + +#define AO_BUTTON_SPARE2 3 +#define AO_BUTTON_3_PORT &stm_gpiob +#define AO_BUTTON_3 7 + +#define AO_BUTTON_SPARE3 4 +#define AO_BUTTON_4_PORT &stm_gpiob +#define AO_BUTTON_4 6 + +#define AO_BUTTON_ARM 5 +#define AO_BUTTON_5_PORT &stm_gpioe +#define AO_BUTTON_5 12 + +#define AO_BUTTON_FIRE 6 +#define AO_BUTTON_6_PORT &stm_gpioa +#define AO_BUTTON_6 4 + +#define AO_BUTTON_SPARE4 7 +#define AO_BUTTON_7_PORT &stm_gpiod +#define AO_BUTTON_7 11 + +#define AO_BUTTON_SELECT 8 +#define AO_BUTTON_8_PORT &stm_gpioe +#define AO_BUTTON_8 13 + +#endif /* _AO_PINS_H_ */ diff --git a/src/telelco-v2.0/ao_telelco.c b/src/telelco-v2.0/ao_telelco.c new file mode 100644 index 00000000..7b04d386 --- /dev/null +++ b/src/telelco-v2.0/ao_telelco.c @@ -0,0 +1,71 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +int +main(void) +{ + ao_clock_init(); + + ao_led_init(LEDS_AVAILABLE); + ao_led_on(AO_LED_GREEN); + ao_task_init(); + + ao_timer_init(); + + ao_spi_init(); + ao_dma_init(); + ao_exti_init(); + + ao_beep_init(); + ao_cmd_init(); + + ao_lcd_stm_init(); + ao_seven_segment_init(); + ao_quadrature_init(); + ao_button_init(); + + ao_eeprom_init(); + + ao_radio_init(); + + ao_usb_init(); + + ao_config_init(); + + ao_lco_init(); + ao_lco_cmd_init(); +// ao_radio_cmac_cmd_init(); + + ao_start_scheduler(); + return 0; +} diff --git a/src/telelco-v2.0/flash-loader/.gitignore b/src/telelco-v2.0/flash-loader/.gitignore new file mode 100644 index 00000000..a32ec26e --- /dev/null +++ b/src/telelco-v2.0/flash-loader/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +telelco*.elf diff --git a/src/telelco-v2.0/flash-loader/Makefile b/src/telelco-v2.0/flash-loader/Makefile new file mode 100644 index 00000000..b4ec2dad --- /dev/null +++ b/src/telelco-v2.0/flash-loader/Makefile @@ -0,0 +1,8 @@ +# +# AltOS flash loader build +# +# + +TOPDIR=../.. +HARDWARE=telelco-v2.0 +include $(TOPDIR)/stm/Makefile-flash.defs diff --git a/src/telelco-v2.0/flash-loader/ao_pins.h b/src/telelco-v2.0/flash-loader/ao_pins.h new file mode 100644 index 00000000..2292f03c --- /dev/null +++ b/src/telelco-v2.0/flash-loader/ao_pins.h @@ -0,0 +1,35 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +/* External crystal at 8MHz */ +#define AO_HSE 8000000 + +#include + +/* Drag race select button. Press at power on to get boot loader */ + +#define AO_BOOT_PIN 1 +#define AO_BOOT_APPLICATION_GPIO stm_gpioe +#define AO_BOOT_APPLICATION_PIN 0 +#define AO_BOOT_APPLICATION_VALUE 1 +#define AO_BOOT_APPLICATION_MODE AO_EXTI_MODE_PULL_UP + +#endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From 85a8846a423ce2d15815466564fb589bb553c742 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 16 May 2018 22:03:25 -0700 Subject: altos/stm: Add per-LED port/pin mode in LED code If you've got a complicated LED pin arrangement, you can ask for each one to be configured separately, instead of using groups in a couple of GPIO registers. The code isn't as efficient this way, but at least it's easy to read. Signed-off-by: Keith Packard --- src/stm/ao_arch.h | 4 ++ src/stm/ao_led.c | 110 +++++++++++++++++++++++++++++++++++++++++++++++++----- 2 files changed, 104 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/stm/ao_arch.h b/src/stm/ao_arch.h index 5f033b66..ecf1c1a7 100644 --- a/src/stm/ao_arch.h +++ b/src/stm/ao_arch.h @@ -153,6 +153,10 @@ ao_adc_init(); #define AO_BOOT_LOADER_BASE ((uint32_t *) 0x08000000) #define HAS_BOOT_LOADER 1 +#ifndef AO_LED_TYPE +#define AO_LED_TYPE uint16_t +#endif + #endif /* _AO_ARCH_H_ */ diff --git a/src/stm/ao_led.c b/src/stm/ao_led.c index 0f39befb..a7033dbb 100644 --- a/src/stm/ao_led.c +++ b/src/stm/ao_led.c @@ -18,11 +18,73 @@ #include "ao.h" -__pdata uint16_t ao_led_enable; +#if LED_PER_LED +static const struct { + struct stm_gpio *port; + uint16_t pin; +} ao_leds[] = { +#ifdef LED_0_PORT + [0] { LED_0_PORT, LED_0_PIN }, +#endif +#ifdef LED_1_PORT + [1] { LED_1_PORT, LED_1_PIN }, +#endif +#ifdef LED_2_PORT + [2] { LED_2_PORT, LED_2_PIN }, +#endif +#ifdef LED_3_PORT + [3] { LED_3_PORT, LED_3_PIN }, +#endif +#ifdef LED_4_PORT + [4] { LED_4_PORT, LED_4_PIN }, +#endif +#ifdef LED_5_PORT + [5] { LED_5_PORT, LED_5_PIN }, +#endif +#ifdef LED_6_PORT + [6] { LED_6_PORT, LED_6_PIN }, +#endif +#ifdef LED_7_PORT + [7] { LED_7_PORT, LED_7_PIN }, +#endif +#ifdef LED_8_PORT + [8] { LED_8_PORT, LED_8_PIN }, +#endif +#ifdef LED_9_PORT + [9] { LED_9_PORT, LED_9_PIN }, +#endif +#ifdef LED_10_PORT + [10] { LED_10_PORT, LED_10_PIN }, +#endif +#ifdef LED_11_PORT + [11] { LED_11_PORT, LED_11_PIN }, +#endif +#ifdef LED_12_PORT + [12] { LED_12_PORT, LED_12_PIN }, +#endif +#ifdef LED_13_PORT + [13] { LED_13_PORT, LED_13_PIN }, +#endif +#ifdef LED_14_PORT + [14] { LED_14_PORT, LED_14_PIN }, +#endif +#ifdef LED_15_PORT + [15] { LED_15_PORT, LED_15_PIN }, +#endif +}; +#define N_LED (sizeof (ao_leds)/sizeof(ao_leds[0])) +#endif +static AO_LED_TYPE ao_led_enable; void -ao_led_on(uint16_t colors) +ao_led_on(AO_LED_TYPE colors) { +#ifdef LED_PER_LED + AO_LED_TYPE i; + for (i = 0; i < N_LED; i++) + if (colors & (1 << i)) + ao_gpio_set(ao_leds[i].port, ao_leds[i].pin, foo, 1); +#else #ifdef LED_PORT LED_PORT->bsrr = (colors & ao_led_enable); #else @@ -33,11 +95,18 @@ ao_led_on(uint16_t colors) LED_PORT_1->bsrr = ((colors & ao_led_enable) & LED_PORT_1_MASK) << LED_PORT_1_SHIFT; #endif #endif +#endif } void -ao_led_off(uint16_t colors) +ao_led_off(AO_LED_TYPE colors) { +#ifdef LED_PER_LED + AO_LED_TYPE i; + for (i = 0; i < N_LED; i++) + if (colors & (1 << i)) + ao_gpio_set(ao_leds[i].port, ao_leds[i].pin, foo, 0); +#else #ifdef LED_PORT LED_PORT->bsrr = (uint32_t) (colors & ao_led_enable) << 16; #else @@ -48,21 +117,28 @@ ao_led_off(uint16_t colors) LED_PORT_1->bsrr = ((uint32_t) (colors & ao_led_enable) & LED_PORT_1_MASK) << (LED_PORT_1_SHIFT + 16); #endif #endif +#endif } void -ao_led_set(uint16_t colors) +ao_led_set(AO_LED_TYPE colors) { - uint16_t on = colors & ao_led_enable; - uint16_t off = ~colors & ao_led_enable; + AO_LED_TYPE on = colors & ao_led_enable; + AO_LED_TYPE off = ~colors & ao_led_enable; ao_led_off(off); ao_led_on(on); } void -ao_led_toggle(uint16_t colors) +ao_led_toggle(AO_LED_TYPE colors) { +#ifdef LED_PER_LED + AO_LED_TYPE i; + for (i = 0; i < N_LED; i++) + if (colors & (1 << i)) + ao_gpio_set(ao_leds[i].port, ao_leds[i].pin, foo, ~ao_gpio_get(ao_leds[i].port, ao_leds[i].pin, foo)); +#else #ifdef LED_PORT LED_PORT->odr ^= (colors & ao_led_enable); #else @@ -73,10 +149,11 @@ ao_led_toggle(uint16_t colors) LED_PORT_1->odr ^= ((colors & ao_led_enable) & LED_PORT_1_MASK) << LED_PORT_1_SHIFT; #endif #endif +#endif } void -ao_led_for(uint16_t colors, uint16_t ticks) __reentrant +ao_led_for(AO_LED_TYPE colors, AO_LED_TYPE ticks) __reentrant { ao_led_on(colors); ao_delay(ticks); @@ -89,11 +166,15 @@ ao_led_for(uint16_t colors, uint16_t ticks) __reentrant } while (0) void -ao_led_init(uint16_t enable) +ao_led_init(AO_LED_TYPE enable) { - int bit; + AO_LED_TYPE bit; ao_led_enable = enable; +#if LED_PER_LED + for (bit = 0; bit < N_LED; bit++) + ao_enable_output(ao_leds[bit].port, ao_leds[bit].pin, foo, 0); +#else #ifdef LED_PORT stm_rcc.ahbenr |= (1 << LED_PORT_ENABLE); LED_PORT->odr &= ~enable; @@ -106,6 +187,10 @@ ao_led_init(uint16_t enable) stm_rcc.ahbenr |= (1 << LED_PORT_1_ENABLE); LED_PORT_1->odr &= ~((enable & ao_led_enable) & LED_PORT_1_MASK) << LED_PORT_1_SHIFT; #endif +#ifdef LED_PORT_2 + stm_rcc.ahbenr |= (1 << LED_PORT_1_ENABLE); + LED_PORT_1->odr &= ~((enable & ao_led_enable) & LED_PORT_1_MASK) << LED_PORT_1_SHIFT; +#endif #endif for (bit = 0; bit < 16; bit++) { if (enable & (1 << bit)) { @@ -120,7 +205,12 @@ ao_led_init(uint16_t enable) if (LED_PORT_1_MASK & (1 << bit)) init_led_pin(LED_PORT_1, bit + LED_PORT_1_SHIFT); #endif +#ifdef LED_PORT_2 + if (LED_PORT_2_MASK & (1 << bit)) + init_led_pin(LED_PORT_2, bit + LED_PORT_2_SHIFT); +#endif #endif } } +#endif } -- cgit v1.2.3 From e272f6a66881b6904037ee7b1afeb9a8a3ec5b2a Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 21 May 2018 22:32:38 -0700 Subject: altos/drivers: Pads with multi-port firing and pad selector DIP switch TeleFire 8 scrambles GPIOs to make layout cleaner, placing the 8 firing outputs across two GPIO ports. We're also adding an 8-unit selector for box number so that boards can be re-configured in the field. These override any ROM-configuration value; you can select the rom value by setting all switches to 'off'. Signed-off-by: Keith Packard --- src/drivers/ao_pad.c | 120 +++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 106 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c index 16b4ae60..b0ec2161 100644 --- a/src/drivers/ao_pad.c +++ b/src/drivers/ao_pad.c @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -69,10 +70,20 @@ ao_strobe(uint8_t v) #endif } +#ifdef AO_PAD_PORT_0 +#define pins_pad(pad) (*((AO_PAD_ ## pad ## _PORT) == AO_PAD_PORT_0 ? (&pins0) : (&pins1))) +#else +#define pins_pad(pad) pins0 +#define AO_PAD_PORT_0 AO_PAD_PORT +#endif + static void ao_pad_run(void) { - AO_PORT_TYPE pins; + AO_PORT_TYPE pins0; +#ifdef AO_PAD_PORT_1 + AO_PORT_TYPE pins1; +#endif for (;;) { while (!ao_pad_ignite) @@ -80,32 +91,63 @@ ao_pad_run(void) /* * Actually set the pad bits */ - pins = 0; + pins0 = 0; +#ifdef AO_PAD_PORT_1 + pins1 = 0; +#endif #if AO_PAD_NUM > 0 if (ao_pad_ignite & (1 << 0)) - pins |= (1 << AO_PAD_PIN_0); + pins_pad(0) |= (1 << AO_PAD_PIN_0); #endif #if AO_PAD_NUM > 1 if (ao_pad_ignite & (1 << 1)) - pins |= (1 << AO_PAD_PIN_1); + pins_pad(1) |= (1 << AO_PAD_PIN_1); #endif #if AO_PAD_NUM > 2 if (ao_pad_ignite & (1 << 2)) - pins |= (1 << AO_PAD_PIN_2); + pins_pad(2) |= (1 << AO_PAD_PIN_2); #endif #if AO_PAD_NUM > 3 if (ao_pad_ignite & (1 << 3)) - pins |= (1 << AO_PAD_PIN_3); + pins_pad(3) |= (1 << AO_PAD_PIN_3); +#endif +#if AO_PAD_NUM > 4 + if (ao_pad_ignite & (1 << 4)) + pins_pad(4) |= (1 << AO_PAD_PIN_4); +#endif +#if AO_PAD_NUM > 5 + if (ao_pad_ignite & (1 << 5)) + pins_pad(5) |= (1 << AO_PAD_PIN_5); +#endif +#if AO_PAD_NUM > 6 + if (ao_pad_ignite & (1 << 6)) + pins_pad(6) |= (1 << AO_PAD_PIN_6); +#endif +#if AO_PAD_NUM > 7 + if (ao_pad_ignite & (1 << 7)) + pins_pad(7) |= (1 << AO_PAD_PIN_7); +#endif +#ifdef AO_PAD_PORT_1 + PRINTD("ignite pins 0x%x 0x%x\n", pins0, pins1); + ao_gpio_set_bits(AO_PAD_PORT_0, pins0); + ao_gpio_set_bits(AO_PAD_PORT_1, pins1); +#else + PRINTD("ignite pins 0x%x\n", pins0); + ao_gpio_set_bits(AO_PAD_PORT_0, pins0); #endif - PRINTD("ignite pins 0x%x\n", pins); - ao_gpio_set_bits(AO_PAD_PORT, pins); while (ao_pad_ignite) { ao_pad_ignite = 0; ao_delay(AO_PAD_FIRE_TIME); } - ao_gpio_clr_bits(AO_PAD_PORT, pins); - PRINTD("turn off pins 0x%x\n", pins); +#ifdef AO_PAD_PORT_1 + ao_gpio_clr_bits(AO_PAD_PORT_0, pins0); + ao_gpio_clr_bits(AO_PAD_PORT_1, pins1); + PRINTD("turn off pins 0x%x 0x%x\n", pins0, pins1); +#else + ao_gpio_set_bits(AO_PAD_PORT_0, pins0); + PRINTD("turn off pins 0x%x\n", pins0); +#endif } } @@ -284,6 +326,23 @@ ao_pad_read_box(void) } #endif +#ifdef AO_PAD_SELECTOR_PORT +static int ao_pad_read_box(void) { + AO_PORT_TYPE value = ao_gpio_get_all(AO_PAD_SELECTOR_PORT); + unsigned pin; + int select = 1; + + for (pin = 0; pin < sizeof (AO_PORT_TYPE) * 8; pin++) { + if (AO_PAD_SELECTOR_PINS & (1 << pin)) { + if ((value & (1 << pin)) == 0) + return select; + select++; + } + } + return ao_config.pad_box; +} +#else + #if HAS_FIXED_PAD_BOX #define ao_pad_read_box() ao_config.pad_box #endif @@ -292,6 +351,8 @@ ao_pad_read_box(void) #define ao_pad_read_box() PAD_BOX #endif +#endif + static void ao_pad(void) { @@ -497,20 +558,51 @@ __code struct ao_cmds ao_pad_cmds[] = { { 0, NULL } }; +#ifndef AO_PAD_PORT_1 +#define AO_PAD_0_PORT AO_PAD_PORT +#define AO_PAD_1_PORT AO_PAD_PORT +#define AO_PAD_2_PORT AO_PAD_PORT +#define AO_PAD_3_PORT AO_PAD_PORT +#define AO_PAD_4_PORT AO_PAD_PORT +#define AO_PAD_5_PORT AO_PAD_PORT +#define AO_PAD_6_PORT AO_PAD_PORT +#define AO_PAD_7_PORT AO_PAD_PORT +#endif + void ao_pad_init(void) { +#ifdef AO_PAD_SELECTOR_PORT + unsigned pin; + + for (pin = 0; pin < sizeof (AO_PORT_TYPE) * 8; pin++) { + if (AO_PAD_SELECTOR_PINS & (1 << pin)) + ao_enable_input(AO_PAD_SELECTOR_PORT, pin, AO_EXTI_MODE_PULL_UP); + } +#endif #if AO_PAD_NUM > 0 - ao_enable_output(AO_PAD_PORT, AO_PAD_PIN_0, AO_PAD_0, 0); + ao_enable_output(AO_PAD_0_PORT, AO_PAD_PIN_0, AO_PAD_0, 0); #endif #if AO_PAD_NUM > 1 - ao_enable_output(AO_PAD_PORT, AO_PAD_PIN_1, AO_PAD_1, 0); + ao_enable_output(AO_PAD_1_PORT, AO_PAD_PIN_1, AO_PAD_1, 0); #endif #if AO_PAD_NUM > 2 - ao_enable_output(AO_PAD_PORT, AO_PAD_PIN_2, AO_PAD_2, 0); + ao_enable_output(AO_PAD_2_PORT, AO_PAD_PIN_2, AO_PAD_2, 0); #endif #if AO_PAD_NUM > 3 - ao_enable_output(AO_PAD_PORT, AO_PAD_PIN_3, AO_PAD_3, 0); + ao_enable_output(AO_PAD_3_PORT, AO_PAD_PIN_3, AO_PAD_3, 0); +#endif +#if AO_PAD_NUM > 4 + ao_enable_output(AO_PAD_4_PORT, AO_PAD_PIN_4, AO_PAD_4, 0); +#endif +#if AO_PAD_NUM > 5 + ao_enable_output(AO_PAD_5_PORT, AO_PAD_PIN_5, AO_PAD_5, 0); +#endif +#if AO_PAD_NUM > 5 + ao_enable_output(AO_PAD_6_PORT, AO_PAD_PIN_6, AO_PAD_6, 0); +#endif +#if AO_PAD_NUM > 7 + ao_enable_output(AO_PAD_7_PORT, AO_PAD_PIN_7, AO_PAD_7, 0); #endif #ifdef AO_STROBE ao_enable_output(AO_STROBE_PORT, AO_STROBE_PIN, AO_STROBE, 0); -- cgit v1.2.3 From 14fa6cee857918966740d05c4ed13b1a79db3caa Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 21 May 2018 22:36:35 -0700 Subject: altos: Add TeleFireEight v1.0 product Eight pyro channel version of the TeleFire boards. Signed-off-by: Keith Packard --- src/telefireeight-v1.0/.gitignore | 2 + src/telefireeight-v1.0/Makefile | 90 ++++++++ src/telefireeight-v1.0/ao_pins.h | 290 +++++++++++++++++++++++++ src/telefireeight-v1.0/ao_telefireeight.c | 55 +++++ src/telefireeight-v1.0/flash-loader/.gitignore | 2 + src/telefireeight-v1.0/flash-loader/Makefile | 8 + src/telefireeight-v1.0/flash-loader/ao_pins.h | 33 +++ 7 files changed, 480 insertions(+) create mode 100644 src/telefireeight-v1.0/.gitignore create mode 100644 src/telefireeight-v1.0/Makefile create mode 100644 src/telefireeight-v1.0/ao_pins.h create mode 100644 src/telefireeight-v1.0/ao_telefireeight.c create mode 100644 src/telefireeight-v1.0/flash-loader/.gitignore create mode 100644 src/telefireeight-v1.0/flash-loader/Makefile create mode 100644 src/telefireeight-v1.0/flash-loader/ao_pins.h (limited to 'src') diff --git a/src/telefireeight-v1.0/.gitignore b/src/telefireeight-v1.0/.gitignore new file mode 100644 index 00000000..22f7e9ef --- /dev/null +++ b/src/telefireeight-v1.0/.gitignore @@ -0,0 +1,2 @@ +telefireeight-* +ao_product.h diff --git a/src/telefireeight-v1.0/Makefile b/src/telefireeight-v1.0/Makefile new file mode 100644 index 00000000..200c3150 --- /dev/null +++ b/src/telefireeight-v1.0/Makefile @@ -0,0 +1,90 @@ +# +# TeleFire build file +# + +include ../stm/Makefile.defs + +INC = \ + ao.h \ + ao_pins.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_pad.h \ + ao_product.h \ + ao_radio_spi.h \ + ao_radio_cmac.h \ + ao_cc1200_CC1200.h \ + ao_cc1200.h \ + stm32l.h +# +# Common AltOS sources +# + +#PROFILE=ao_profile.c +#PROFILE_DEF=-DAO_PROFILE=1 + +ALTOS_SRC = \ + ao_boot_chain.c \ + ao_interrupt.c \ + ao_product.c \ + ao_romconfig.c \ + ao_cmd.c \ + ao_adc_stm.c \ + ao_data.c \ + ao_config.c \ + ao_task.c \ + ao_led.c \ + ao_stdio.c \ + ao_panic.c \ + ao_timer.c \ + ao_mutex.c \ + ao_freq.c \ + ao_dma_stm.c \ + ao_spi_stm.c \ + ao_beep_stm.c \ + ao_eeprom_stm.c \ + ao_usb_stm.c \ + ao_exti_stm.c \ + ao_cc1200.c \ + ao_radio_cmac.c \ + ao_aes.c \ + ao_aes_tables.c \ + ao_pad.c \ + ao_radio_cmac_cmd.c + +PRODUCT_SRC = \ + ao_telefireeight.c + +PRODUCT=TeleFireEight-v1.0 +PRODUCT_DEF=-DTELEFIREEIGHT_V_1_0 +IDPRODUCT=0x000f + +CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) -Os -g + +PROGNAME = telefireeight-v1.0 +PROG = $(PROGNAME)-$(VERSION).elf +HEX = $(PROGNAME)-$(VERSION).ihx + +SRC = $(ALTOS_SRC) $(PRODUCT_SRC) +OBJ=$(SRC:.c=.o) + +all: $(PROG) $(HEX) + +$(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 $(PROGNAME)-*.elf $(PROGNAME)-*.ihx + rm -f ao_product.h + +install: + +uninstall: + diff --git a/src/telefireeight-v1.0/ao_pins.h b/src/telefireeight-v1.0/ao_pins.h new file mode 100644 index 00000000..8e3529e7 --- /dev/null +++ b/src/telefireeight-v1.0/ao_pins.h @@ -0,0 +1,290 @@ +/* + * Copyright © 2010 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +#define HAS_RADIO 1 +#define HAS_RADIO_RATE 1 +#define HAS_TELEMETRY 0 +#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 IGNITE_ON_P0 0 +#define PACKET_HAS_MASTER 0 +#define PACKET_HAS_SLAVE 0 +#define AO_DATA_RING 32 +#define USE_EEPROM_CONFIG 1 +#define USE_STORAGE_CONFIG 0 +#define HAS_AES 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) + +#define AO_CC1200_FOSC 40000000 + +/* 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_SPI_1 1 +#define SPI_1_PA5_PA6_PA7 0 +#define SPI_1_PB3_PB4_PB5 0 +#define SPI_1_PE13_PE14_PE15 1 +#define SPI_1_GPIO (&stm_gpioe) +#define SPI_1_SCK 13 +#define SPI_1_MISO 14 +#define SPI_1_MOSI 15 +#define SPI_1_OSPEEDR STM_OSPEEDR_10MHz + +#define HAS_SPI_2 0 +#define SPI_2_PB13_PB14_PB15 0 +#define SPI_2_PD1_PD3_PD4 0 + +#define HAS_I2C_1 0 + +#define HAS_I2C_2 0 + +#define PACKET_HAS_SLAVE 0 +#define PACKET_HAS_MASTER 0 + +#define FAST_TIMER_FREQ 10000 /* .1ms for debouncing */ + +/* + * Radio is a cc1200 connected via SPI + */ + +#define AO_RADIO_CAL_DEFAULT 5695733 + +#define AO_CC1200_SPI_CS_PORT (&stm_gpioe) +#define AO_CC1200_SPI_CS_PIN 11 +#define AO_CC1200_SPI_BUS AO_SPI_1_PE13_PE14_PE15 +#define AO_CC1200_SPI stm_spi1 +#define AO_CC1200_SPI_SPEED AO_SPI_SPEED_FAST + +#define AO_CC1200_INT_PORT (&stm_gpioe) +#define AO_CC1200_INT_PIN (12) + +#define AO_CC1200_INT_GPIO 2 +#define AO_CC1200_INT_GPIO_IOCFG CC1200_IOCFG2 + +#define LED_PER_LED 1 +#define LED_TYPE uint16_t + +/* Continuity leds 1-8 */ +#define LED_0_PORT (&stm_gpiob) +#define LED_0_PIN 13 +#define LED_1_PORT (&stm_gpiob) +#define LED_1_PIN 12 +#define LED_2_PORT (&stm_gpiob) +#define LED_2_PIN 11 +#define LED_3_PORT (&stm_gpiob) +#define LED_3_PIN 10 +#define LED_4_PORT (&stm_gpioc) +#define LED_4_PIN 9 +#define LED_5_PORT (&stm_gpioa) +#define LED_5_PIN 8 +#define LED_6_PORT (&stm_gpioa) +#define LED_6_PIN 9 +#define LED_7_PORT (&stm_gpioa) +#define LED_7_PIN 10 + +#define LED_PIN_CONTINUITY_0 0 +#define LED_PIN_CONTINUITY_1 1 +#define LED_PIN_CONTINUITY_2 2 +#define LED_PIN_CONTINUITY_3 3 +#define LED_PIN_CONTINUITY_4 4 +#define LED_PIN_CONTINUITY_5 5 +#define LED_PIN_CONTINUITY_6 6 +#define LED_PIN_CONTINUITY_7 7 + +/* ARM */ +#define LED_8_PORT (&stm_gpioe) +#define LED_8_PIN 3 + +#define LED_PIN_ARMED 8 + +/* RF good/marginal/poor */ +#define LED_9_PORT (&stm_gpioe) +#define LED_9_PIN 4 +#define LED_10_PORT (&stm_gpioe) +#define LED_10_PIN 5 +#define LED_11_PORT (&stm_gpioe) +#define LED_11_PIN 6 + +#define LED_PIN_GREEN 9 +#define LED_PIN_AMBER 10 +#define LED_PIN_RED 11 + +#define AO_LED_CONTINUITY(c) (1 << (c)) +#define AO_LED_CONTINUITY_MASK (0xff) + +#define AO_LED_ARMED (1 << LED_PIN_ARMED) +#define AO_LED_GREEN (1 << LED_PIN_GREEN) +#define AO_LED_AMBER (1 << LED_PIN_AMBER) +#define AO_LED_RED (1 << LED_PIN_RED) + +#define LEDS_AVAILABLE (0xfff) + +/* Alarm A */ +#define AO_SIREN +#define AO_SIREN_PORT (&stm_gpiod) +#define AO_SIREN_PIN 10 + +/* Alarm B */ +#define AO_STROBE +#define AO_STROBE_PORT (&stm_gpiod) +#define AO_STROBE_PIN 11 + +/* Pad selector is on PD0-7 */ + +#define HAS_FIXED_PAD_BOX 1 +#define AO_PAD_SELECTOR_PORT (&stm_gpiod) +#define AO_PAD_SELECTOR_PINS (0xff) + +#define SPI_CONST 0x00 + +#define AO_PAD_NUM 8 +#define AO_PAD_PORT_0 (&stm_gpiod) +#define AO_PAD_PORT_1 (&stm_gpiob) + +#define AO_PAD_PIN_0 9 +#define AO_PAD_0_PORT (&stm_gpiod) +#define AO_PAD_ADC_0 3 + +#define AO_PAD_PIN_1 8 +#define AO_PAD_1_PORT (&stm_gpiod) +#define AO_PAD_ADC_1 2 + +#define AO_PAD_PIN_2 15 +#define AO_PAD_2_PORT (&stm_gpiob) +#define AO_PAD_ADC_2 1 + +#define AO_PAD_PIN_3 14 +#define AO_PAD_3_PORT (&stm_gpiob) +#define AO_PAD_ADC_3 0 + +#define AO_PAD_PIN_4 12 +#define AO_PAD_4_PORT (&stm_gpiod) +#define AO_PAD_ADC_4 7 + +#define AO_PAD_PIN_5 13 +#define AO_PAD_5_PORT (&stm_gpiod) +#define AO_PAD_ADC_5 6 + +#define AO_PAD_PIN_6 14 +#define AO_PAD_6_PORT (&stm_gpiod) +#define AO_PAD_ADC_6 5 + +#define AO_PAD_PIN_7 15 +#define AO_PAD_7_PORT (&stm_gpiod) +#define AO_PAD_ADC_7 4 + +#define AO_PAD_ALL_CHANNELS (0xff) + +/* 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 8 +#define AO_PAD_ADC_BATT 15 + +#define AO_ADC_FIRST_PIN 0 + +#define AO_NUM_ADC 10 + +#define AO_ADC_SQ1 AO_PAD_ADC_0 +#define AO_ADC_SQ2 AO_PAD_ADC_1 +#define AO_ADC_SQ3 AO_PAD_ADC_2 +#define AO_ADC_SQ4 AO_PAD_ADC_3 +#define AO_ADC_SQ5 AO_PAD_ADC_4 +#define AO_ADC_SQ6 AO_PAD_ADC_5 +#define AO_ADC_SQ7 AO_PAD_ADC_6 +#define AO_ADC_SQ8 AO_PAD_ADC_7 +#define AO_ADC_SQ9 AO_PAD_ADC_PYRO +#define AO_ADC_SQ10 AO_PAD_ADC_BATT + +#define AO_PYRO_R_PYRO_SENSE 200 +#define AO_PYRO_R_SENSE_GND 22 + +#define AO_FIRE_R_POWER_FET 0 +#define AO_FIRE_R_FET_SENSE 200 +#define AO_FIRE_R_SENSE_GND 22 + +#define HAS_ADC_TEMP 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 " \ + "4: %5d 5: %5d 6: %5d 7: %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.sense[4], \ + (p)->adc.sense[5], \ + (p)->adc.sense[6], \ + (p)->adc.sense[7], \ + (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_4) | \ + (1 << AO_PAD_ADC_5) | \ + (1 << AO_PAD_ADC_6) | \ + (1 << AO_PAD_ADC_7) | \ + (1 << AO_PAD_ADC_PYRO) | \ + (1 << AO_PAD_ADC_BATT)) + +#endif /* _AO_PINS_H_ */ diff --git a/src/telefireeight-v1.0/ao_telefireeight.c b/src/telefireeight-v1.0/ao_telefireeight.c new file mode 100644 index 00000000..bdcf3213 --- /dev/null +++ b/src/telefireeight-v1.0/ao_telefireeight.c @@ -0,0 +1,55 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include +#include + +void +main(void) +{ + ao_clock_init(); + + ao_led_init(LEDS_AVAILABLE); + + ao_task_init(); + + ao_timer_init(); + ao_spi_init(); + ao_dma_init(); + ao_exti_init(); + + ao_cmd_init(); + + ao_adc_init(); + + ao_eeprom_init(); + + ao_radio_init(); + + ao_usb_init(); + + ao_config_init(); + + ao_pad_init(); + +// ao_radio_cmac_cmd_init(); + + ao_start_scheduler(); +} diff --git a/src/telefireeight-v1.0/flash-loader/.gitignore b/src/telefireeight-v1.0/flash-loader/.gitignore new file mode 100644 index 00000000..65fe7eab --- /dev/null +++ b/src/telefireeight-v1.0/flash-loader/.gitignore @@ -0,0 +1,2 @@ +*.elf +*.ihx diff --git a/src/telefireeight-v1.0/flash-loader/Makefile b/src/telefireeight-v1.0/flash-loader/Makefile new file mode 100644 index 00000000..baf0e3f3 --- /dev/null +++ b/src/telefireeight-v1.0/flash-loader/Makefile @@ -0,0 +1,8 @@ +# +# AltOS flash loader build +# +# + +TOPDIR=../.. +HARDWARE=telefireeight-v1.0 +include $(TOPDIR)/stm/Makefile-flash.defs diff --git a/src/telefireeight-v1.0/flash-loader/ao_pins.h b/src/telefireeight-v1.0/flash-loader/ao_pins.h new file mode 100644 index 00000000..889feef6 --- /dev/null +++ b/src/telefireeight-v1.0/flash-loader/ao_pins.h @@ -0,0 +1,33 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +/* External crystal at 8MHz */ +#define AO_HSE 8000000 + +#include + +#define AO_BOOT_PIN 1 +#define AO_BOOT_APPLICATION_GPIO stm_gpioe +#define AO_BOOT_APPLICATION_PIN 2 +#define AO_BOOT_APPLICATION_VALUE 1 +#define AO_BOOT_APPLICATION_MODE AO_EXTI_MODE_PULL_UP + +#endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From eb70e334ffc77e737bb21ab3fe777a982d80ee3b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 11:12:36 -0700 Subject: altos/telelco-v2: Change select button to alternate box/pad mode Instead of a three-way, just make the select button two-way and have the arming switch disable it. Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_lco_v2.c | 79 ++++++++++++++------------------------------ 1 file changed, 24 insertions(+), 55 deletions(-) (limited to 'src') diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index 6f2b618a..16903d78 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -52,15 +52,12 @@ static uint16_t ao_lco_tick_offset[AO_PAD_MAX_BOXES]; /* UI values */ static uint8_t ao_lco_armed; static uint8_t ao_lco_firing; -static uint16_t ao_lco_fire_tick; -static uint8_t ao_lco_fire_down; static uint8_t ao_lco_drag_race; static uint8_t ao_lco_pad; static int16_t ao_lco_box; static uint8_t ao_lco_select_mode; #define AO_LCO_SELECT_PAD 0 #define AO_LCO_SELECT_BOX 1 -#define AO_LCO_SELECT_NONE 2 static struct ao_pad_query ao_pad_query; @@ -167,19 +164,22 @@ ao_lco_pad_first(uint8_t box) static void ao_lco_set_select(void) { - switch (ao_lco_select_mode) { - case AO_LCO_SELECT_PAD: - ao_led_off(AO_LED_BOX); - ao_led_on(AO_LED_PAD); - break; - case AO_LCO_SELECT_BOX: - ao_led_off(AO_LED_PAD); - ao_led_on(AO_LED_BOX); - break; - default: + if (ao_lco_armed) { ao_led_off(AO_LED_PAD); ao_led_off(AO_LED_BOX); - break; + } else { + switch (ao_lco_select_mode) { + case AO_LCO_SELECT_PAD: + ao_led_off(AO_LED_BOX); + ao_led_on(AO_LED_PAD); + break; + case AO_LCO_SELECT_BOX: + ao_led_off(AO_LED_PAD); + ao_led_on(AO_LED_BOX); + break; + default: + break; + } } } @@ -254,7 +254,6 @@ ao_lco_drag_enable(void) memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); ao_lco_drag_beep(5); ao_lco_set_display(); - ao_lco_fire_down = 0; } static void @@ -265,34 +264,6 @@ ao_lco_drag_disable(void) memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); ao_lco_drag_beep(2); ao_lco_set_display(); - ao_lco_fire_down = 0; -} - -static uint16_t -ao_lco_drag_button_check(uint16_t now, uint16_t delay) -{ - uint16_t button_delay = ~0; - - /* - * Check to see if the button has been held down long enough - * to switch in/out of drag race mode - */ - if (ao_lco_fire_down) { - if (ao_lco_drag_race) { - if ((int16_t) (now - ao_lco_fire_tick) >= AO_LCO_DRAG_RACE_STOP_TIME) - ao_lco_drag_disable(); - else - button_delay = ao_lco_fire_tick + AO_LCO_DRAG_RACE_STOP_TIME - now; - } else { - if ((int16_t) (now - ao_lco_fire_tick) >= AO_LCO_DRAG_RACE_START_TIME) - ao_lco_drag_enable(); - else - button_delay = ao_lco_fire_tick + AO_LCO_DRAG_RACE_START_TIME - now; - } - if (delay > button_delay) - delay = button_delay; - } - return delay; } static uint16_t @@ -330,12 +301,11 @@ ao_lco_drag_monitor(void) continue; now = ao_time(); - delay = ao_lco_drag_button_check(now, delay); delay = ao_lco_drag_warn_check(now, delay); delay = ao_lco_drag_beep_check(now, delay); /* check to see if there's anything left to do here */ - if (!ao_lco_fire_down && !ao_lco_drag_race && !ao_lco_drag_beep_count) { + if (!ao_lco_drag_race && !ao_lco_drag_beep_count) { delay = ~0; ao_lco_drag_active = 0; } @@ -412,12 +382,11 @@ ao_lco_input(void) if (ao_lco_drag_race) { uint8_t box; - for (box = ao_lco_min_box; box <= ao_lco_max_box; box++) { - if (ao_lco_selected[box]) { - ao_wakeup(&ao_lco_armed); + for (box = ao_lco_min_box; box <= ao_lco_max_box; box++) + if (ao_lco_selected[box]) break; - } - } + if (box > ao_lco_max_box) + ao_lco_armed = 0; } else { memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); if (ao_lco_pad != 0) @@ -426,11 +395,11 @@ ao_lco_input(void) ao_lco_armed = 0; } } + ao_lco_set_select(); ao_wakeup(&ao_lco_armed); break; case AO_BUTTON_FIRE: if (ao_lco_armed) { - ao_lco_fire_down = 0; ao_lco_firing = event.value; PRINTD("Firing %d\n", ao_lco_firing); ao_wakeup(&ao_lco_armed); @@ -453,10 +422,10 @@ ao_lco_input(void) ao_lco_drag_disable(); break; case AO_BUTTON_SELECT: - ao_lco_select_mode++; - if (ao_lco_select_mode > AO_LCO_SELECT_NONE) - ao_lco_select_mode = AO_LCO_SELECT_PAD; - ao_lco_set_select(); + if (!ao_lco_armed) { + ao_lco_select_mode = 1 - ao_lco_select_mode; + ao_lco_set_select(); + } break; } break; -- cgit v1.2.3 From 863a9d523b26a8a5f8fbd5b516bd7dd914ee079e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 11:23:03 -0700 Subject: altos/ao_button.c: Support up to 16 buttons TeleLCO v2.0 has more than 4 Signed-off-by: Keith Packard --- src/drivers/ao_button.c | 69 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 69 insertions(+) (limited to 'src') diff --git a/src/drivers/ao_button.c b/src/drivers/ao_button.c index 07e92c67..f6a9676b 100644 --- a/src/drivers/ao_button.c +++ b/src/drivers/ao_button.c @@ -68,6 +68,39 @@ _ao_button_get(uint8_t b) #endif #if AO_BUTTON_COUNT > 4 case 4: return ao_button_value(4); +#endif +#if AO_BUTTON_COUNT > 5 + case 5: return ao_button_value(5); +#endif +#if AO_BUTTON_COUNT > 6 + case 6: return ao_button_value(6); +#endif +#if AO_BUTTON_COUNT > 7 + case 7: return ao_button_value(7); +#endif +#if AO_BUTTON_COUNT > 8 + case 8: return ao_button_value(8); +#endif +#if AO_BUTTON_COUNT > 9 + case 9: return ao_button_value(9); +#endif +#if AO_BUTTON_COUNT > 10 + case 10: return ao_button_value(10); +#endif +#if AO_BUTTON_COUNT > 11 + case 11: return ao_button_value(11); +#endif +#if AO_BUTTON_COUNT > 12 + case 12: return ao_button_value(12); +#endif +#if AO_BUTTON_COUNT > 13 + case 13: return ao_button_value(13); +#endif +#if AO_BUTTON_COUNT > 14 + case 14: return ao_button_value(14); +#endif +#if AO_BUTTON_COUNT > 15 + case 15: return ao_button_value(15); #endif } return 0; @@ -144,4 +177,40 @@ ao_button_init(void) #if AO_BUTTON_COUNT > 4 init(4); #endif +#if AO_BUTTON_COUNT > 5 + init(5); +#endif +#if AO_BUTTON_COUNT > 6 + init(6); +#endif +#if AO_BUTTON_COUNT > 7 + init(7); +#endif +#if AO_BUTTON_COUNT > 8 + init(8); +#endif +#if AO_BUTTON_COUNT > 9 + init(9); +#endif +#if AO_BUTTON_COUNT > 10 + init(10); +#endif +#if AO_BUTTON_COUNT > 11 + init(11); +#endif +#if AO_BUTTON_COUNT > 12 + init(12); +#endif +#if AO_BUTTON_COUNT > 13 + init(13); +#endif +#if AO_BUTTON_COUNT > 14 + init(14); +#endif +#if AO_BUTTON_COUNT > 15 + init(15); +#endif +#if AO_BUTTON_COUNT > 16 + #error too many buttons +#endif } -- cgit v1.2.3 From 7e2a2849f58e98adc1114bb8f3a6319408d93691 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 12:06:41 -0700 Subject: altos/telelco-v2.0: Only switch encoder mode on push, not release Was not checking event value. Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_lco_v2.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index 16903d78..1f38ec81 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -422,9 +422,11 @@ ao_lco_input(void) ao_lco_drag_disable(); break; case AO_BUTTON_SELECT: - if (!ao_lco_armed) { - ao_lco_select_mode = 1 - ao_lco_select_mode; - ao_lco_set_select(); + if (event.value) { + if (!ao_lco_armed) { + ao_lco_select_mode = 1 - ao_lco_select_mode; + ao_lco_set_select(); + } } break; } -- cgit v1.2.3 From a4dbc940cc6c8ff5565e8af21f2dcb4ae090380c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 13:07:06 -0700 Subject: altos/drivers: Make quadrature debounce longer by default. Fix state tracking This increases the default debounce time for quadrature encoders to 30ms, which cleans up the mechanical encoders on TeleLCO v0.2. Also change state tracking to explicitly check for expected state values to avoid mis-triggering. Signed-off-by: Keith Packard --- src/drivers/ao_quadrature.c | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index deecfb79..d9cdfe7c 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -39,20 +39,24 @@ static struct ao_debounce ao_debounce_state[AO_QUADRATURE_COUNT][2]; #define pinb(q) AO_QUADRATURE_ ## q ## _B ## _PIN #define isr(q) ao_quadrature_isr_ ## q -#define DEBOUNCE 10 +#ifndef AO_QUADRATURE_DEBOUNCE +#define AO_QUADRATURE_DEBOUNCE 30 +#endif static uint8_t ao_debounce(uint8_t cur, struct ao_debounce *debounce) { - if (cur == debounce->state) - debounce->count = 0; - else { - if (++debounce->count == DEBOUNCE) { - debounce->state = cur; - debounce->count = 0; - } +#if AO_QUADRATURE_DEBOUNCE > 0 + if (debounce->count > 0) { + debounce->count--; + } else if (cur != debounce->state) { + debounce->state = cur; + debounce->count = AO_QUADRATURE_DEBOUNCE; } return debounce->state; +#else + return cur; +#endif } static uint16_t @@ -84,9 +88,9 @@ _ao_quadrature_set(uint8_t q, uint8_t new) { uint8_t old = ao_quadrature_state[q]; if (old != new && new == 0) { - if (old & 2) + if (old == 2) _ao_quadrature_queue(q, 1); - else if (old & 1) + else if (old == 1) _ao_quadrature_queue(q, -1); } ao_quadrature_state[q] = new; -- cgit v1.2.3 From f172b139ddc3fcc17cf89b57e5126264c0faa45e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 13:08:33 -0700 Subject: altos/telelco-v2.0: Reduce quadrature debounce time The optical encoders shouldn't bounce at all; let's try a minimal value. Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_pins.h | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/telelco-v2.0/ao_pins.h b/src/telelco-v2.0/ao_pins.h index d8cf4dec..46a766c1 100644 --- a/src/telelco-v2.0/ao_pins.h +++ b/src/telelco-v2.0/ao_pins.h @@ -305,6 +305,7 @@ */ #define AO_QUADRATURE_COUNT 1 +#define AO_QUADRATURE_DEBOUNCE 1 #define AO_QUADRATURE_0_PORT &stm_gpioe #define AO_QUADRATURE_0_A 15 -- cgit v1.2.3 From ccad6e970c5c7416561443f1e6e2fa8f80698957 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 14:15:34 -0700 Subject: altos/driver: Add support for one-step-per-click quadrature encoder The mechanical encoders go through all four steps per click while the optical ones have a single step per click. Support both kinds. Signed-off-by: Keith Packard --- src/drivers/ao_quadrature.c | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) (limited to 'src') diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index d9cdfe7c..98bd7c08 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -55,6 +55,7 @@ ao_debounce(uint8_t cur, struct ao_debounce *debounce) } return debounce->state; #else + (void) debounce; return cur; #endif } @@ -83,16 +84,38 @@ _ao_quadrature_queue(uint8_t q, int8_t step) ao_wakeup(&ao_quadrature_count[q]); } +#if AO_QUADRATURE_SINGLE_CODE +struct ao_quadrature_step { + uint8_t inc; + uint8_t dec; +}; + +static struct ao_quadrature_step ao_quadrature_steps[4] = { + [0] = { .inc = 1, .dec = 2 }, + [1] = { .inc = 3, .dec = 0 }, + [3] = { .inc = 2, .dec = 1 }, + [2] = { .inc = 0, .dec = 3 }, +}; +#endif + static void _ao_quadrature_set(uint8_t q, uint8_t new) { uint8_t old = ao_quadrature_state[q]; +#ifdef AO_QUADRATURE_SINGLE_CODE + if (new == ao_quadrature_steps[old].inc) { + _ao_quadrature_queue(q, 1); + } else if (new == ao_quadrature_steps[old].dec) { + _ao_quadrature_queue(q, -1); + } +#else if (old != new && new == 0) { if (old == 2) _ao_quadrature_queue(q, 1); else if (old == 1) _ao_quadrature_queue(q, -1); } +#endif ao_quadrature_state[q] = new; } -- cgit v1.2.3 From a26cf26e3416b1982abec249678a32a420bf8809 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 14:26:12 -0700 Subject: altos/telelco-v2.0: Use single-step quadrature code Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_pins.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/telelco-v2.0/ao_pins.h b/src/telelco-v2.0/ao_pins.h index 46a766c1..cdca5c10 100644 --- a/src/telelco-v2.0/ao_pins.h +++ b/src/telelco-v2.0/ao_pins.h @@ -305,7 +305,8 @@ */ #define AO_QUADRATURE_COUNT 1 -#define AO_QUADRATURE_DEBOUNCE 1 +#define AO_QUADRATURE_DEBOUNCE 0 +#define AO_QUADRATURE_SINGLE_CODE 1 #define AO_QUADRATURE_0_PORT &stm_gpioe #define AO_QUADRATURE_0_A 15 -- cgit v1.2.3 From 2c94ba66d5b4c99b43ab965331bf1faa270a9768 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 14:26:47 -0700 Subject: altos/telelco-v2.0: Only look at drag select presses, not releases Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_lco_v2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index 1f38ec81..748b552f 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -406,7 +406,7 @@ ao_lco_input(void) } break; case AO_BUTTON_DRAG_SELECT: - if (ao_lco_drag_race) { + if (event.value && ao_lco_drag_race) { if (ao_lco_pad != 0) { ao_lco_selected[ao_lco_box] ^= (1 << (ao_lco_pad - 1)); PRINTD("Toggle box %d pad %d (pads now %x) to drag race\n", -- cgit v1.2.3 From b1f1a4513391aa595eb64552f2aa4cfedd0cc0ff Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 14:35:18 -0700 Subject: altos/telelco-v2.0: Mixed up the select_button pin (is E0, was set to E13) Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_pins.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/telelco-v2.0/ao_pins.h b/src/telelco-v2.0/ao_pins.h index cdca5c10..77caf254 100644 --- a/src/telelco-v2.0/ao_pins.h +++ b/src/telelco-v2.0/ao_pins.h @@ -355,6 +355,6 @@ #define AO_BUTTON_SELECT 8 #define AO_BUTTON_8_PORT &stm_gpioe -#define AO_BUTTON_8 13 +#define AO_BUTTON_8 0 #endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From 98ea690cc7514f43254f3a6c72668c11820f657c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 14:41:51 -0700 Subject: altos/telelco-v2.0: Fix button names and pin mappings. Add drag LED. Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_lco_v2.c | 6 ++++-- src/telelco-v2.0/ao_pins.h | 8 ++++---- 2 files changed, 8 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index 748b552f..cc6ab13a 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -252,6 +252,7 @@ ao_lco_drag_enable(void) PRINTD("Drag enable\n"); ao_lco_drag_race = 1; memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); + ao_led_on(AO_LED_DRAG); ao_lco_drag_beep(5); ao_lco_set_display(); } @@ -261,6 +262,7 @@ ao_lco_drag_disable(void) { PRINTD("Drag disable\n"); ao_lco_drag_race = 0; + ao_led_off(AO_LED_DRAG); memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); ao_lco_drag_beep(2); ao_lco_set_display(); @@ -415,13 +417,13 @@ ao_lco_input(void) } } break; - case AO_BUTTON_MODE_SELECT: + case AO_BUTTON_DRAG_MODE: if (event.value) ao_lco_drag_enable(); else ao_lco_drag_disable(); break; - case AO_BUTTON_SELECT: + case AO_BUTTON_ENCODER_SELECT: if (event.value) { if (!ao_lco_armed) { ao_lco_select_mode = 1 - ao_lco_select_mode; diff --git a/src/telelco-v2.0/ao_pins.h b/src/telelco-v2.0/ao_pins.h index 77caf254..73ce9d6c 100644 --- a/src/telelco-v2.0/ao_pins.h +++ b/src/telelco-v2.0/ao_pins.h @@ -321,13 +321,13 @@ #define AO_BUTTON_COUNT 9 #define AO_BUTTON_MODE AO_EXTI_MODE_PULL_UP -#define AO_BUTTON_MODE_SELECT 0 +#define AO_BUTTON_DRAG_MODE 0 #define AO_BUTTON_0_PORT &stm_gpioe #define AO_BUTTON_0 1 #define AO_BUTTON_DRAG_SELECT 1 #define AO_BUTTON_1_PORT &stm_gpioe -#define AO_BUTTON_1 1 +#define AO_BUTTON_1 0 #define AO_BUTTON_SPARE1 2 #define AO_BUTTON_2_PORT &stm_gpiob @@ -353,8 +353,8 @@ #define AO_BUTTON_7_PORT &stm_gpiod #define AO_BUTTON_7 11 -#define AO_BUTTON_SELECT 8 +#define AO_BUTTON_ENCODER_SELECT 8 #define AO_BUTTON_8_PORT &stm_gpioe -#define AO_BUTTON_8 0 +#define AO_BUTTON_8 13 #endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From 29f9880f7b09bc135d34bf0dcb221bdede7726b3 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 14:54:47 -0700 Subject: altos/telelco-v2.0: A bit fancier with the drag-mode LED show Continutity leds now show whether the pad is included in the drag race *and* the continutity using four different patterns: Drag select & igniter present: long on, short off Drag select & igniter missing: short on, long off Not select & igniter present: solid on Not select & igniter missing: solid off Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_lco_v2.c | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index cc6ab13a..d78f54d2 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -581,23 +581,32 @@ ao_lco_igniter_status(void) for (c = 0; c < AO_LED_CONTINUITY_NUM; c++) { uint8_t status; - if (ao_lco_drag_race) { - if (ao_lco_selected[ao_lco_box] & (1 << c) && t) + if (ao_pad_query.channels & (1 << c)) + status = ao_pad_query.igniter_status[c]; + else + status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN; + + if (ao_lco_drag_race && (ao_lco_selected[ao_lco_box] & (1 << c))) { + uint8_t on = 0; + if (status == AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN) { + if (t) + on = 1; + } else { + if (!t) + on = 1; + } + if (on) ao_led_on(continuity_led[c]); else ao_led_off(continuity_led[c]); } else { - if (ao_pad_query.channels & (1 << c)) - status = ao_pad_query.igniter_status[c]; - else - status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN; if (status == AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN) ao_led_on(continuity_led[c]); else ao_led_off(continuity_led[c]); } } - t = 1-t; + t = (t + 1) & 3; } } -- cgit v1.2.3 From eb31a40b3499287e0a52324a9adc3728883ed957 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 15:07:06 -0700 Subject: altos/telelco-v2.0: Add debug output for pad voltage display Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_lco_v2.c | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index d78f54d2..371b978a 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -102,6 +102,7 @@ ao_lco_set_voltage(uint16_t decivolts) { uint8_t tens, ones, tenths; + PRINTD("voltage %d\n", decivolts); tenths = decivolts % 10; ones = (decivolts / 10) % 10; tens = (decivolts / 100) % 10; -- cgit v1.2.3 From dbeb9b91a48418c5bc9f6edccaef20c3ef77d45d Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 15:12:26 -0700 Subject: altos/telelco-v2.0: Change drag race selected/no-continutity pattern Make it turn on at the same time as the pads with continuity, but turn off sooner Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_lco_v2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index 371b978a..45895908 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -593,7 +593,7 @@ ao_lco_igniter_status(void) if (t) on = 1; } else { - if (!t) + if (t == 1) on = 1; } if (on) -- cgit v1.2.3 From 3fb27f0ab7163c96945a60fd406e5423552206a2 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 16:09:19 -0700 Subject: altos/stm: Add polling ADC sampler API This just fetches ADC values on demand instead of loading them periodically into a ring buffer from the timer interrupt. Signed-off-by: Keith Packard --- src/stm/ao_adc_single.h | 24 +++ src/stm/ao_adc_single_stm.c | 363 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 387 insertions(+) create mode 100644 src/stm/ao_adc_single.h create mode 100644 src/stm/ao_adc_single_stm.c (limited to 'src') diff --git a/src/stm/ao_adc_single.h b/src/stm/ao_adc_single.h new file mode 100644 index 00000000..f9d953ca --- /dev/null +++ b/src/stm/ao_adc_single.h @@ -0,0 +1,24 @@ +/* + * Copyright © 2018 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + */ + +#ifndef _AO_ADC_SINGLE_H_ +#define _AO_ADC_SINGLE_H_ + +void +ao_adc_single_get(struct ao_adc *packet); + +void +ao_adc_single_init(void); + +#endif /* _AO_ADC_SINGLE_H_ */ diff --git a/src/stm/ao_adc_single_stm.c b/src/stm/ao_adc_single_stm.c new file mode 100644 index 00000000..907a064e --- /dev/null +++ b/src/stm/ao_adc_single_stm.c @@ -0,0 +1,363 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include + +static uint8_t ao_adc_ready; + +#define AO_ADC_CR2_VAL ((0 << STM_ADC_CR2_SWSTART) | \ + (STM_ADC_CR2_EXTEN_DISABLE << STM_ADC_CR2_EXTEN) | \ + (0 << STM_ADC_CR2_EXTSEL) | \ + (0 << STM_ADC_CR2_JWSTART) | \ + (STM_ADC_CR2_JEXTEN_DISABLE << STM_ADC_CR2_JEXTEN) | \ + (0 << STM_ADC_CR2_JEXTSEL) | \ + (0 << STM_ADC_CR2_ALIGN) | \ + (0 << STM_ADC_CR2_EOCS) | \ + (1 << STM_ADC_CR2_DDS) | \ + (1 << STM_ADC_CR2_DMA) | \ + (STM_ADC_CR2_DELS_UNTIL_READ << STM_ADC_CR2_DELS) | \ + (0 << STM_ADC_CR2_CONT) | \ + (1 << STM_ADC_CR2_ADON)) + +/* + * Callback from DMA ISR + * + * Shut down DMA engine, signal anyone waiting + */ +static void ao_adc_done(int index) +{ + (void) index; + ao_dma_done_transfer(STM_DMA_INDEX(STM_DMA_CHANNEL_ADC1)); + ao_adc_ready = 1; + ao_wakeup((void *) &ao_adc_ready); +} + +/* + * Start the ADC sequence using the DMA engine + */ +static void +ao_adc_poll(struct ao_adc *packet) +{ + ao_adc_ready = 0; + stm_adc.sr = 0; + ao_dma_set_transfer(STM_DMA_INDEX(STM_DMA_CHANNEL_ADC1), + &stm_adc.dr, + (void *) packet, + AO_NUM_ADC, + (0 << STM_DMA_CCR_MEM2MEM) | + (STM_DMA_CCR_PL_HIGH << STM_DMA_CCR_PL) | + (STM_DMA_CCR_MSIZE_16 << STM_DMA_CCR_MSIZE) | + (STM_DMA_CCR_PSIZE_16 << STM_DMA_CCR_PSIZE) | + (1 << STM_DMA_CCR_MINC) | + (0 << STM_DMA_CCR_PINC) | + (0 << STM_DMA_CCR_CIRC) | + (STM_DMA_CCR_DIR_PER_TO_MEM << STM_DMA_CCR_DIR)); + ao_dma_set_isr(STM_DMA_INDEX(STM_DMA_CHANNEL_ADC1), ao_adc_done); + ao_dma_start(STM_DMA_INDEX(STM_DMA_CHANNEL_ADC1)); + + stm_adc.cr2 = AO_ADC_CR2_VAL | (1 << STM_ADC_CR2_SWSTART); +} + +/* + * Fetch a copy of the most recent ADC data + */ +void +ao_adc_single_get(struct ao_adc *packet) +{ + ao_adc_poll(packet); + ao_arch_block_interrupts(); + while (!ao_adc_ready) + ao_sleep(&ao_adc_ready); + ao_arch_release_interrupts(); +} + +#ifdef AO_ADC_SQ1_NAME +static const char *ao_adc_name[AO_NUM_ADC] = { + AO_ADC_SQ1_NAME, +#ifdef AO_ADC_SQ2_NAME + AO_ADC_SQ2_NAME, +#endif +#ifdef AO_ADC_SQ3_NAME + AO_ADC_SQ3_NAME, +#endif +#ifdef AO_ADC_SQ4_NAME + AO_ADC_SQ4_NAME, +#endif +#ifdef AO_ADC_SQ5_NAME + AO_ADC_SQ5_NAME, +#endif +#ifdef AO_ADC_SQ6_NAME + AO_ADC_SQ6_NAME, +#endif +#ifdef AO_ADC_SQ7_NAME + AO_ADC_SQ7_NAME, +#endif +#ifdef AO_ADC_SQ8_NAME + AO_ADC_SQ8_NAME, +#endif +#ifdef AO_ADC_SQ9_NAME + AO_ADC_SQ9_NAME, +#endif +#ifdef AO_ADC_SQ10_NAME + AO_ADC_SQ10_NAME, +#endif +#ifdef AO_ADC_SQ11_NAME + AO_ADC_SQ11_NAME, +#endif +#ifdef AO_ADC_SQ12_NAME + AO_ADC_SQ12_NAME, +#endif +#ifdef AO_ADC_SQ13_NAME + AO_ADC_SQ13_NAME, +#endif +#ifdef AO_ADC_SQ14_NAME + AO_ADC_SQ14_NAME, +#endif +#ifdef AO_ADC_SQ15_NAME + AO_ADC_SQ15_NAME, +#endif +#ifdef AO_ADC_SQ16_NAME + AO_ADC_SQ16_NAME, +#endif +#ifdef AO_ADC_SQ17_NAME + AO_ADC_SQ17_NAME, +#endif +#ifdef AO_ADC_SQ18_NAME + AO_ADC_SQ18_NAME, +#endif +#ifdef AO_ADC_SQ19_NAME + AO_ADC_SQ19_NAME, +#endif +#ifdef AO_ADC_SQ20_NAME + AO_ADC_SQ20_NAME, +#endif +#ifdef AO_ADC_SQ21_NAME + #error "too many ADC names" +#endif +}; +#endif + +static void +ao_adc_dump(void) +{ + struct ao_adc packet; + ao_adc_single_get(&packet); + AO_ADC_DUMP(&packet); +} + +__code struct ao_cmds ao_adc_cmds[] = { + { ao_adc_dump, "a\0Display current ADC values" }, + { 0, NULL }, +}; + +void +ao_adc_single_init(void) +{ +#ifdef AO_ADC_PIN0_PORT + stm_rcc.ahbenr |= AO_ADC_RCC_AHBENR; +#endif + +#ifdef AO_ADC_PIN0_PORT + stm_moder_set(AO_ADC_PIN0_PORT, AO_ADC_PIN0_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN1_PORT + stm_moder_set(AO_ADC_PIN1_PORT, AO_ADC_PIN1_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN2_PORT + stm_moder_set(AO_ADC_PIN2_PORT, AO_ADC_PIN2_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN3_PORT + stm_moder_set(AO_ADC_PIN3_PORT, AO_ADC_PIN3_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN4_PORT + stm_moder_set(AO_ADC_PIN4_PORT, AO_ADC_PIN4_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN5_PORT + stm_moder_set(AO_ADC_PIN5_PORT, AO_ADC_PIN5_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN6_PORT + stm_moder_set(AO_ADC_PIN6_PORT, AO_ADC_PIN6_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN7_PORT + stm_moder_set(AO_ADC_PIN7_PORT, AO_ADC_PIN7_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN8_PORT + stm_moder_set(AO_ADC_PIN8_PORT, AO_ADC_PIN8_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN9_PORT + stm_moder_set(AO_ADC_PIN9_PORT, AO_ADC_PIN9_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN10_PORT + stm_moder_set(AO_ADC_PIN10_PORT, AO_ADC_PIN10_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN11_PORT + stm_moder_set(AO_ADC_PIN11_PORT, AO_ADC_PIN11_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN12_PORT + stm_moder_set(AO_ADC_PIN12_PORT, AO_ADC_PIN12_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN13_PORT + stm_moder_set(AO_ADC_PIN13_PORT, AO_ADC_PIN13_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN14_PORT + stm_moder_set(AO_ADC_PIN14_PORT, AO_ADC_PIN14_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN15_PORT + stm_moder_set(AO_ADC_PIN15_PORT, AO_ADC_PIN15_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN16_PORT + stm_moder_set(AO_ADC_PIN16_PORT, AO_ADC_PIN16_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN17_PORT + stm_moder_set(AO_ADC_PIN17_PORT, AO_ADC_PIN17_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN18_PORT + stm_moder_set(AO_ADC_PIN18_PORT, AO_ADC_PIN18_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN19_PORT + stm_moder_set(AO_ADC_PIN19_PORT, AO_ADC_PIN19_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN20_PORT + stm_moder_set(AO_ADC_PIN20_PORT, AO_ADC_PIN20_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN21_PORT + stm_moder_set(AO_ADC_PIN21_PORT, AO_ADC_PIN21_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN22_PORT + stm_moder_set(AO_ADC_PIN22_PORT, AO_ADC_PIN22_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN23_PORT + stm_moder_set(AO_ADC_PIN23_PORT, AO_ADC_PIN23_PIN, STM_MODER_ANALOG); +#endif +#ifdef AO_ADC_PIN24_PORT + #error "Too many ADC ports" +#endif + + stm_rcc.apb2enr |= (1 << STM_RCC_APB2ENR_ADC1EN); + + /* Turn off ADC during configuration */ + stm_adc.cr2 = 0; + + stm_adc.cr1 = ((0 << STM_ADC_CR1_OVRIE ) | + (STM_ADC_CR1_RES_12 << STM_ADC_CR1_RES ) | + (0 << STM_ADC_CR1_AWDEN ) | + (0 << STM_ADC_CR1_JAWDEN ) | + (0 << STM_ADC_CR1_PDI ) | + (0 << STM_ADC_CR1_PDD ) | + (0 << STM_ADC_CR1_DISCNUM ) | + (0 << STM_ADC_CR1_JDISCEN ) | + (0 << STM_ADC_CR1_DISCEN ) | + (0 << STM_ADC_CR1_JAUTO ) | + (0 << STM_ADC_CR1_AWDSGL ) | + (1 << STM_ADC_CR1_SCAN ) | + (0 << STM_ADC_CR1_JEOCIE ) | + (0 << STM_ADC_CR1_AWDIE ) | + (0 << STM_ADC_CR1_EOCIE ) | + (0 << STM_ADC_CR1_AWDCH )); + + /* 384 cycle sample time for everyone */ + stm_adc.smpr1 = 0x3ffff; + stm_adc.smpr2 = 0x3fffffff; + stm_adc.smpr3 = 0x3fffffff; + + stm_adc.sqr1 = ((AO_NUM_ADC - 1) << 20); + stm_adc.sqr2 = 0; + stm_adc.sqr3 = 0; + stm_adc.sqr4 = 0; + stm_adc.sqr5 = 0; +#if AO_NUM_ADC > 0 + stm_adc.sqr5 |= (AO_ADC_SQ1 << 0); +#endif +#if AO_NUM_ADC > 1 + stm_adc.sqr5 |= (AO_ADC_SQ2 << 5); +#endif +#if AO_NUM_ADC > 2 + stm_adc.sqr5 |= (AO_ADC_SQ3 << 10); +#endif +#if AO_NUM_ADC > 3 + stm_adc.sqr5 |= (AO_ADC_SQ4 << 15); +#endif +#if AO_NUM_ADC > 4 + stm_adc.sqr5 |= (AO_ADC_SQ5 << 20); +#endif +#if AO_NUM_ADC > 5 + stm_adc.sqr5 |= (AO_ADC_SQ6 << 25); +#endif +#if AO_NUM_ADC > 6 + stm_adc.sqr4 |= (AO_ADC_SQ7 << 0); +#endif +#if AO_NUM_ADC > 7 + stm_adc.sqr4 |= (AO_ADC_SQ8 << 5); +#endif +#if AO_NUM_ADC > 8 + stm_adc.sqr4 |= (AO_ADC_SQ9 << 10); +#endif +#if AO_NUM_ADC > 9 + stm_adc.sqr4 |= (AO_ADC_SQ10 << 15); +#endif +#if AO_NUM_ADC > 10 + stm_adc.sqr4 |= (AO_ADC_SQ11 << 20); +#endif +#if AO_NUM_ADC > 11 + stm_adc.sqr4 |= (AO_ADC_SQ12 << 25); +#endif +#if AO_NUM_ADC > 12 + stm_adc.sqr3 |= (AO_ADC_SQ13 << 0); +#endif +#if AO_NUM_ADC > 13 + stm_adc.sqr3 |= (AO_ADC_SQ14 << 5); +#endif +#if AO_NUM_ADC > 14 + stm_adc.sqr3 |= (AO_ADC_SQ15 << 10); +#endif +#if AO_NUM_ADC > 15 + stm_adc.sqr3 |= (AO_ADC_SQ16 << 15); +#endif +#if AO_NUM_ADC > 16 + stm_adc.sqr3 |= (AO_ADC_SQ17 << 20); +#endif +#if AO_NUM_ADC > 17 + stm_adc.sqr3 |= (AO_ADC_SQ18 << 25); +#endif +#if AO_NUM_ADC > 18 +#error "need to finish stm_adc.sqr settings" +#endif + + /* Turn ADC on */ + stm_adc.cr2 = AO_ADC_CR2_VAL; + + /* Wait for ADC to be ready */ + while (!(stm_adc.sr & (1 << STM_ADC_SR_ADONS))) + ; + +#ifndef HAS_ADC_TEMP +#error Please define HAS_ADC_TEMP +#endif +#if HAS_ADC_TEMP + stm_adc.ccr = ((1 << STM_ADC_CCR_TSVREFE)); +#else + stm_adc.ccr = 0; +#endif + /* Clear any stale status bits */ + stm_adc.sr = 0; + + ao_dma_alloc(STM_DMA_INDEX(STM_DMA_CHANNEL_ADC1)); + + ao_cmd_register(&ao_adc_cmds[0]); +} -- cgit v1.2.3 From 42a261a56606be69b5fb90fd6017c70a1e5d72dd Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 16:10:51 -0700 Subject: altos/telelco-v2.0: Display local batt voltage at startup Show the battery voltage for a second at startup. Signed-off-by: Keith Packard --- src/telelco-v2.0/Makefile | 2 ++ src/telelco-v2.0/ao_lco_v2.c | 14 ++++++++++++++ src/telelco-v2.0/ao_pins.h | 36 ++++++++++++++++++++++++++++++++++++ src/telelco-v2.0/ao_telelco.c | 2 ++ 4 files changed, 54 insertions(+) (limited to 'src') diff --git a/src/telelco-v2.0/Makefile b/src/telelco-v2.0/Makefile index 75c66abf..4871993d 100644 --- a/src/telelco-v2.0/Makefile +++ b/src/telelco-v2.0/Makefile @@ -50,6 +50,8 @@ ALTOS_SRC = \ ao_spi_stm.c \ ao_beep_stm.c \ ao_eeprom_stm.c \ + ao_adc_single_stm.c \ + ao_convert_volt.c \ ao_fast_timer.c \ ao_lcd_stm.c \ ao_usb_stm.c \ diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index 45895908..140955da 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -23,6 +23,7 @@ #include #include #include +#include #define DEBUG 1 @@ -625,6 +626,18 @@ ao_lco_arm_warn(void) } } +static void +ao_lco_batt_voltage(void) +{ + struct ao_adc packet; + int16_t decivolt; + + ao_adc_single_get(&packet); + decivolt = ao_battery_decivolt(packet.v_batt); + ao_lco_set_voltage(decivolt); + ao_delay(AO_MS_TO_TICKS(1000)); +} + static struct ao_task ao_lco_input_task; static struct ao_task ao_lco_monitor_task; static struct ao_task ao_lco_arm_warn_task; @@ -636,6 +649,7 @@ ao_lco_monitor(void) uint16_t delay; uint8_t box; + ao_lco_batt_voltage(); ao_lco_search(); ao_add_task(&ao_lco_input_task, ao_lco_input, "lco input"); ao_add_task(&ao_lco_arm_warn_task, ao_lco_arm_warn, "lco arm warn"); diff --git a/src/telelco-v2.0/ao_pins.h b/src/telelco-v2.0/ao_pins.h index 73ce9d6c..4311bac8 100644 --- a/src/telelco-v2.0/ao_pins.h +++ b/src/telelco-v2.0/ao_pins.h @@ -357,4 +357,40 @@ #define AO_BUTTON_8_PORT &stm_gpioe #define AO_BUTTON_8 13 +/* ADC */ + +struct ao_adc { + int16_t v_batt; +}; + +#define AO_ADC_DUMP(p) \ + printf("batt: %5d\n", p.v_batt) + +#define HAS_ADC_TEMP 0 +#define HAS_BATTERY_REPORT 1 + +#define AO_ADC_V_BATT 0 +#define AO_ADC_V_BATT_PORT (&stm_gpioa) +#define AO_ADC_V_BATT_PIN 0 + +#define AO_ADC_RCC_AHBENR (1 << STM_RCC_AHBENR_GPIOAEN) + +#define AO_ADC_PIN0_PORT AO_ADC_V_BATT_PORT +#define AO_ADC_PIN0_PIN AO_ADC_V_BATT_PIN + +#define AO_ADC_SQ1 AO_ADC_V_BATT + +#define AO_NUM_ADC 1 + +/* + * Voltage divider on ADC battery sampler + */ +#define AO_BATTERY_DIV_PLUS 15 /* 15k */ +#define AO_BATTERY_DIV_MINUS 27 /* 27k */ + +/* + * ADC reference in decivolts + */ +#define AO_ADC_REFERENCE_DV 33 + #endif /* _AO_PINS_H_ */ diff --git a/src/telelco-v2.0/ao_telelco.c b/src/telelco-v2.0/ao_telelco.c index 7b04d386..9693c657 100644 --- a/src/telelco-v2.0/ao_telelco.c +++ b/src/telelco-v2.0/ao_telelco.c @@ -30,6 +30,7 @@ #include #include #include +#include int main(void) @@ -45,6 +46,7 @@ main(void) ao_spi_init(); ao_dma_init(); ao_exti_init(); + ao_adc_single_init(); ao_beep_init(); ao_cmd_init(); -- cgit v1.2.3 From 16971e550d1018fb6acf3fee3c56098f5ba9921e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 19:18:48 -0700 Subject: altos/stm: Enable HSI timer when HAS_ADC_SINGLE is set The ADC unit always uses the HSI timer. Signed-off-by: Keith Packard --- src/stm/ao_timer.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/stm/ao_timer.c b/src/stm/ao_timer.c index 1576a6c9..7b526902 100644 --- a/src/stm/ao_timer.c +++ b/src/stm/ao_timer.c @@ -149,7 +149,7 @@ ao_clock_init(void) #define STM_RCC_CFGR_PLLSRC_TARGET_CLOCK (0 << STM_RCC_CFGR_PLLSRC) #endif -#if !AO_HSE || HAS_ADC +#if !AO_HSE || HAS_ADC || HAS_ADC_SINGLE /* Enable HSI RC clock 16MHz */ stm_rcc.cr |= (1 << STM_RCC_CR_HSION); while (!(stm_rcc.cr & (1 << STM_RCC_CR_HSIRDY))) -- cgit v1.2.3 From 484ffebc54048bbe276e0e2c0e2ab52c6a1761c1 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 28 May 2018 19:19:43 -0700 Subject: altos/telelco-v2.0: Set HAS_ADC_SINGLE to enable HSI timer The ADC unit requires the HSI timer to run, so make sure it's enabled for this device. Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_pins.h | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/telelco-v2.0/ao_pins.h b/src/telelco-v2.0/ao_pins.h index 4311bac8..61a8676a 100644 --- a/src/telelco-v2.0/ao_pins.h +++ b/src/telelco-v2.0/ao_pins.h @@ -366,6 +366,7 @@ struct ao_adc { #define AO_ADC_DUMP(p) \ printf("batt: %5d\n", p.v_batt) +#define HAS_ADC_SINGLE 1 #define HAS_ADC_TEMP 0 #define HAS_BATTERY_REPORT 1 -- cgit v1.2.3 From ab22c56ca02db87a84b6950c024a90b086f5ce28 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 31 May 2018 16:24:33 -0700 Subject: altos/telefireeight-v1.0: Fix ADC definitions Was missing all of the GPIO configuration bits, which probably meant the ADCs were returning garbage values. Signed-off-by: Keith Packard --- src/telefireeight-v1.0/ao_pins.h | 108 +++++++++++++++++++++++++++------------ 1 file changed, 76 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/src/telefireeight-v1.0/ao_pins.h b/src/telefireeight-v1.0/ao_pins.h index 8e3529e7..271d2cbe 100644 --- a/src/telefireeight-v1.0/ao_pins.h +++ b/src/telefireeight-v1.0/ao_pins.h @@ -189,35 +189,89 @@ #define AO_PAD_PIN_0 9 #define AO_PAD_0_PORT (&stm_gpiod) -#define AO_PAD_ADC_0 3 +#define AO_ADC_SENSE_PAD_0 3 +#define AO_ADC_SENSE_PAD_0_PORT (&stm_gpioa) +#define AO_ADC_SENSE_PAD_0_PIN 3 #define AO_PAD_PIN_1 8 #define AO_PAD_1_PORT (&stm_gpiod) -#define AO_PAD_ADC_1 2 +#define AO_ADC_SENSE_PAD_1 2 +#define AO_ADC_SENSE_PAD_1_PORT (&stm_gpioa) +#define AO_ADC_SENSE_PAD_1_PIN 2 #define AO_PAD_PIN_2 15 #define AO_PAD_2_PORT (&stm_gpiob) -#define AO_PAD_ADC_2 1 +#define AO_ADC_SENSE_PAD_2 1 +#define AO_ADC_SENSE_PAD_2_PORT (&stm_gpioa) +#define AO_ADC_SENSE_PAD_2_PIN 1 #define AO_PAD_PIN_3 14 #define AO_PAD_3_PORT (&stm_gpiob) -#define AO_PAD_ADC_3 0 +#define AO_ADC_SENSE_PAD_3 0 +#define AO_ADC_SENSE_PAD_3_PORT (&stm_gpioa) +#define AO_ADC_SENSE_PAD_3_PIN 0 #define AO_PAD_PIN_4 12 #define AO_PAD_4_PORT (&stm_gpiod) -#define AO_PAD_ADC_4 7 +#define AO_ADC_SENSE_PAD_4 7 +#define AO_ADC_SENSE_PAD_4_PORT (&stm_gpioa) +#define AO_ADC_SENSE_PAD_4_PIN 7 #define AO_PAD_PIN_5 13 #define AO_PAD_5_PORT (&stm_gpiod) -#define AO_PAD_ADC_5 6 +#define AO_ADC_SENSE_PAD_5 6 +#define AO_ADC_SENSE_PAD_5_PORT (&stm_gpioa) +#define AO_ADC_SENSE_PAD_5_PIN 6 #define AO_PAD_PIN_6 14 #define AO_PAD_6_PORT (&stm_gpiod) -#define AO_PAD_ADC_6 5 +#define AO_ADC_SENSE_PAD_6 5 +#define AO_ADC_SENSE_PAD_6_PORT (&stm_gpioa) +#define AO_ADC_SENSE_PAD_6_PIN 5 #define AO_PAD_PIN_7 15 #define AO_PAD_7_PORT (&stm_gpiod) -#define AO_PAD_ADC_7 4 +#define AO_ADC_SENSE_PAD_7 4 +#define AO_ADC_SENSE_PAD_7_PORT (&stm_gpioa) +#define AO_ADC_SENSE_PAD_7_PIN 4 + +#define AO_ADC_PYRO 8 +#define AO_ADC_PYRO_PORT (&stm_gpiob) +#define AO_ADC_PYRO_PIN 0 + +#define AO_ADC_BATT 15 +#define AO_ADC_BATT_PORT (&stm_gpioc) +#define AO_ADC_BATT_PIN 5 + +#define AO_ADC_PIN0_PORT AO_ADC_SENSE_PAD_0_PORT +#define AO_ADC_PIN0_PIN AO_ADC_SENSE_PAD_0_PIN + +#define AO_ADC_PIN1_PORT AO_ADC_SENSE_PAD_1_PORT +#define AO_ADC_PIN1_PIN AO_ADC_SENSE_PAD_1_PIN + +#define AO_ADC_PIN2_PORT AO_ADC_SENSE_PAD_2_PORT +#define AO_ADC_PIN2_PIN AO_ADC_SENSE_PAD_2_PIN + +#define AO_ADC_PIN3_PORT AO_ADC_SENSE_PAD_3_PORT +#define AO_ADC_PIN3_PIN AO_ADC_SENSE_PAD_3_PIN + +#define AO_ADC_PIN4_PORT AO_ADC_SENSE_PAD_4_PORT +#define AO_ADC_PIN4_PIN AO_ADC_SENSE_PAD_4_PIN + +#define AO_ADC_PIN5_PORT AO_ADC_SENSE_PAD_5_PORT +#define AO_ADC_PIN5_PIN AO_ADC_SENSE_PAD_5_PIN + +#define AO_ADC_PIN6_PORT AO_ADC_SENSE_PAD_6_PORT +#define AO_ADC_PIN6_PIN AO_ADC_SENSE_PAD_6_PIN + +#define AO_ADC_PIN7_PORT AO_ADC_SENSE_PAD_7_PORT +#define AO_ADC_PIN7_PIN AO_ADC_SENSE_PAD_7_PIN + +#define AO_ADC_PIN8_PORT AO_ADC_PYRO_PORT +#define AO_ADC_PIN8_PIN AO_ADC_PYRO_PIN + +#define AO_ADC_PIN9_PORT AO_ADC_BATT_PORT +#define AO_ADC_PIN9_PIN AO_ADC_BATT_PIN #define AO_PAD_ALL_CHANNELS (0xff) @@ -226,23 +280,24 @@ #define AO_PAD_NO_IGNITER 16904 #define AO_PAD_GOOD_IGNITER 22514 -#define AO_PAD_ADC_PYRO 8 -#define AO_PAD_ADC_BATT 15 - #define AO_ADC_FIRST_PIN 0 #define AO_NUM_ADC 10 -#define AO_ADC_SQ1 AO_PAD_ADC_0 -#define AO_ADC_SQ2 AO_PAD_ADC_1 -#define AO_ADC_SQ3 AO_PAD_ADC_2 -#define AO_ADC_SQ4 AO_PAD_ADC_3 -#define AO_ADC_SQ5 AO_PAD_ADC_4 -#define AO_ADC_SQ6 AO_PAD_ADC_5 -#define AO_ADC_SQ7 AO_PAD_ADC_6 -#define AO_ADC_SQ8 AO_PAD_ADC_7 -#define AO_ADC_SQ9 AO_PAD_ADC_PYRO -#define AO_ADC_SQ10 AO_PAD_ADC_BATT +#define AO_ADC_SQ1 AO_ADC_SENSE_PAD_0 +#define AO_ADC_SQ2 AO_ADC_SENSE_PAD_1 +#define AO_ADC_SQ3 AO_ADC_SENSE_PAD_2 +#define AO_ADC_SQ4 AO_ADC_SENSE_PAD_3 +#define AO_ADC_SQ5 AO_ADC_SENSE_PAD_4 +#define AO_ADC_SQ6 AO_ADC_SENSE_PAD_5 +#define AO_ADC_SQ7 AO_ADC_SENSE_PAD_6 +#define AO_ADC_SQ8 AO_ADC_SENSE_PAD_7 +#define AO_ADC_SQ9 AO_ADC_PYRO +#define AO_ADC_SQ10 AO_ADC_BATT + +#define AO_ADC_RCC_AHBENR ((1 << STM_RCC_AHBENR_GPIOAEN) | \ + (1 << STM_RCC_AHBENR_GPIOBEN) | \ + (1 << STM_RCC_AHBENR_GPIOCEN)) #define AO_PYRO_R_PYRO_SENSE 200 #define AO_PYRO_R_SENSE_GND 22 @@ -276,15 +331,4 @@ struct ao_adc { (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_4) | \ - (1 << AO_PAD_ADC_5) | \ - (1 << AO_PAD_ADC_6) | \ - (1 << AO_PAD_ADC_7) | \ - (1 << AO_PAD_ADC_PYRO) | \ - (1 << AO_PAD_ADC_BATT)) - #endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From 99642f9c4f89aa5577376a9cd118e7cff26e2762 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 31 May 2018 22:39:45 -0700 Subject: altos/telefire*: Do voltage tests in decivolts instead of ADC units This requires that each pad device specify the ADC reference voltage and all relevant voltage divider resistor values explicitly so that the ADC values can be converted to decivolts and compared with one another. This should be easier to understand in the future when reading the code. Signed-off-by: Keith Packard --- src/drivers/ao_pad.c | 128 +++++++++++++++++++++------------------ src/telefire-v0.1/ao_pins.h | 15 +++-- src/telefire-v0.2/ao_pins.h | 17 ++++-- src/telefireeight-v1.0/ao_pins.h | 17 ++++-- src/telefiretwo-v0.1/ao_pins.h | 13 ++++ 5 files changed, 118 insertions(+), 72 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c index b0ec2161..ed749d1c 100644 --- a/src/drivers/ao_pad.c +++ b/src/drivers/ao_pad.c @@ -153,13 +153,36 @@ ao_pad_run(void) #define AO_PAD_ARM_SIREN_INTERVAL 200 -#ifndef AO_PYRO_R_PYRO_SENSE -#define AO_PYRO_R_PYRO_SENSE 100 -#define AO_PYRO_R_SENSE_GND 27 -#define AO_FIRE_R_POWER_FET 100 -#define AO_FIRE_R_FET_SENSE 100 -#define AO_FIRE_R_SENSE_GND 27 -#endif +/* Resistor values needed for various voltage test ratios: + * + * Net names involved: + * + * V_BATT Battery power, after the initial power switch + * V_PYRO Pyro power, after the pyro power switch (and initial power switch) + * PYRO_SENSE ADC input to sense V_PYRO voltage + * BATT_SENSE ADC input to sense V_BATT voltage + * IGNITER FET output to pad (the other pad lead hooks to V_PYRO) + * IGNITER_SENSE ADC input to sense igniter voltage + * + * AO_PAD_R_V_BATT_BATT_SENSE Resistor from battery rail to battery sense input + * AO_PAD_R_BATT_SENSE_GND Resistor from battery sense input to ground + * + * AO_PAD_R_V_BATT_V_PYRO Resistor from battery rail to pyro rail + * AO_PAD_R_V_PYRO_PYRO_SENSE Resistor from pyro rail to pyro sense input + * AO_PAD_R_PYRO_SENSE_GND Resistor from pyro sense input to ground + * + * AO_PAD_R_V_PYRO_IGNITER Optional resistors from pyro rail to FET igniter output + * AO_PAD_R_IGNITER_IGNITER_SENSE Resistors from FET igniter output to igniter sense ADC inputs + * AO_PAD_R_IGNITER_SENSE_GND Resistors from igniter sense ADC inputs to ground + */ + +int16_t +ao_pad_decivolt(int16_t adc, int16_t r_plus, int16_t r_minus) +{ + int32_t mul = (int32_t) AO_ADC_REFERENCE_DV * (r_plus + r_minus); + int32_t div = (int32_t) AO_ADC_MAX * r_minus; + return ((int32_t) adc * mul + mul/2) / div; +} static void ao_pad_monitor(void) @@ -174,6 +197,7 @@ ao_pad_monitor(void) sample = ao_data_head; for (;;) { __pdata int16_t pyro; + ao_arch_critical( while (sample == ao_data_head) ao_sleep((void *) DATA_TO_XDATA(&ao_data_head)); @@ -183,28 +207,18 @@ ao_pad_monitor(void) packet = &ao_data_ring[sample]; sample = ao_data_ring_next(sample); - pyro = packet->adc.pyro; + /* Reply battery voltage */ + query.battery = ao_pad_decivolt(packet->adc.batt, AO_PAD_R_V_BATT_BATT_SENSE, AO_PAD_R_BATT_SENSE_GND); -#define VOLTS_TO_PYRO(x) ((int16_t) ((x) * ((1.0 * AO_PYRO_R_SENSE_GND) / \ - (1.0 * (AO_PYRO_R_SENSE_GND + AO_PYRO_R_PYRO_SENSE)) / 3.3 * AO_ADC_MAX))) + /* Current pyro voltage */ + pyro = ao_pad_decivolt(packet->adc.pyro, + AO_PAD_R_V_PYRO_PYRO_SENSE, + AO_PAD_R_PYRO_SENSE_GND); - -#define VOLTS_TO_FIRE(x) ((int16_t) ((x) * ((1.0 * AO_FIRE_R_SENSE_GND) / \ - (1.0 * (AO_FIRE_R_SENSE_GND + AO_FIRE_R_FET_SENSE)) / 3.3 * AO_ADC_MAX))) - - /* convert ADC value to voltage in tenths, then add .2 for the diode drop */ - query.battery = (packet->adc.batt + 96) / 192 + 2; cur = 0; - if (pyro > VOLTS_TO_PYRO(10)) { + if (pyro > query.battery * 7 / 8) { query.arm_status = AO_PAD_ARM_STATUS_ARMED; cur |= AO_LED_ARMED; -#if AO_FIRE_R_POWER_FET - } else if (pyro > VOLTS_TO_PYRO(5)) { - if ((ao_time() % 100) < 50) - cur |= AO_LED_ARMED; - query.arm_status = AO_PAD_ARM_STATUS_UNKNOWN; - arm_beep_time = 0; -#endif } else { query.arm_status = AO_PAD_ARM_STATUS_DISARMED; arm_beep_time = 0; @@ -217,54 +231,50 @@ ao_pad_monitor(void) cur |= AO_LED_GREEN; for (c = 0; c < AO_PAD_NUM; c++) { - int16_t sense = packet->adc.sense[c]; + int16_t sense = ao_pad_decivolt(packet->adc.sense[c], + AO_PAD_R_IGNITER_IGNITER_SENSE, + AO_PAD_R_IGNITER_SENSE_GND); uint8_t status = AO_PAD_IGNITER_STATUS_UNKNOWN; /* - * pyro is run through a divider, so pyro = v_pyro * 27 / 127 ~= v_pyro / 20 - * v_pyro = pyro * 127 / 27 - * - * v_pyro \ - * 100k igniter - * output / - * 100k \ - * sense relay - * 27k / - * gnd --- + * Here's the resistor stack on each + * igniter channel. Note that + * AO_PAD_R_V_PYRO_IGNITER is optional * - * v_pyro \ - * 200k igniter - * output / - * 200k \ - * sense relay - * 22k / - * gnd --- + * v_pyro \ + * AO_PAD_R_V_PYRO_IGNITER igniter + * output / + * AO_PAD_R_IGNITER_IGNITER_SENSE \ + * sense relay + * AO_PAD_R_IGNITER_SENSE_GND / + * gnd --- * - * If the relay is closed, then sense will be 0 - * If no igniter is present, then sense will be v_pyro * 27k/227k = pyro * 127 / 227 ~= pyro/2 - * If igniter is present, then sense will be v_pyro * 27k/127k ~= v_pyro / 20 = pyro */ -#if AO_FIRE_R_POWER_FET +#ifdef AO_PAD_R_V_PYRO_IGNITER if (sense <= pyro / 8) { + /* close to zero → relay is closed */ status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_CLOSED; if ((ao_time() % 100) < 50) cur |= AO_LED_CONTINUITY(c); - } else - if (pyro / 8 * 3 <= sense && sense <= pyro / 8 * 5) - status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN; - else if (pyro / 8 * 7 <= sense) { - status = AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN; - cur |= AO_LED_CONTINUITY(c); - } -#else - if (sense >= pyro / 8 * 5) { - status = AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN; - cur |= AO_LED_CONTINUITY(c); - } else { - status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN; } + else #endif + { + if (sense >= (pyro * 7) / 8) { + + /* sense close to pyro voltage; igniter is good + */ + status = AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN; + cur |= AO_LED_CONTINUITY(c); + } else { + + /* relay not shorted (if we can tell), + * and igniter not obviously present + */ + status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN; + } + } query.igniter_status[c] = status; } if (cur != prev) { diff --git a/src/telefire-v0.1/ao_pins.h b/src/telefire-v0.1/ao_pins.h index ee0ca716..8266589e 100644 --- a/src/telefire-v0.1/ao_pins.h +++ b/src/telefire-v0.1/ao_pins.h @@ -100,10 +100,17 @@ #define AO_STROBE_PIN 1 #define AO_STROBE P1_1 -/* 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_ADC_REFERENCE_DV 33 +#define AO_PAD_R_V_BATT_BATT_SENSE 100 +#define AO_PAD_R_BATT_SENSE_GND 27 + +#define AO_PAD_R_V_BATT_V_PYRO 100 +#define AO_PAD_R_V_PYRO_PYRO_SENSE 100 +#define AO_PAD_R_PYRO_SENSE_GND 27 + +#define AO_PAD_R_V_PYRO_IGNITER 100 +#define AO_PAD_R_IGNITER_IGNITER_SENSE 100 +#define AO_PAD_R_IGNITER_SENSE_GND 27 #define AO_PAD_ADC_PYRO 4 #define AO_PAD_ADC_BATT 5 diff --git a/src/telefire-v0.2/ao_pins.h b/src/telefire-v0.2/ao_pins.h index 4faeb65f..65f5bdcc 100644 --- a/src/telefire-v0.2/ao_pins.h +++ b/src/telefire-v0.2/ao_pins.h @@ -111,15 +111,24 @@ #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 +#define AO_ADC_REFERENCE_DV 33 +#define AO_PAD_R_V_BATT_BATT_SENSE 100 +#define AO_PAD_R_BATT_SENSE_GND 27 + +#define AO_PAD_R_V_BATT_V_PYRO 100 +#define AO_PAD_R_V_PYRO_PYRO_SENSE 100 +#define AO_PAD_R_PYRO_SENSE_GND 27 + +#define AO_PAD_R_V_PYRO_IGNITER 100 +#define AO_PAD_R_IGNITER_IGNITER_SENSE 100 +#define AO_PAD_R_IGNITER_SENSE_GND 27 + + struct ao_adc { int16_t sense[AO_PAD_NUM]; int16_t pyro; diff --git a/src/telefireeight-v1.0/ao_pins.h b/src/telefireeight-v1.0/ao_pins.h index 271d2cbe..15e1fa11 100644 --- a/src/telefireeight-v1.0/ao_pins.h +++ b/src/telefireeight-v1.0/ao_pins.h @@ -295,16 +295,23 @@ #define AO_ADC_SQ9 AO_ADC_PYRO #define AO_ADC_SQ10 AO_ADC_BATT +#define AO_ADC_REFERENCE_DV 33 + #define AO_ADC_RCC_AHBENR ((1 << STM_RCC_AHBENR_GPIOAEN) | \ (1 << STM_RCC_AHBENR_GPIOBEN) | \ (1 << STM_RCC_AHBENR_GPIOCEN)) -#define AO_PYRO_R_PYRO_SENSE 200 -#define AO_PYRO_R_SENSE_GND 22 -#define AO_FIRE_R_POWER_FET 0 -#define AO_FIRE_R_FET_SENSE 200 -#define AO_FIRE_R_SENSE_GND 22 +#define AO_PAD_R_V_BATT_BATT_SENSE 200 +#define AO_PAD_R_BATT_SENSE_GND 22 + +#define AO_PAD_R_V_BATT_V_PYRO 200 +#define AO_PAD_R_V_PYRO_PYRO_SENSE 200 +#define AO_PAD_R_PYRO_SENSE_GND 22 + +#undef AO_PAD_R_V_PYRO_IGNITER +#define AO_PAD_R_IGNITER_IGNITER_SENSE 200 +#define AO_PAD_R_IGNITER_SENSE_GND 22 #define HAS_ADC_TEMP 0 diff --git a/src/telefiretwo-v0.1/ao_pins.h b/src/telefiretwo-v0.1/ao_pins.h index 1e5c0d09..dedd0fad 100644 --- a/src/telefiretwo-v0.1/ao_pins.h +++ b/src/telefiretwo-v0.1/ao_pins.h @@ -191,6 +191,19 @@ #define HAS_ADC_TEMP 0 +#define AO_ADC_REFERENCE_DV 33 + +#define AO_PAD_R_V_BATT_BATT_SENSE 200 +#define AO_PAD_R_BATT_SENSE_GND 22 + +#define AO_PAD_R_V_BATT_V_PYRO 200 +#define AO_PAD_R_V_PYRO_PYRO_SENSE 200 +#define AO_PAD_R_PYRO_SENSE_GND 22 + +#undef AO_PAD_R_V_PYRO_IGNITER +#define AO_PAD_R_IGNITER_IGNITER_SENSE 200 +#define AO_PAD_R_IGNITER_SENSE_GND 22 + struct ao_adc { int16_t sense[AO_PAD_NUM]; int16_t pyro; -- cgit v1.2.3 From f755181fd3ec82d7644591e2ed4b99a244acfd6e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 31 May 2018 22:42:13 -0700 Subject: altos/telelco-v2.0: Test displays at power on At boot, light up all LEDS and LCD segments for a second to let the user verify that things are working correctly. Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_lco_v2.c | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) (limited to 'src') diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index 140955da..1be8ecb2 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -626,6 +626,23 @@ ao_lco_arm_warn(void) } } +/* + * Light up everything for a second at power on to let the user + * visually inspect the system for correct operation + */ +static void +ao_lco_display_test() +{ + ao_mutex_get(&ao_lco_display_mutex); + ao_seven_segment_set(AO_LCO_PAD_DIGIT, 8 | 0x10); + ao_seven_segment_set(AO_LCO_BOX_DIGIT_1, 8 | 0x10); + ao_seven_segment_set(AO_LCO_BOX_DIGIT_10, 8 | 0x10); + ao_mutex_put(&ao_lco_display_mutex); + ao_led_on(LEDS_AVAILABLE); + ao_delay(AO_MS_TO_TICKS(1000)); + ao_led_off(LEDS_AVAILABLE); +} + static void ao_lco_batt_voltage(void) { @@ -649,6 +666,7 @@ ao_lco_monitor(void) uint16_t delay; uint8_t box; + ao_lco_display_test(); ao_lco_batt_voltage(); ao_lco_search(); ao_add_task(&ao_lco_input_task, ao_lco_input, "lco input"); -- cgit v1.2.3 From 5d9881547706b49cc22b98cc5c94e01e8a7f5181 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 31 May 2018 22:45:49 -0700 Subject: altos/telelco: Remove STATIC_TEST code from stand-alone LCO boxes This command conflicts with the 'debug' command in these devices and so wasn't usable anyways. Signed-off-by: Keith Packard --- src/drivers/ao_lco_cmd.c | 11 ++++++++++- src/telelco-v0.3/ao_pins.h | 2 ++ src/telelco-v2.0/ao_pins.h | 1 + 3 files changed, 13 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/ao_lco_cmd.c b/src/drivers/ao_lco_cmd.c index 8de21fb6..dba9a76a 100644 --- a/src/drivers/ao_lco_cmd.c +++ b/src/drivers/ao_lco_cmd.c @@ -22,6 +22,10 @@ #include #include +#ifndef HAS_STATIC_TEST +#define HAS_STATIC_TEST 1 +#endif + static __pdata uint16_t lco_box; static __pdata uint8_t lco_channels; static __pdata uint16_t tick_offset; @@ -150,6 +154,7 @@ lco_fire_cmd(void) __reentrant } } +#if HAS_STATIC_TEST static void lco_static_cmd(void) __reentrant { @@ -182,6 +187,7 @@ lco_static_cmd(void) __reentrant ao_delay(AO_MS_TO_TICKS(100)); } } +#endif static void lco_arm_cmd(void) __reentrant @@ -208,18 +214,21 @@ lco_ignite_cmd(void) __reentrant } +#if HAS_STATIC_TEST static void lco_endstatic_cmd(void) __reentrant { lco_ignite(AO_PAD_ENDSTATIC); } +#endif static __code struct ao_cmds ao_lco_cmds[] = { { lco_report_cmd, "l \0Get remote status" }, { lco_fire_cmd, "F \0Fire remote igniters" }, - { lco_fire_cmd, "F \0Fire remote igniters" }, +#if HAS_STATIC_TEST { lco_static_cmd, "S \0Initiate static test" }, { lco_endstatic_cmd, "D\0End static test (and download someday)" }, +#endif { lco_arm_cmd, "a \0Arm remote igniter" }, { lco_ignite_cmd, "i \0Pulse remote igniter" }, { 0, NULL }, diff --git a/src/telelco-v0.3/ao_pins.h b/src/telelco-v0.3/ao_pins.h index dd4aaafb..6fd897a0 100644 --- a/src/telelco-v0.3/ao_pins.h +++ b/src/telelco-v0.3/ao_pins.h @@ -54,6 +54,8 @@ #define HAS_RADIO_RATE 1 #define HAS_TELEMETRY 0 #define HAS_AES 1 +#define HAS_STATIC_TEST 0 + #define HAS_SPI_1 0 #define SPI_1_PA5_PA6_PA7 0 diff --git a/src/telelco-v2.0/ao_pins.h b/src/telelco-v2.0/ao_pins.h index 61a8676a..48aeabdf 100644 --- a/src/telelco-v2.0/ao_pins.h +++ b/src/telelco-v2.0/ao_pins.h @@ -54,6 +54,7 @@ #define HAS_RADIO_RATE 1 #define HAS_TELEMETRY 0 #define HAS_AES 1 +#define HAS_STATIC_TEST 0 #define HAS_SPI_1 0 #define SPI_1_PA5_PA6_PA7 0 -- cgit v1.2.3 From 5c465b0049659246908b15be68806b1390e8ebe5 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 16 Jun 2018 23:47:15 -0700 Subject: altos/stm: Remove unused ADC names from ao_adc_single_stm.c Signed-off-by: Keith Packard --- src/stm/ao_adc_single_stm.c | 66 --------------------------------------------- 1 file changed, 66 deletions(-) (limited to 'src') diff --git a/src/stm/ao_adc_single_stm.c b/src/stm/ao_adc_single_stm.c index 907a064e..8a7fda4a 100644 --- a/src/stm/ao_adc_single_stm.c +++ b/src/stm/ao_adc_single_stm.c @@ -87,72 +87,6 @@ ao_adc_single_get(struct ao_adc *packet) ao_arch_release_interrupts(); } -#ifdef AO_ADC_SQ1_NAME -static const char *ao_adc_name[AO_NUM_ADC] = { - AO_ADC_SQ1_NAME, -#ifdef AO_ADC_SQ2_NAME - AO_ADC_SQ2_NAME, -#endif -#ifdef AO_ADC_SQ3_NAME - AO_ADC_SQ3_NAME, -#endif -#ifdef AO_ADC_SQ4_NAME - AO_ADC_SQ4_NAME, -#endif -#ifdef AO_ADC_SQ5_NAME - AO_ADC_SQ5_NAME, -#endif -#ifdef AO_ADC_SQ6_NAME - AO_ADC_SQ6_NAME, -#endif -#ifdef AO_ADC_SQ7_NAME - AO_ADC_SQ7_NAME, -#endif -#ifdef AO_ADC_SQ8_NAME - AO_ADC_SQ8_NAME, -#endif -#ifdef AO_ADC_SQ9_NAME - AO_ADC_SQ9_NAME, -#endif -#ifdef AO_ADC_SQ10_NAME - AO_ADC_SQ10_NAME, -#endif -#ifdef AO_ADC_SQ11_NAME - AO_ADC_SQ11_NAME, -#endif -#ifdef AO_ADC_SQ12_NAME - AO_ADC_SQ12_NAME, -#endif -#ifdef AO_ADC_SQ13_NAME - AO_ADC_SQ13_NAME, -#endif -#ifdef AO_ADC_SQ14_NAME - AO_ADC_SQ14_NAME, -#endif -#ifdef AO_ADC_SQ15_NAME - AO_ADC_SQ15_NAME, -#endif -#ifdef AO_ADC_SQ16_NAME - AO_ADC_SQ16_NAME, -#endif -#ifdef AO_ADC_SQ17_NAME - AO_ADC_SQ17_NAME, -#endif -#ifdef AO_ADC_SQ18_NAME - AO_ADC_SQ18_NAME, -#endif -#ifdef AO_ADC_SQ19_NAME - AO_ADC_SQ19_NAME, -#endif -#ifdef AO_ADC_SQ20_NAME - AO_ADC_SQ20_NAME, -#endif -#ifdef AO_ADC_SQ21_NAME - #error "too many ADC names" -#endif -}; -#endif - static void ao_adc_dump(void) { -- cgit v1.2.3 From 7821e341a7cf55e32b926eb4474220dabb6eeb81 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 16 Jun 2018 23:47:42 -0700 Subject: altos/stm: Define ADC channels for TEMP and V_REF These are fixed at 16 and 17. Signed-off-by: Keith Packard --- src/stm/stm32l.h | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src') diff --git a/src/stm/stm32l.h b/src/stm/stm32l.h index 201f4f36..1da817e7 100644 --- a/src/stm/stm32l.h +++ b/src/stm/stm32l.h @@ -1454,6 +1454,9 @@ struct stm_adc { extern struct stm_adc stm_adc; +#define STM_ADC_SQ_TEMP 16 +#define STM_ADC_SQ_V_REF 17 + #define STM_ADC_SR_JCNR 9 #define STM_ADC_SR_RCNR 8 #define STM_ADC_SR_ADONS 6 -- cgit v1.2.3 From abac6c59d6b004d52a7b782865d2d987732cd61e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 17 Jun 2018 00:17:29 -0700 Subject: altos/telelco-v0: Test display, show batt voltage at boot Just like telelco v2 Signed-off-by: Keith Packard --- src/drivers/ao_lco.c | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) (limited to 'src') diff --git a/src/drivers/ao_lco.c b/src/drivers/ao_lco.c index e1806ca3..91a0593f 100644 --- a/src/drivers/ao_lco.c +++ b/src/drivers/ao_lco.c @@ -23,6 +23,9 @@ #include #include #include +#if HAS_ADC_SINGLE +#include +#endif #define DEBUG 1 @@ -640,6 +643,37 @@ ao_lco_arm_warn(void) } } +/* + * Light up everything for a second at power on to let the user + * visually inspect the system for correct operation + */ +static void +ao_lco_display_test() +{ + ao_mutex_get(&ao_lco_display_mutex); + ao_seven_segment_set(AO_LCO_PAD_DIGIT, 8 | 0x10); + ao_seven_segment_set(AO_LCO_BOX_DIGIT_1, 8 | 0x10); + ao_seven_segment_set(AO_LCO_BOX_DIGIT_10, 8 | 0x10); + ao_mutex_put(&ao_lco_display_mutex); + ao_led_on(LEDS_AVAILABLE); + ao_delay(AO_MS_TO_TICKS(1000)); + ao_led_off(LEDS_AVAILABLE); +} + +#if HAS_ADC_SINGLE +static void +ao_lco_batt_voltage(void) +{ + struct ao_adc packet; + int16_t decivolt; + + ao_adc_single_get(&packet); + decivolt = ao_battery_decivolt(packet.v_batt); + ao_lco_set_voltage(decivolt); + ao_delay(AO_MS_TO_TICKS(1000)); +} +#endif + static struct ao_task ao_lco_input_task; static struct ao_task ao_lco_monitor_task; static struct ao_task ao_lco_arm_warn_task; @@ -651,6 +685,10 @@ ao_lco_monitor(void) uint16_t delay; uint8_t box; + ao_lco_display_test(); +#if HAS_ADC_SINGLE + ao_lco_batt_voltage(); +#endif ao_lco_search(); ao_add_task(&ao_lco_input_task, ao_lco_input, "lco input"); ao_add_task(&ao_lco_arm_warn_task, ao_lco_arm_warn, "lco arm warn"); -- cgit v1.2.3 From 16482bf5d63eafc5256f14a218d3a374f6584e0a Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 16 Jun 2018 23:51:26 -0700 Subject: altos/driver: Improve quadrature debouncing for mechanical encoders Track state of encoder better and only bump values when the full motion is detected (all four states). Signed-off-by: Keith Packard --- src/drivers/ao_quadrature.c | 81 +++++++++++++++++++++++++++------------------ src/telelco-v0.2/ao_pins.h | 1 + src/telelco-v0.3/ao_pins.h | 1 + 3 files changed, 51 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index 98bd7c08..59e84518 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -23,6 +23,10 @@ #include __xdata int32_t ao_quadrature_count[AO_QUADRATURE_COUNT]; +#ifndef AO_QUADRATURE_SINGLE_CODE +static int8_t ao_quadrature_step[AO_QUADRATURE_COUNT]; +#endif + static uint8_t ao_quadrature_state[AO_QUADRATURE_COUNT]; struct ao_debounce { @@ -40,7 +44,7 @@ static struct ao_debounce ao_debounce_state[AO_QUADRATURE_COUNT][2]; #define isr(q) ao_quadrature_isr_ ## q #ifndef AO_QUADRATURE_DEBOUNCE -#define AO_QUADRATURE_DEBOUNCE 30 +#error must define AO_QUADRATURE_DEBOUNCE #endif static uint8_t @@ -75,8 +79,21 @@ ao_quadrature_read(struct stm_gpio *gpio, uint8_t pin_a, uint8_t pin_b, struct a #define _ao_quadrature_get(q) ao_quadrature_read(port(q), bita(q), bitb(q), ao_debounce_state[q]) static void -_ao_quadrature_queue(uint8_t q, int8_t step) +_ao_quadrature_step(uint8_t q, int8_t step) { +#ifndef AO_QUADRATURE_SINGLE_CODE + ao_quadrature_step[q] += step; + if (ao_quadrature_state[q] != 0) + return; + if (ao_quadrature_step[q] >= 4) { + ao_quadrature_step[q] = 0; + step = 1; + } else if (ao_quadrature_step[q] <= -4) { + ao_quadrature_step[q] = 0; + step = -1; + } else + return; +#endif ao_quadrature_count[q] += step; #if AO_EVENT ao_event_put_isr(AO_EVENT_QUADRATURE, q, step); @@ -84,39 +101,28 @@ _ao_quadrature_queue(uint8_t q, int8_t step) ao_wakeup(&ao_quadrature_count[q]); } -#if AO_QUADRATURE_SINGLE_CODE -struct ao_quadrature_step { - uint8_t inc; - uint8_t dec; +static const struct { + uint8_t prev, next; +} ao_quadrature_steps[4] = { + [0] { .prev = 2, .next = 1 }, + [1] { .prev = 0, .next = 3 }, + [3] { .prev = 1, .next = 2 }, + [2] { .prev = 3, .next = 0 }, }; -static struct ao_quadrature_step ao_quadrature_steps[4] = { - [0] = { .inc = 1, .dec = 2 }, - [1] = { .inc = 3, .dec = 0 }, - [3] = { .inc = 2, .dec = 1 }, - [2] = { .inc = 0, .dec = 3 }, -}; -#endif - static void _ao_quadrature_set(uint8_t q, uint8_t new) { - uint8_t old = ao_quadrature_state[q]; + uint8_t old; -#ifdef AO_QUADRATURE_SINGLE_CODE - if (new == ao_quadrature_steps[old].inc) { - _ao_quadrature_queue(q, 1); - } else if (new == ao_quadrature_steps[old].dec) { - _ao_quadrature_queue(q, -1); - } -#else - if (old != new && new == 0) { - if (old == 2) - _ao_quadrature_queue(q, 1); - else if (old == 1) - _ao_quadrature_queue(q, -1); - } -#endif + ao_arch_block_interrupts(); + old = ao_quadrature_state[q]; ao_quadrature_state[q] = new; + ao_arch_release_interrupts(); + + if (new == ao_quadrature_steps[old].next) + _ao_quadrature_step(q, 1); + else if (new == ao_quadrature_steps[old].prev) + _ao_quadrature_step(q, -1); } static void @@ -151,21 +157,32 @@ ao_quadrature_test(void) uint8_t q; int32_t c; uint8_t s; +#ifndef AO_QUADRATURE_SINGLE_CODE + int8_t t = 0; +#endif ao_cmd_decimal(); q = ao_cmd_lex_i; - if (q >= AO_QUADRATURE_COUNT) { + if (q >= AO_QUADRATURE_COUNT) ao_cmd_status = ao_cmd_syntax_error; + if (ao_cmd_status != ao_cmd_success) return; - } c = -10000; s = 0; while (ao_quadrature_count[q] != 10) { if (ao_quadrature_count[q] != c || - ao_quadrature_state[q] != s) { +#ifndef AO_QUADRATURE_SINGLE_CODE + ao_quadrature_step[q] != t || +#endif + ao_quadrature_state[q] != s) + { c = ao_quadrature_count[q]; s = ao_quadrature_state[q]; +#ifndef AO_QUADRATURE_SINGLE_CODE + t = ao_quadrature_step[q]; + printf("step %3d ", t); +#endif printf ("count %3d state %2x\n", c, s); flush(); } diff --git a/src/telelco-v0.2/ao_pins.h b/src/telelco-v0.2/ao_pins.h index b90fca8b..f0da4fe4 100644 --- a/src/telelco-v0.2/ao_pins.h +++ b/src/telelco-v0.2/ao_pins.h @@ -253,6 +253,7 @@ #define AO_QUADRATURE_1_B 0 #define AO_QUADRATURE_BOX 1 +#define AO_QUADRATURE_DEBOUNCE 10 /* * Buttons diff --git a/src/telelco-v0.3/ao_pins.h b/src/telelco-v0.3/ao_pins.h index 6fd897a0..6023739c 100644 --- a/src/telelco-v0.3/ao_pins.h +++ b/src/telelco-v0.3/ao_pins.h @@ -252,6 +252,7 @@ #define AO_QUADRATURE_1_B 0 #define AO_QUADRATURE_BOX 1 +#define AO_QUADRATURE_DEBOUNCE 10 /* * Buttons -- cgit v1.2.3 From 291bcd751991960ae3dad8c6523323fdcbafa16d Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 16 Jun 2018 23:50:11 -0700 Subject: altos/telelco-v0.2-cc1200: Add build for NAR hacked TeleLCO v0.2 board This has the CC1120 replaced with a CC1200, but with 32MHz xtals instead of 40MHz on the radio chip. Signed-off-by: Keith Packard --- src/Makefile | 1 + src/telelco-v0.2-cc1200/Makefile | 108 ++++++++ src/telelco-v0.2-cc1200/ao_pins.h | 324 ++++++++++++++++++++++++ src/telelco-v0.2-cc1200/ao_telelco.c | 79 ++++++ src/telelco-v0.2-cc1200/flash-loader/.gitignore | 2 + src/telelco-v0.2-cc1200/flash-loader/Makefile | 8 + src/telelco-v0.2-cc1200/flash-loader/ao_pins.h | 35 +++ 7 files changed, 557 insertions(+) create mode 100644 src/telelco-v0.2-cc1200/Makefile create mode 100644 src/telelco-v0.2-cc1200/ao_pins.h create mode 100644 src/telelco-v0.2-cc1200/ao_telelco.c create mode 100644 src/telelco-v0.2-cc1200/flash-loader/.gitignore create mode 100644 src/telelco-v0.2-cc1200/flash-loader/Makefile create mode 100644 src/telelco-v0.2-cc1200/flash-loader/ao_pins.h (limited to 'src') diff --git a/src/Makefile b/src/Makefile index e8008128..6bbb2a38 100644 --- a/src/Makefile +++ b/src/Makefile @@ -37,6 +37,7 @@ ARMM3DIRS=\ telegps-v1.0 telegps-v1.0/flash-loader \ telegps-v2.0 telegps-v2.0/flash-loader \ telelco-v0.2 telelco-v0.2/flash-loader \ + telelco-v0.2-cc1200 telelco-v0.2-cc1200/flash-loader \ telelco-v0.3 telelco-v0.3/flash-loader \ teledongle-v3.0 teledongle-v3.0/flash-loader \ teleballoon-v2.0 \ diff --git a/src/telelco-v0.2-cc1200/Makefile b/src/telelco-v0.2-cc1200/Makefile new file mode 100644 index 00000000..f29e2930 --- /dev/null +++ b/src/telelco-v0.2-cc1200/Makefile @@ -0,0 +1,108 @@ +# +# AltOS build for TeleLCO +# +# + +include ../stm/Makefile.defs + +INC = \ + ao.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_boot.h \ + ao_companion.h \ + ao_data.h \ + ao_sample.h \ + ao_pins.h \ + ao_product.h \ + ao_seven_segment.h \ + ao_lco.h \ + ao_lco_cmd.h \ + ao_lco_func.h \ + ao_radio_spi.h \ + ao_radio_cmac.h \ + ao_cc1200_CC1200.h \ + ao_cc1200.h \ + stm32l.h + +# +# Common AltOS sources +# + +#PROFILE=ao_profile.c +#PROFILE_DEF=-DAO_PROFILE=1 + +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_freq.c \ + ao_dma_stm.c \ + ao_spi_stm.c \ + ao_beep_stm.c \ + ao_eeprom_stm.c \ + ao_adc_single_stm.c \ + ao_convert_volt.c \ + ao_fast_timer.c \ + ao_lcd_stm.c \ + ao_usb_stm.c \ + ao_exti_stm.c \ + ao_cc1200.c \ + ao_radio_cmac.c \ + ao_aes.c \ + ao_aes_tables.c \ + ao_fec_tx.c \ + ao_fec_rx.c \ + ao_seven_segment.c \ + ao_quadrature.c \ + ao_button.c \ + ao_event.c \ + ao_lco.c \ + ao_lco_cmd.c \ + ao_lco_func.c \ + ao_radio_cmac_cmd.c + +PRODUCT=TeleLCO-v0.2 +PRODUCT_DEF=-DTELELCO +IDPRODUCT=0x0023 + +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) $(HEX) + +$(PROG): Makefile $(OBJ) altos.ld + $(call quiet,CC) $(LDFLAGS) $(CFLAGS) -o $(PROG) $(OBJ) $(LIBS) + +../altitude.h: make-altitude + 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/telelco-v0.2-cc1200/ao_pins.h b/src/telelco-v0.2-cc1200/ao_pins.h new file mode 100644 index 00000000..bc325e06 --- /dev/null +++ b/src/telelco-v0.2-cc1200/ao_pins.h @@ -0,0 +1,324 @@ +/* + * Copyright © 2012 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +/* 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) + +#define AO_CC1200_FOSC 32000000 + +/* 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_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 +#define HAS_RADIO_RATE 1 +#define HAS_TELEMETRY 0 +#define HAS_AES 1 +#define HAS_STATIC_TEST 0 + + +#define HAS_SPI_1 0 +#define SPI_1_PA5_PA6_PA7 0 +#define SPI_1_PB3_PB4_PB5 0 +#define SPI_1_PE13_PE14_PE15 0 + +#define HAS_SPI_2 1 /* CC1120 */ +#define SPI_2_PB13_PB14_PB15 0 +#define SPI_2_PD1_PD3_PD4 1 +#define SPI_2_GPIO (&stm_gpiod) +#define SPI_2_SCK 1 +#define SPI_2_MISO 3 +#define SPI_2_MOSI 4 +#define SPI_2_OSPEEDR STM_OSPEEDR_10MHz + +#define HAS_I2C_1 0 + +#define HAS_I2C_2 0 + +#define PACKET_HAS_SLAVE 0 +#define PACKET_HAS_MASTER 0 + +#define FAST_TIMER_FREQ 10000 /* .1ms for debouncing */ + +/* + * Radio is a cc1200 connected via SPI + */ + +#define AO_RADIO_CAL_DEFAULT 5695733 + +#define AO_FEC_DEBUG 0 +#define AO_CC1200_SPI_CS_PORT (&stm_gpiod) +#define AO_CC1200_SPI_CS_PIN 0 +#define AO_CC1200_SPI_BUS AO_SPI_2_PD1_PD3_PD4 +#define AO_CC1200_SPI stm_spi2 +#define AO_CC1200_SPI_SPEED AO_SPI_SPEED_FAST + +#define AO_CC1200_INT_PORT (&stm_gpioc) +#define AO_CC1200_INT_PIN (15) + +#define AO_CC1200_INT_GPIO 2 +#define AO_CC1200_INT_GPIO_IOCFG CC1200_IOCFG2 + +#define LOW_LEVEL_DEBUG 0 + +#define LED_PORT_ENABLE STM_RCC_AHBENR_GPIOCEN +#define LED_PORT (&stm_gpioc) +#define LED_PIN_RED 7 +#define LED_PIN_AMBER 8 +#define LED_PIN_GREEN 9 +#define LED_PIN_CONTINUITY_3 10 +#define LED_PIN_CONTINUITY_2 11 +#define LED_PIN_CONTINUITY_1 12 +#define LED_PIN_CONTINUITY_0 13 +#define LED_PIN_REMOTE_ARM 14 +#define AO_LED_RED (1 << LED_PIN_RED) +#define AO_LED_AMBER (1 << LED_PIN_AMBER) +#define AO_LED_GREEN (1 << LED_PIN_GREEN) +#define AO_LED_CONTINUITY_3 (1 << LED_PIN_CONTINUITY_3) +#define AO_LED_CONTINUITY_2 (1 << LED_PIN_CONTINUITY_2) +#define AO_LED_CONTINUITY_1 (1 << LED_PIN_CONTINUITY_1) +#define AO_LED_CONTINUITY_0 (1 << LED_PIN_CONTINUITY_0) + +#define AO_LED_CONTINUITY_NUM 4 + +#define AO_LED_REMOTE_ARM (1 << LED_PIN_REMOTE_ARM) + +#define LEDS_AVAILABLE (AO_LED_RED | \ + AO_LED_AMBER | \ + AO_LED_GREEN | \ + AO_LED_CONTINUITY_3 | \ + AO_LED_CONTINUITY_2 | \ + AO_LED_CONTINUITY_1 | \ + AO_LED_CONTINUITY_0 | \ + AO_LED_REMOTE_ARM) + +/* LCD displays */ + +#define LCD_DEBUG 0 +#define SEVEN_SEGMENT_DEBUG 0 + +#define AO_LCD_STM_SEG_ENABLED_0 ( \ + (1 << 0) | /* PA1 */ \ + (1 << 1) | /* PA2 */ \ + (1 << 2) | /* PA3 */ \ + (1 << 3) | /* PA6 */ \ + (1 << 4) | /* PA7 */ \ + (1 << 5) | /* PB0 */ \ + (1 << 6) | /* PB1 */ \ + (1 << 7) | /* PB3 */ \ + (1 << 8) | /* PB4 */ \ + (1 << 9) | /* PB5 */ \ + (1 << 10) | /* PB10 */ \ + (1 << 11) | /* PB11 */ \ + (1 << 12) | /* PB12 */ \ + (1 << 13) | /* PB13 */ \ + (1 << 14) | /* PB14 */ \ + (1 << 15) | /* PB15 */ \ + (1 << 16) | /* PB8 */ \ + (1 << 17) | /* PA15 */ \ + (1 << 18) | /* PC0 */ \ + (1 << 19) | /* PC1 */ \ + (1 << 20) | /* PC2 */ \ + (1 << 21) | /* PC3 */ \ + (1 << 22) | /* PC4 */ \ + (1 << 23) | /* PC5 */ \ + (0 << 24) | /* PC6 */ \ + (0 << 25) | /* PC7 */ \ + (0 << 26) | /* PC8 */ \ + (0 << 27) | /* PC9 */ \ + (0 << 28) | /* PC10 or PD8 */ \ + (0 << 29) | /* PC11 or PD9 */ \ + (0 << 30) | /* PC12 or PD10 */ \ + (0 << 31)) /* PD2 or PD11 */ + +#define AO_LCD_STM_SEG_ENABLED_1 ( \ + (0 << 0) | /* PD12 */ \ + (0 << 1) | /* PD13 */ \ + (0 << 2) | /* PD14 */ \ + (0 << 3) | /* PD15 */ \ + (0 << 4) | /* PE0 */ \ + (0 << 5) | /* PE1 */ \ + (0 << 6) | /* PE2 */ \ + (0 << 7)) /* PE3 */ + +#define AO_LCD_STM_COM_ENABLED ( \ + (1 << 0) | /* PA8 */ \ + (0 << 1) | /* PA9 */ \ + (0 << 2) | /* PA10 */ \ + (0 << 3) | /* PB9 */ \ + (0 << 4) | /* PC10 */ \ + (0 << 5) | /* PC11 */ \ + (0 << 6)) /* PC12 */ + +#define AO_LCD_28_ON_C 0 + +#define AO_LCD_DUTY STM_LCD_CR_DUTY_STATIC + +#define AO_LCD_PER_DIGIT 1 + +#define AO_LCD_DIGITS 3 +#define AO_LCD_SEGMENTS 8 + +#define AO_SEGMENT_MAP { \ + /* pad segments */ \ + { 0, 14 }, \ + { 0, 13 }, \ + { 0, 15 }, \ + { 0, 17 }, \ + { 0, 16 }, \ + { 0, 8 }, \ + { 0, 9 }, \ + { 0, 7 }, \ + /* box1 segments */ \ + { 0, 10 }, \ + { 0, 6 }, \ + { 0, 11 }, \ + { 0, 12 }, \ + { 0, 21 }, \ + { 0, 19 }, \ + { 0, 20 }, \ + { 0, 18 }, \ + /* box0 segments */ \ + { 0, 22 }, \ + { 0, 4 }, \ + { 0, 23 }, \ + { 0, 5 }, \ + { 0, 3 }, \ + { 0, 1 }, \ + { 0, 2 }, \ + { 0, 0 }, \ +} + +/* + * Use event queue for input devices + */ + +#define AO_EVENT 1 + +/* + * Knobs + */ + +#define AO_QUADRATURE_COUNT 2 + +#define AO_QUADRATURE_0_PORT &stm_gpioe +#define AO_QUADRATURE_0_A 3 +#define AO_QUADRATURE_0_B 2 + +#define AO_QUADRATURE_PAD 0 + +#define AO_QUADRATURE_1_PORT &stm_gpioe +#define AO_QUADRATURE_1_A 1 +#define AO_QUADRATURE_1_B 0 + +#define AO_QUADRATURE_BOX 1 +#define AO_QUADRATURE_DEBOUNCE 10 + +/* + * Buttons + */ + +#define AO_BUTTON_COUNT 2 +#define AO_BUTTON_MODE AO_EXTI_MODE_PULL_UP + +#define AO_BUTTON_0_PORT &stm_gpioe +#define AO_BUTTON_0 4 + +#define AO_BUTTON_ARM 0 + +#define AO_BUTTON_1_PORT &stm_gpioe +#define AO_BUTTON_1 5 + +#define AO_BUTTON_FIRE 1 + +/* ADC */ + +#define HAS_ADC_SINGLE 1 +#define HAS_BATTERY_REPORT 1 + +struct ao_adc { + int16_t v_batt; + int16_t temp; + int16_t v_ref; +}; + +#if HAS_ADC_SINGLE +#define AO_ADC_DUMP(p) \ + printf("batt: %5d temp: %5d v_ref: %5d\n", \ + (p)->v_batt, (p)->temp, (p)->v_ref) +#endif +#if HAS_ADC +#define AO_ADC_DUMP(p) \ + printf("%5d: batt: %5d temp %5d v_ref %5d\n", \ + (p)->tick, (p)->adc.v_batt, (p)->adc.temp, (p)->adc.v_ref) +#endif + +#define HAS_ADC_TEMP 1 + +#define AO_ADC_V_BATT 0 +#define AO_ADC_V_BATT_PORT (&stm_gpioa) +#define AO_ADC_V_BATT_PIN 0 + +#define AO_ADC_RCC_AHBENR (1 << STM_RCC_AHBENR_GPIOAEN) + +#define AO_ADC_PIN0_PORT AO_ADC_V_BATT_PORT +#define AO_ADC_PIN0_PIN AO_ADC_V_BATT_PIN + +#define AO_ADC_SQ1 AO_ADC_V_BATT +#define AO_ADC_SQ2 STM_ADC_SQ_TEMP +#define AO_ADC_SQ3 STM_ADC_SQ_V_REF + +#define AO_NUM_ADC 3 + +/* + * Voltage divider on ADC battery sampler + */ +#define AO_BATTERY_DIV_PLUS 15 /* 15k */ +#define AO_BATTERY_DIV_MINUS 27 /* 27k */ + +/* + * ADC reference in decivolts + */ +#define AO_ADC_REFERENCE_DV 33 + +#endif /* _AO_PINS_H_ */ diff --git a/src/telelco-v0.2-cc1200/ao_telelco.c b/src/telelco-v0.2-cc1200/ao_telelco.c new file mode 100644 index 00000000..3266da00 --- /dev/null +++ b/src/telelco-v0.2-cc1200/ao_telelco.c @@ -0,0 +1,79 @@ +/* + * Copyright © 2011 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if HAS_ADC_SINGLE +#include +#endif +#if HAS_ADC +#include +#endif + +int +main(void) +{ + ao_clock_init(); + ao_task_init(); + ao_timer_init(); + ao_dma_init(); + + ao_led_init(LEDS_AVAILABLE); + ao_led_on(AO_LED_GREEN); + + + ao_spi_init(); + ao_exti_init(); + + ao_beep_init(); + ao_cmd_init(); + + ao_lcd_stm_init(); + ao_seven_segment_init(); + ao_quadrature_init(); + ao_button_init(); + + ao_adc_single_init(); + + ao_eeprom_init(); + + ao_radio_init(); + + ao_usb_init(); + + ao_config_init(); + + ao_lco_init(); + ao_lco_cmd_init(); +// ao_radio_cmac_cmd_init(); + + ao_start_scheduler(); + return 0; +} diff --git a/src/telelco-v0.2-cc1200/flash-loader/.gitignore b/src/telelco-v0.2-cc1200/flash-loader/.gitignore new file mode 100644 index 00000000..a32ec26e --- /dev/null +++ b/src/telelco-v0.2-cc1200/flash-loader/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +telelco*.elf diff --git a/src/telelco-v0.2-cc1200/flash-loader/Makefile b/src/telelco-v0.2-cc1200/flash-loader/Makefile new file mode 100644 index 00000000..679e61ba --- /dev/null +++ b/src/telelco-v0.2-cc1200/flash-loader/Makefile @@ -0,0 +1,8 @@ +# +# AltOS flash loader build +# +# + +TOPDIR=../.. +HARDWARE=telelco-v0.2 +include $(TOPDIR)/stm/Makefile-flash.defs diff --git a/src/telelco-v0.2-cc1200/flash-loader/ao_pins.h b/src/telelco-v0.2-cc1200/flash-loader/ao_pins.h new file mode 100644 index 00000000..72f840c0 --- /dev/null +++ b/src/telelco-v0.2-cc1200/flash-loader/ao_pins.h @@ -0,0 +1,35 @@ +/* + * Copyright © 2013 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +/* External crystal at 8MHz */ +#define AO_HSE 8000000 + +#include + +/* Arm switch. Press at power on to get boot loader */ + +#define AO_BOOT_PIN 1 +#define AO_BOOT_APPLICATION_GPIO stm_gpioe +#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_ */ -- cgit v1.2.3 From 290506129fb6cb664da3a5e3ca450a0dcdff0398 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 17 Jun 2018 01:20:01 -0700 Subject: altos/telelco-v2.0: Fix AO_ADC_DUMP Was printing p.v_batt instead of (p)->v_batt. I don't know why the former even worked. Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_pins.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/telelco-v2.0/ao_pins.h b/src/telelco-v2.0/ao_pins.h index 48aeabdf..95998dc7 100644 --- a/src/telelco-v2.0/ao_pins.h +++ b/src/telelco-v2.0/ao_pins.h @@ -365,7 +365,7 @@ struct ao_adc { }; #define AO_ADC_DUMP(p) \ - printf("batt: %5d\n", p.v_batt) + printf("batt: %5d\n", (p)->v_batt) #define HAS_ADC_SINGLE 1 #define HAS_ADC_TEMP 0 -- cgit v1.2.3 From 42ee832368f6ff796444823d53935d824acd329a Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 17 Jun 2018 12:28:51 -0700 Subject: altos/stm: Let products override AO_LED_TYPE for stm processors No need requiring it to be uint16_t Signed-off-by: Keith Packard --- src/stm/ao_arch.h | 2 -- 1 file changed, 2 deletions(-) (limited to 'src') diff --git a/src/stm/ao_arch.h b/src/stm/ao_arch.h index ecf1c1a7..679dba44 100644 --- a/src/stm/ao_arch.h +++ b/src/stm/ao_arch.h @@ -30,8 +30,6 @@ #define AO_STACK_SIZE 512 #endif -#define AO_LED_TYPE uint16_t - #ifndef AO_TICK_TYPE #define AO_TICK_TYPE uint16_t #define AO_TICK_SIGNED int16_t -- cgit v1.2.3 From 731ba39a22719a8c431eb63895cd8ba553975114 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 17 Jun 2018 12:29:36 -0700 Subject: altos/ao_pad: Use AO_LED_TYPE for computing active LEDs. instead of uint8_t Signed-off-by: Keith Packard --- src/drivers/ao_pad.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c index ed749d1c..499b5238 100644 --- a/src/drivers/ao_pad.c +++ b/src/drivers/ao_pad.c @@ -189,7 +189,7 @@ ao_pad_monitor(void) { uint8_t c; uint8_t sample; - __pdata uint8_t prev = 0, cur = 0; + __pdata AO_LED_TYPE prev = 0, cur = 0; __pdata uint8_t beeping = 0; __xdata volatile struct ao_data *packet; __pdata uint16_t arm_beep_time = 0; -- cgit v1.2.3 From cef5a5725e3bf2c17965599bd76eb93cb05a0f80 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 17 Jun 2018 12:30:09 -0700 Subject: altos/ao_pad: Turn on all LEDs for a moment at power up Allow validation of all LED connections. Signed-off-by: Keith Packard --- src/drivers/ao_pad.c | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c index 499b5238..d2b22b95 100644 --- a/src/drivers/ao_pad.c +++ b/src/drivers/ao_pad.c @@ -370,6 +370,8 @@ ao_pad(void) int8_t ret; ao_pad_box = 0; + ao_led_set(LEDS_AVAILABLE); + ao_delay(AO_MS_TO_TICKS(500)); ao_led_set(0); for (;;) { FLUSHD(); -- cgit v1.2.3 From 9ece56e3ad76af6ae623ad7a6782c0c933839be8 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 17 Jun 2018 12:35:23 -0700 Subject: altos/ao_pad: Move LED test to the monitor task It sets the LEDs, so it's best to not start doing that until we've tested them. Signed-off-by: Keith Packard --- src/drivers/ao_pad.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_pad.c b/src/drivers/ao_pad.c index d2b22b95..c6efc311 100644 --- a/src/drivers/ao_pad.c +++ b/src/drivers/ao_pad.c @@ -195,6 +195,9 @@ ao_pad_monitor(void) __pdata uint16_t arm_beep_time = 0; sample = ao_data_head; + ao_led_set(LEDS_AVAILABLE); + ao_delay(AO_MS_TO_TICKS(1000)); + ao_led_set(0); for (;;) { __pdata int16_t pyro; @@ -370,9 +373,6 @@ ao_pad(void) int8_t ret; ao_pad_box = 0; - ao_led_set(LEDS_AVAILABLE); - ao_delay(AO_MS_TO_TICKS(500)); - ao_led_set(0); for (;;) { FLUSHD(); while (ao_pad_disabled) -- cgit v1.2.3 From 655fd8e1490b70061cd81edf1d019e0469843688 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 18 Jun 2018 10:53:35 -0700 Subject: altos/telelco-v2.0: Do all drag race beeping from the beeping thread Instead of starting the beep from the calling thread, just update some state and poke the beeping thread. Signed-off-by: Keith Packard --- src/telelco-v2.0/ao_lco_v2.c | 122 +++++++++++++++++++++---------------------- 1 file changed, 60 insertions(+), 62 deletions(-) (limited to 'src') diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index 1be8ecb2..21b2c54d 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -186,7 +186,6 @@ ao_lco_set_select(void) } static struct ao_task ao_lco_drag_task; -static uint8_t ao_lco_drag_active; static uint8_t ao_lco_drag_beep_count; static uint8_t ao_lco_drag_beep_on; static uint16_t ao_lco_drag_beep_time; @@ -195,37 +194,21 @@ static uint16_t ao_lco_drag_warn_time; #define AO_LCO_DRAG_BEEP_TIME AO_MS_TO_TICKS(50) #define AO_LCO_DRAG_WARN_TIME AO_SEC_TO_TICKS(5) +/* Request 'beeps' additional drag race beeps */ static void -ao_lco_drag_beep_start(void) -{ - ao_beep(AO_BEEP_HIGH); - PRINTD("beep start\n"); - ao_lco_drag_beep_on = 1; - ao_lco_drag_beep_time = ao_time() + AO_LCO_DRAG_BEEP_TIME; -} - -static void -ao_lco_drag_beep_stop(void) -{ - ao_beep(0); - PRINTD("beep stop\n"); - ao_lco_drag_beep_on = 0; - if (ao_lco_drag_beep_count) { - --ao_lco_drag_beep_count; - if (ao_lco_drag_beep_count) - ao_lco_drag_beep_time = ao_time() + AO_LCO_DRAG_BEEP_TIME; - } -} - -static void -ao_lco_drag_beep(uint8_t beeps) +ao_lco_drag_add_beeps(uint8_t beeps) { PRINTD("beep %d\n", beeps); - if (!ao_lco_drag_beep_count) - ao_lco_drag_beep_start(); + if (ao_lco_drag_beep_count == 0) + ao_lco_drag_beep_time = ao_time(); ao_lco_drag_beep_count += beeps; + ao_wakeup(&ao_lco_drag_beep_count); } +/* Check whether it's time to change the beeper status, then either + * turn it on or off as necessary and bump the remaining beep counts + */ + static uint16_t ao_lco_drag_beep_check(uint16_t now, uint16_t delay) { @@ -234,16 +217,32 @@ ao_lco_drag_beep_check(uint16_t now, uint16_t delay) (int16_t) (now - ao_lco_drag_beep_time)); if (ao_lco_drag_beep_count) { if ((int16_t) (now - ao_lco_drag_beep_time) >= 0) { - if (ao_lco_drag_beep_on) - ao_lco_drag_beep_stop(); - else - ao_lco_drag_beep_start(); + if (ao_lco_drag_beep_on) { + ao_beep(0); + PRINTD("beep stop\n"); + ao_lco_drag_beep_on = 0; + if (ao_lco_drag_beep_count) { + --ao_lco_drag_beep_count; + if (ao_lco_drag_beep_count) + ao_lco_drag_beep_time = now + AO_LCO_DRAG_BEEP_TIME; + } + } else { + ao_beep(AO_BEEP_HIGH); + PRINTD("beep start\n"); + ao_lco_drag_beep_on = 1; + ao_lco_drag_beep_time = now + AO_LCO_DRAG_BEEP_TIME; + } } } if (ao_lco_drag_beep_count) { - if (delay > AO_LCO_DRAG_BEEP_TIME) - delay = AO_LCO_DRAG_BEEP_TIME; + uint16_t beep_delay = 0; + + if (ao_lco_drag_beep_time > now) + beep_delay = ao_lco_drag_beep_time - now; + + if (delay > beep_delay) + delay = beep_delay; } return delay; } @@ -251,39 +250,47 @@ ao_lco_drag_beep_check(uint16_t now, uint16_t delay) static void ao_lco_drag_enable(void) { - PRINTD("Drag enable\n"); - ao_lco_drag_race = 1; - memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); - ao_led_on(AO_LED_DRAG); - ao_lco_drag_beep(5); - ao_lco_set_display(); + if (!ao_lco_drag_race) { + PRINTD("Drag enable\n"); + ao_lco_drag_race = 1; + memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); + ao_led_on(AO_LED_DRAG); + ao_lco_drag_add_beeps(5); + ao_lco_set_display(); + } } static void ao_lco_drag_disable(void) { - PRINTD("Drag disable\n"); - ao_lco_drag_race = 0; - ao_led_off(AO_LED_DRAG); - memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); - ao_lco_drag_beep(2); - ao_lco_set_display(); + if (ao_lco_drag_race) { + PRINTD("Drag disable\n"); + ao_lco_drag_race = 0; + ao_led_off(AO_LED_DRAG); + memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); + ao_lco_drag_add_beeps(2); + ao_lco_set_display(); + } } +/* add a beep if it's time to warn the user that drag race mode is + * active + */ + static uint16_t ao_lco_drag_warn_check(uint16_t now, uint16_t delay) { - uint16_t warn_delay = ~0; - if (ao_lco_drag_race) { + uint16_t warn_delay; + if ((int16_t) (now - ao_lco_drag_warn_time) >= 0) { - ao_lco_drag_beep(1); + ao_lco_drag_add_beeps(1); ao_lco_drag_warn_time = now + AO_LCO_DRAG_WARN_TIME; } warn_delay = ao_lco_drag_warn_time - now; + if (delay > warn_delay) + delay = warn_delay; } - if (delay > warn_delay) - delay = warn_delay; return delay; } @@ -294,25 +301,16 @@ ao_lco_drag_monitor(void) uint16_t now; for (;;) { - PRINTD("Drag monitor active %d delay %d\n", ao_lco_drag_active, delay); + PRINTD("Drag monitor count %d delay %d\n", ao_lco_drag_beep_count, delay); if (delay == (uint16_t) ~0) - ao_sleep(&ao_lco_drag_active); + ao_sleep(&ao_lco_drag_beep_count); else - ao_sleep_for(&ao_lco_drag_active, delay); + ao_sleep_for(&ao_lco_drag_beep_count, delay); delay = ~0; - if (!ao_lco_drag_active) - continue; - now = ao_time(); delay = ao_lco_drag_warn_check(now, delay); delay = ao_lco_drag_beep_check(now, delay); - - /* check to see if there's anything left to do here */ - if (!ao_lco_drag_race && !ao_lco_drag_beep_count) { - delay = ~0; - ao_lco_drag_active = 0; - } } } @@ -415,7 +413,7 @@ ao_lco_input(void) ao_lco_selected[ao_lco_box] ^= (1 << (ao_lco_pad - 1)); PRINTD("Toggle box %d pad %d (pads now %x) to drag race\n", ao_lco_pad, ao_lco_box, ao_lco_selected[ao_lco_box]); - ao_lco_drag_beep(ao_lco_pad); + ao_lco_drag_add_beeps(ao_lco_pad); } } break; -- cgit v1.2.3 From e56e1dc20b3bf18073766da4e26e97d9e1d419fc Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 2 Jul 2018 14:21:48 -0700 Subject: altos/test: Compute and show height error tracker in ao_flight_test Enable the computation of ao_error_h_sq_avg in ao_flight_test even when an accelerometer is present to allow review of that data. Signed-off-by: Keith Packard --- src/kernel/ao_kalman.c | 10 +++++++--- src/test/ao_flight_test.c | 6 +----- 2 files changed, 8 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c index ac41085d..e4cc6d4b 100644 --- a/src/kernel/ao_kalman.c +++ b/src/kernel/ao_kalman.c @@ -45,7 +45,11 @@ static __pdata ao_k_t ao_avg_height_scaled; __xdata ao_v_t ao_avg_height; __pdata ao_v_t ao_error_h; -#if !HAS_ACCEL +#if !HAS_ACCEL || AO_FLIGHT_TEST +#define AO_ERROR_H_SQ_AVG 1 +#endif + +#if AO_ERROR_H_SQ_AVG __pdata ao_v_t ao_error_h_sq_avg; #endif @@ -85,7 +89,7 @@ ao_kalman_predict(void) static void ao_kalman_err_height(void) { -#if !HAS_ACCEL +#if AO_ERROR_H_SQ_AVG ao_v_t e; #endif ao_v_t height_distrust; @@ -95,7 +99,7 @@ ao_kalman_err_height(void) ao_error_h = ao_sample_height - (ao_v_t) (ao_k_height >> 16); -#if !HAS_ACCEL +#if AO_ERROR_H_SQ_AVG e = ao_error_h; if (e < 0) e = -e; diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 8fe3b5df..746a6814 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -305,7 +305,7 @@ struct ao_task { #define AO_MS_TO_TICKS(ms) ((ms) / 10) #define AO_SEC_TO_TICKS(s) ((s) * 100) -#define AO_FLIGHT_TEST +#define AO_FLIGHT_TEST 1 int ao_flight_debug; @@ -438,10 +438,6 @@ static uint16_t pyros_fired; static struct ao_mpu6000_sample ao_ground_mpu6000; #endif -#if HAS_ACCEL -int ao_error_h_sq_avg; -#endif - void ao_test_exit(void) { -- cgit v1.2.3 From c48d4b60da8b23bf358e6c14d7332d3a360c2d2f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 3 Jul 2018 01:00:15 -0700 Subject: altos: Share common LCO functions in ao_lco_bits.c Move common code out of ao_lco.c and ao_lco_v2.c into a shared file, ao_lco_bits.c. Update ao_lco.h to include shared functions. Signed-off-by: Keith Packard --- src/drivers/ao_lco.c | 517 +++++---------------------------------- src/drivers/ao_lco.h | 116 +++++++++ src/drivers/ao_lco_bits.c | 468 +++++++++++++++++++++++++++++++++++ src/telelco-v0.2-cc1200/Makefile | 1 + src/telelco-v0.2/Makefile | 1 + src/telelco-v0.3/Makefile | 1 + src/telelco-v2.0/Makefile | 1 + src/telelco-v2.0/ao_lco_v2.c | 475 +++-------------------------------- 8 files changed, 679 insertions(+), 901 deletions(-) create mode 100644 src/drivers/ao_lco_bits.c (limited to 'src') diff --git a/src/drivers/ao_lco.c b/src/drivers/ao_lco.c index 91a0593f..6b270042 100644 --- a/src/drivers/ao_lco.c +++ b/src/drivers/ao_lco.c @@ -27,15 +27,6 @@ #include #endif -#define DEBUG 1 - -#if DEBUG -static uint8_t ao_lco_debug; -#define PRINTD(...) do { if (!ao_lco_debug) break; printf ("\r%5u %s: ", ao_tick_count, __func__); printf(__VA_ARGS__); flush(); } while(0) -#else -#define PRINTD(...) -#endif - #define AO_LCO_PAD_DIGIT 0 #define AO_LCO_BOX_DIGIT_1 1 #define AO_LCO_BOX_DIGIT_10 2 @@ -46,28 +37,15 @@ static uint8_t ao_lco_debug; #define AO_LCO_VALID_LAST 1 #define AO_LCO_VALID_EVER 2 -static uint8_t ao_lco_min_box, ao_lco_max_box; -static uint8_t ao_lco_selected[AO_PAD_MAX_BOXES]; -static uint8_t ao_lco_valid[AO_PAD_MAX_BOXES]; -static uint8_t ao_lco_channels[AO_PAD_MAX_BOXES]; -static uint16_t ao_lco_tick_offset[AO_PAD_MAX_BOXES]; - /* UI values */ -static uint8_t ao_lco_armed; -static uint8_t ao_lco_firing; static uint16_t ao_lco_fire_tick; static uint8_t ao_lco_fire_down; -static uint8_t ao_lco_drag_race; -static uint8_t ao_lco_pad; -static int16_t ao_lco_box; #define AO_LCO_BOX_DRAG 0x1000 -static struct ao_pad_query ao_pad_query; - static uint8_t ao_lco_display_mutex; -static void +void ao_lco_set_pad(uint8_t pad) { ao_mutex_get(&ao_lco_display_mutex); @@ -92,7 +70,7 @@ ao_lco_set_pad(uint8_t pad) (0 << 5) | \ (0 << 6)) -static void +void ao_lco_set_box(uint16_t box) { ao_mutex_get(&ao_lco_display_mutex); @@ -106,7 +84,7 @@ ao_lco_set_box(uint16_t box) ao_mutex_put(&ao_lco_display_mutex); } -static void +void ao_lco_set_voltage(uint16_t decivolts) { uint8_t tens, ones, tenths; @@ -121,7 +99,7 @@ ao_lco_set_voltage(uint16_t decivolts) ao_mutex_put(&ao_lco_display_mutex); } -static void +void ao_lco_set_display(void) { if (ao_lco_pad == 0 && ao_lco_box != AO_LCO_BOX_DRAG) { @@ -135,13 +113,7 @@ ao_lco_set_display(void) } } -#define MASK_SIZE(n) (((n) + 7) >> 3) -#define MASK_ID(n) ((n) >> 3) -#define MASK_SHIFT(n) ((n) & 7) - -static uint8_t ao_lco_box_mask[MASK_SIZE(AO_PAD_MAX_BOXES)]; - -static uint8_t +uint8_t ao_lco_box_present(uint16_t box) { if (box == AO_LCO_BOX_DRAG) @@ -149,117 +121,11 @@ ao_lco_box_present(uint16_t box) if (box >= AO_PAD_MAX_BOXES) return 0; - return (ao_lco_box_mask[MASK_ID(box)] >> MASK_SHIFT(box)) & 1; -} - -static uint8_t -ao_lco_pad_present(uint8_t box, uint8_t pad) -{ - /* voltage measurement is always valid */ - if (pad == 0) - return 1; - if (!ao_lco_channels[box]) - return 0; - if (pad > AO_PAD_MAX_CHANNELS) - return 0; - return (ao_lco_channels[box] >> (pad - 1)) & 1; -} - -static uint8_t -ao_lco_pad_first(uint8_t box) -{ - uint8_t pad; - - for (pad = 1; pad <= AO_PAD_MAX_CHANNELS; pad++) - if (ao_lco_pad_present(box, pad)) - return pad; - return 0; + return (ao_lco_box_mask[AO_LCO_MASK_ID(box)] >> AO_LCO_MASK_SHIFT(box)) & 1; } static struct ao_task ao_lco_drag_task; static uint8_t ao_lco_drag_active; -static uint8_t ao_lco_drag_beep_count; -static uint8_t ao_lco_drag_beep_on; -static uint16_t ao_lco_drag_beep_time; -static uint16_t ao_lco_drag_warn_time; - -#define AO_LCO_DRAG_BEEP_TIME AO_MS_TO_TICKS(50) -#define AO_LCO_DRAG_WARN_TIME AO_SEC_TO_TICKS(5) - -static void -ao_lco_drag_beep_start(void) -{ - ao_beep(AO_BEEP_HIGH); - PRINTD("beep start\n"); - ao_lco_drag_beep_on = 1; - ao_lco_drag_beep_time = ao_time() + AO_LCO_DRAG_BEEP_TIME; -} - -static void -ao_lco_drag_beep_stop(void) -{ - ao_beep(0); - PRINTD("beep stop\n"); - ao_lco_drag_beep_on = 0; - if (ao_lco_drag_beep_count) { - --ao_lco_drag_beep_count; - if (ao_lco_drag_beep_count) - ao_lco_drag_beep_time = ao_time() + AO_LCO_DRAG_BEEP_TIME; - } -} - -static void -ao_lco_drag_beep(uint8_t beeps) -{ - PRINTD("beep %d\n", beeps); - if (!ao_lco_drag_beep_count) - ao_lco_drag_beep_start(); - ao_lco_drag_beep_count += beeps; -} - -static uint16_t -ao_lco_drag_beep_check(uint16_t now, uint16_t delay) -{ - PRINTD("beep check count %d delta %d\n", - ao_lco_drag_beep_count, - (int16_t) (now - ao_lco_drag_beep_time)); - if (ao_lco_drag_beep_count) { - if ((int16_t) (now - ao_lco_drag_beep_time) >= 0) { - if (ao_lco_drag_beep_on) - ao_lco_drag_beep_stop(); - else - ao_lco_drag_beep_start(); - } - } - - if (ao_lco_drag_beep_count) { - if (delay > AO_LCO_DRAG_BEEP_TIME) - delay = AO_LCO_DRAG_BEEP_TIME; - } - return delay; -} - -static void -ao_lco_drag_enable(void) -{ - PRINTD("Drag enable\n"); - ao_lco_drag_race = 1; - memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); - ao_lco_drag_beep(5); - ao_lco_set_display(); - ao_lco_fire_down = 0; -} - -static void -ao_lco_drag_disable(void) -{ - PRINTD("Drag disable\n"); - ao_lco_drag_race = 0; - memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); - ao_lco_drag_beep(2); - ao_lco_set_display(); - ao_lco_fire_down = 0; -} static uint16_t ao_lco_drag_button_check(uint16_t now, uint16_t delay) @@ -272,13 +138,17 @@ ao_lco_drag_button_check(uint16_t now, uint16_t delay) */ if (ao_lco_fire_down) { if (ao_lco_drag_race) { - if ((int16_t) (now - ao_lco_fire_tick) >= AO_LCO_DRAG_RACE_STOP_TIME) + if ((int16_t) (now - ao_lco_fire_tick) >= AO_LCO_DRAG_RACE_STOP_TIME) { ao_lco_drag_disable(); + ao_lco_fire_down = 0; + } else button_delay = ao_lco_fire_tick + AO_LCO_DRAG_RACE_STOP_TIME - now; } else { - if ((int16_t) (now - ao_lco_fire_tick) >= AO_LCO_DRAG_RACE_START_TIME) + if ((int16_t) (now - ao_lco_fire_tick) >= AO_LCO_DRAG_RACE_START_TIME) { ao_lco_drag_enable(); + ao_lco_fire_down = 0; + } else button_delay = ao_lco_fire_tick + AO_LCO_DRAG_RACE_START_TIME - now; } @@ -288,35 +158,20 @@ ao_lco_drag_button_check(uint16_t now, uint16_t delay) return delay; } -static uint16_t -ao_lco_drag_warn_check(uint16_t now, uint16_t delay) -{ - uint16_t warn_delay = ~0; - - if (ao_lco_drag_race) { - if ((int16_t) (now - ao_lco_drag_warn_time) >= 0) { - ao_lco_drag_beep(1); - ao_lco_drag_warn_time = now + AO_LCO_DRAG_WARN_TIME; - } - warn_delay = ao_lco_drag_warn_time - now; - } - if (delay > warn_delay) - delay = warn_delay; - return delay; -} - static void ao_lco_drag_monitor(void) { uint16_t delay = ~0; uint16_t now; + ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200)); for (;;) { - PRINTD("Drag monitor active %d delay %d\n", ao_lco_drag_active, delay); + PRINTD("Drag monitor count %d active %d delay %d\n", + ao_lco_drag_beep_count, ao_lco_drag_active, delay); if (delay == (uint16_t) ~0) - ao_sleep(&ao_lco_drag_active); + ao_sleep(&ao_lco_drag_beep_count); else - ao_sleep_for(&ao_lco_drag_active, delay); + ao_sleep_for(&ao_lco_drag_beep_count, delay); delay = ~0; if (!ao_lco_drag_active) @@ -335,14 +190,40 @@ ao_lco_drag_monitor(void) } } +static void +ao_lco_step_box(int8_t dir) +{ + int16_t new_box = ao_lco_box; + do { + if (new_box == AO_LCO_BOX_DRAG) { + if (dir < 0) + new_box = ao_lco_max_box; + else + new_box = ao_lco_min_box; + } else { + new_box += dir; + if (new_box > ao_lco_max_box) + new_box = AO_LCO_BOX_DRAG; + else if (new_box < ao_lco_min_box) + new_box = AO_LCO_BOX_DRAG; + } + if (new_box == ao_lco_box) + break; + } while (!ao_lco_box_present(new_box)); + if (ao_lco_box != new_box) { + ao_lco_box = new_box; + ao_lco_pad = 1; + if (ao_lco_box != AO_LCO_BOX_DRAG) + ao_lco_channels[ao_lco_box] = 0; + ao_lco_set_display(); + } +} + static void ao_lco_input(void) { static struct ao_event event; - int8_t dir, new_pad; - int16_t new_box; - ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200)); for (;;) { ao_event_get(&event); PRINTD("event type %d unit %d value %d\n", @@ -351,106 +232,38 @@ ao_lco_input(void) case AO_EVENT_QUADRATURE: switch (event.unit) { case AO_QUADRATURE_PAD: - if (!ao_lco_armed) { - dir = (int8_t) event.value; - new_pad = ao_lco_pad; - do { - new_pad += dir; - if (new_pad > AO_PAD_MAX_CHANNELS) - new_pad = 0; - if (new_pad < 0) - new_pad = AO_PAD_MAX_CHANNELS; - if (new_pad == ao_lco_pad) - break; - } while (!ao_lco_pad_present(ao_lco_box, new_pad)); - if (new_pad != ao_lco_pad) { - ao_lco_pad = new_pad; - ao_lco_set_display(); - } - } + if (!ao_lco_armed) + ao_lco_step_pad((int8_t) event.value); break; case AO_QUADRATURE_BOX: - if (!ao_lco_armed) { - dir = (int8_t) event.value; - new_box = ao_lco_box; - do { - if (new_box == AO_LCO_BOX_DRAG) { - if (dir < 0) - new_box = ao_lco_max_box; - else - new_box = ao_lco_min_box; - } else { - new_box += dir; - if (new_box > ao_lco_max_box) - new_box = AO_LCO_BOX_DRAG; - else if (new_box < ao_lco_min_box) - new_box = AO_LCO_BOX_DRAG; - } - if (new_box == ao_lco_box) - break; - } while (!ao_lco_box_present(new_box)); - if (ao_lco_box != new_box) { - ao_lco_box = new_box; - ao_lco_pad = 1; - if (ao_lco_box != AO_LCO_BOX_DRAG) - ao_lco_channels[ao_lco_box] = 0; - ao_lco_set_display(); - } - } + if (!ao_lco_armed) + ao_lco_step_box((int8_t) event.value); break; } break; case AO_EVENT_BUTTON: switch (event.unit) { case AO_BUTTON_ARM: - ao_lco_armed = event.value; - PRINTD("Armed %d\n", ao_lco_armed); - if (ao_lco_armed) { - if (ao_lco_drag_race) { - uint8_t box; - - for (box = ao_lco_min_box; box <= ao_lco_max_box; box++) { - if (ao_lco_selected[box]) { - ao_wakeup(&ao_lco_armed); - break; - } - } - } else { - memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); - if (ao_lco_pad != 0 && ao_lco_box != AO_LCO_BOX_DRAG) - ao_lco_selected[ao_lco_box] = (1 << (ao_lco_pad - 1)); - else - ao_lco_armed = 0; - } - } - ao_wakeup(&ao_lco_armed); + ao_lco_set_armed(event.value); break; case AO_BUTTON_FIRE: if (ao_lco_armed) { ao_lco_fire_down = 0; - ao_lco_firing = event.value; - PRINTD("Firing %d\n", ao_lco_firing); - ao_wakeup(&ao_lco_armed); + ao_lco_set_firing(event.value); } else { if (event.value) { if (ao_lco_box == AO_LCO_BOX_DRAG) { ao_lco_fire_down = 1; ao_lco_fire_tick = ao_time(); ao_lco_drag_active = 1; + ao_wakeup(&ao_lco_drag_beep_count); + } else { + ao_lco_toggle_drag(); } - if (ao_lco_drag_race) { - if (ao_lco_pad != 0 && ao_lco_box != AO_LCO_BOX_DRAG) { - ao_lco_selected[ao_lco_box] ^= (1 << (ao_lco_pad - 1)); - PRINTD("Toggle box %d pad %d (pads now %x) to drag race\n", - ao_lco_pad, ao_lco_box, ao_lco_selected[ao_lco_box]); - ao_lco_drag_beep(ao_lco_pad); - } - } - ao_wakeup(&ao_lco_drag_active); } else { ao_lco_fire_down = 0; if (ao_lco_drag_active) - ao_wakeup(&ao_lco_drag_active); + ao_wakeup(&ao_lco_drag_beep_count); } } break; @@ -460,189 +273,6 @@ ao_lco_input(void) } } -static AO_LED_TYPE continuity_led[AO_LED_CONTINUITY_NUM] = { -#ifdef AO_LED_CONTINUITY_0 - AO_LED_CONTINUITY_0, -#endif -#ifdef AO_LED_CONTINUITY_1 - AO_LED_CONTINUITY_1, -#endif -#ifdef AO_LED_CONTINUITY_2 - AO_LED_CONTINUITY_2, -#endif -#ifdef AO_LED_CONTINUITY_3 - AO_LED_CONTINUITY_3, -#endif -#ifdef AO_LED_CONTINUITY_4 - AO_LED_CONTINUITY_4, -#endif -#ifdef AO_LED_CONTINUITY_5 - AO_LED_CONTINUITY_5, -#endif -#ifdef AO_LED_CONTINUITY_6 - AO_LED_CONTINUITY_6, -#endif -#ifdef AO_LED_CONTINUITY_7 - AO_LED_CONTINUITY_7, -#endif -}; - -static uint8_t -ao_lco_get_channels(uint8_t box, struct ao_pad_query *query) -{ - int8_t r; - - r = ao_lco_query(box, query, &ao_lco_tick_offset[box]); - if (r == AO_RADIO_CMAC_OK) { - ao_lco_channels[box] = query->channels; - ao_lco_valid[box] = AO_LCO_VALID_LAST | AO_LCO_VALID_EVER; - } else - ao_lco_valid[box] &= ~AO_LCO_VALID_LAST; - PRINTD("ao_lco_get_channels(%d) rssi %d valid %d ret %d offset %d\n", box, ao_radio_cmac_rssi, ao_lco_valid[box], r, ao_lco_tick_offset[box]); - ao_wakeup(&ao_pad_query); - return ao_lco_valid[box]; -} - -static void -ao_lco_update(void) -{ - if (ao_lco_box != AO_LCO_BOX_DRAG) { - uint8_t previous_valid = ao_lco_valid[ao_lco_box]; - - if (ao_lco_get_channels(ao_lco_box, &ao_pad_query) & AO_LCO_VALID_LAST) { - if (!(previous_valid & AO_LCO_VALID_EVER)) { - if (ao_lco_pad != 0) - ao_lco_pad = ao_lco_pad_first(ao_lco_box); - ao_lco_set_display(); - } - if (ao_lco_pad == 0) - ao_lco_set_display(); - } - } -} - -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); -} - -static void -ao_lco_search(void) -{ - int8_t r; - int8_t try; - uint8_t box; - uint8_t boxes = 0; - - ao_lco_box_reset_present(); - ao_lco_set_pad(0); - for (box = 0; box < AO_PAD_MAX_BOXES; box++) { - if ((box % 10) == 0) - ao_lco_set_box(box); - for (try = 0; try < 3; try++) { - ao_lco_tick_offset[box] = 0; - r = ao_lco_query(box, &ao_pad_query, &ao_lco_tick_offset[box]); - PRINTD("box %d result %d offset %d\n", box, r, ao_lco_tick_offset[box]); - if (r == AO_RADIO_CMAC_OK) { - ++boxes; - ao_lco_box_set_present(box); - ao_lco_set_pad(boxes % 10); - ao_delay(AO_MS_TO_TICKS(30)); - break; - } - } - } - if (ao_lco_min_box <= ao_lco_max_box) - ao_lco_box = ao_lco_min_box; - else - ao_lco_min_box = ao_lco_max_box = ao_lco_box = 0; - memset(ao_lco_valid, 0, sizeof (ao_lco_valid)); - memset(ao_lco_channels, 0, sizeof (ao_lco_channels)); - ao_lco_pad = 1; - ao_lco_set_display(); -} - -static void -ao_lco_igniter_status(void) -{ - uint8_t c; - uint8_t t = 0; - - for (;;) { - ao_sleep(&ao_pad_query); - PRINTD("RSSI %d VALID %d\n", ao_radio_cmac_rssi, ao_lco_box == AO_LCO_BOX_DRAG ? -1 : ao_lco_valid[ao_lco_box]); - if (ao_lco_box == AO_LCO_BOX_DRAG) { - ao_led_off(AO_LED_RED|AO_LED_GREEN|AO_LED_AMBER); - for (c = 0; c < AO_LED_CONTINUITY_NUM; c++) - ao_led_off(continuity_led[c]); - } else { - if (!(ao_lco_valid[ao_lco_box] & AO_LCO_VALID_LAST)) { - ao_led_on(AO_LED_RED); - ao_led_off(AO_LED_GREEN|AO_LED_AMBER); - continue; - } - if (ao_radio_cmac_rssi < -90) { - ao_led_on(AO_LED_AMBER); - ao_led_off(AO_LED_RED|AO_LED_GREEN); - } else { - ao_led_on(AO_LED_GREEN); - ao_led_off(AO_LED_RED|AO_LED_AMBER); - } - if (ao_pad_query.arm_status) - ao_led_on(AO_LED_REMOTE_ARM); - else - ao_led_off(AO_LED_REMOTE_ARM); - - for (c = 0; c < AO_LED_CONTINUITY_NUM; c++) { - uint8_t status; - - if (ao_lco_drag_race) { - if (ao_lco_selected[ao_lco_box] & (1 << c) && t) - ao_led_on(continuity_led[c]); - else - ao_led_off(continuity_led[c]); - } else { - if (ao_pad_query.channels & (1 << c)) - status = ao_pad_query.igniter_status[c]; - else - status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN; - if (status == AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN) - ao_led_on(continuity_led[c]); - else - ao_led_off(continuity_led[c]); - } - } - t = 1-t; - } - } -} - -static void -ao_lco_arm_warn(void) -{ - for (;;) { - while (!ao_lco_armed) - ao_sleep(&ao_lco_armed); - ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200)); - ao_delay(AO_MS_TO_TICKS(200)); - } -} - /* * Light up everything for a second at power on to let the user * visually inspect the system for correct operation @@ -680,11 +310,8 @@ static struct ao_task ao_lco_arm_warn_task; static struct ao_task ao_lco_igniter_status_task; static void -ao_lco_monitor(void) +ao_lco_main(void) { - uint16_t delay; - uint8_t box; - ao_lco_display_test(); #if HAS_ADC_SINGLE ao_lco_batt_voltage(); @@ -694,33 +321,7 @@ ao_lco_monitor(void) ao_add_task(&ao_lco_arm_warn_task, ao_lco_arm_warn, "lco arm warn"); ao_add_task(&ao_lco_igniter_status_task, ao_lco_igniter_status, "lco igniter status"); ao_add_task(&ao_lco_drag_task, ao_lco_drag_monitor, "drag race"); - for (;;) { - PRINTD("monitor armed %d firing %d\n", - ao_lco_armed, ao_lco_firing); - - if (ao_lco_armed && ao_lco_firing) { - ao_lco_ignite(AO_PAD_FIRE); - } else { - ao_lco_update(); - if (ao_lco_armed) { - for (box = ao_lco_min_box; box <= ao_lco_max_box; box++) { - if (ao_lco_selected[box]) { - PRINTD("Arming box %d pads %x\n", - box, ao_lco_selected[box]); - if (ao_lco_valid[box] & AO_LCO_VALID_EVER) { - ao_lco_arm(box, ao_lco_selected[box], ao_lco_tick_offset[box]); - ao_delay(AO_MS_TO_TICKS(10)); - } - } - } - } - } - if (ao_lco_armed && ao_lco_firing) - delay = AO_MS_TO_TICKS(100); - else - delay = AO_SEC_TO_TICKS(1); - ao_sleep_for(&ao_lco_armed, delay); - } + ao_lco_monitor(); } #if DEBUG @@ -742,7 +343,7 @@ __code struct ao_cmds ao_lco_cmds[] = { void ao_lco_init(void) { - ao_add_task(&ao_lco_monitor_task, ao_lco_monitor, "lco monitor"); + ao_add_task(&ao_lco_monitor_task, ao_lco_main, "lco monitor"); #if DEBUG ao_cmd_register(&ao_lco_cmds[0]); #endif diff --git a/src/drivers/ao_lco.h b/src/drivers/ao_lco.h index 5721eed5..d5aace0d 100644 --- a/src/drivers/ao_lco.h +++ b/src/drivers/ao_lco.h @@ -19,7 +19,123 @@ #ifndef _AO_LCO_H_ #define _AO_LCO_H_ +#include + +#define DEBUG 1 + +#if DEBUG +extern uint8_t ao_lco_debug; +#define PRINTD(...) do { if (!ao_lco_debug) break; printf ("\r%5u %s: ", ao_tick_count, __func__); printf(__VA_ARGS__); flush(); } while(0) +#else +#define PRINTD(...) +#endif + +extern uint8_t ao_lco_drag_race; /* TRUE when drag race mode enabled */ +extern uint8_t ao_lco_pad; /* Currently selected pad */ +extern int16_t ao_lco_box; /* Currently selected box */ + +extern uint8_t ao_lco_armed; +extern uint8_t ao_lco_firing; + +extern struct ao_pad_query ao_pad_query; /* Last received QUERY from pad */ + +#define AO_LCO_VALID_LAST 1 +#define AO_LCO_VALID_EVER 2 + +#define AO_LCO_PAD_VOLTAGE 0 /* Pad number to show box voltage */ + +extern uint8_t ao_lco_min_box, ao_lco_max_box; +extern uint8_t ao_lco_selected[AO_PAD_MAX_BOXES]; +extern uint8_t ao_lco_valid[AO_PAD_MAX_BOXES]; +extern uint8_t ao_lco_channels[AO_PAD_MAX_BOXES]; +extern uint16_t ao_lco_tick_offset[AO_PAD_MAX_BOXES]; + +#define AO_LCO_MASK_SIZE(n) (((n) + 7) >> 3) +#define AO_LCO_MASK_ID(n) ((n) >> 3) +#define AO_LCO_MASK_SHIFT(n) ((n) & 7) + +extern uint8_t ao_lco_box_mask[AO_LCO_MASK_SIZE(AO_PAD_MAX_BOXES)]; + +/* + * Shared functions + */ + +void +ao_lco_igniter_status(void); + +void +ao_lco_update(void); + +uint8_t +ao_lco_pad_present(uint8_t box, uint8_t pad); + +uint8_t +ao_lco_pad_first(uint8_t box); + +void +ao_lco_step_pad(int8_t dir); + +void +ao_lco_set_armed(uint8_t armed); + +void +ao_lco_set_firing(uint8_t firing); + +void +ao_lco_toggle_drag(void); + +void +ao_lco_search(void); + +void +ao_lco_monitor(void); + +extern uint8_t ao_lco_drag_beep_count; + +/* enable drag race mode */ +void +ao_lco_drag_enable(void); + +/* disable drag race mode */ +void +ao_lco_drag_disable(void); + +/* Handle drag beeps, return new delay */ +uint16_t +ao_lco_drag_beep_check(uint16_t now, uint16_t delay); + +/* Check if it's time to beep during drag race. Return new delay */ +uint16_t +ao_lco_drag_warn_check(uint16_t now, uint16_t delay); + +/* Request 'beeps' additional drag race beeps */ +void +ao_lco_drag_add_beeps(uint8_t beeps); + +/* task function for beeping while arm is active */ +void +ao_lco_arm_warn(void); + +/* + * Provided by the hw-specific driver code + */ + +void +ao_lco_set_pad(uint8_t pad); + +void +ao_lco_set_box(uint16_t box); + +void +ao_lco_set_voltage(uint16_t decivolts); + +void +ao_lco_set_display(void); + void ao_lco_init(void); +uint8_t +ao_lco_box_present(uint16_t box); + #endif /* _AO_LCO_H_ */ diff --git a/src/drivers/ao_lco_bits.c b/src/drivers/ao_lco_bits.c new file mode 100644 index 00000000..b2124d1b --- /dev/null +++ b/src/drivers/ao_lco_bits.c @@ -0,0 +1,468 @@ +/* + * Copyright © 2018 Keith Packard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + */ + +#include +#include +#include + +uint8_t ao_lco_debug; + +uint8_t ao_lco_pad; +int16_t ao_lco_box; +uint8_t ao_lco_drag_race; + +uint8_t ao_lco_armed; /* arm active */ +uint8_t ao_lco_firing; /* fire active */ + +uint8_t ao_lco_min_box, ao_lco_max_box; +uint8_t ao_lco_selected[AO_PAD_MAX_BOXES]; +uint8_t ao_lco_valid[AO_PAD_MAX_BOXES]; +uint8_t ao_lco_channels[AO_PAD_MAX_BOXES]; +uint16_t ao_lco_tick_offset[AO_PAD_MAX_BOXES]; + +struct ao_pad_query ao_pad_query; + +static AO_LED_TYPE continuity_led[AO_LED_CONTINUITY_NUM] = { +#ifdef AO_LED_CONTINUITY_0 + AO_LED_CONTINUITY_0, +#endif +#ifdef AO_LED_CONTINUITY_1 + AO_LED_CONTINUITY_1, +#endif +#ifdef AO_LED_CONTINUITY_2 + AO_LED_CONTINUITY_2, +#endif +#ifdef AO_LED_CONTINUITY_3 + AO_LED_CONTINUITY_3, +#endif +#ifdef AO_LED_CONTINUITY_4 + AO_LED_CONTINUITY_4, +#endif +#ifdef AO_LED_CONTINUITY_5 + AO_LED_CONTINUITY_5, +#endif +#ifdef AO_LED_CONTINUITY_6 + AO_LED_CONTINUITY_6, +#endif +#ifdef AO_LED_CONTINUITY_7 + AO_LED_CONTINUITY_7, +#endif +}; + +/* Set LEDs to match remote box status */ +void +ao_lco_igniter_status(void) +{ + uint8_t c; + uint8_t t = 0; + + for (;;) { + if (ao_lco_drag_race) + ao_sleep_for(&ao_pad_query, AO_MS_TO_TICKS(50)); + else + ao_sleep(&ao_pad_query); + PRINTD("RSSI %d VALID %d\n", ao_radio_cmac_rssi, ao_lco_valid[ao_lco_box]); + if (!(ao_lco_valid[ao_lco_box] & AO_LCO_VALID_LAST)) { + ao_led_on(AO_LED_RED); + ao_led_off(AO_LED_GREEN|AO_LED_AMBER); + continue; + } + if (ao_radio_cmac_rssi < -90) { + ao_led_on(AO_LED_AMBER); + ao_led_off(AO_LED_RED|AO_LED_GREEN); + } else { + ao_led_on(AO_LED_GREEN); + ao_led_off(AO_LED_RED|AO_LED_AMBER); + } + if (ao_pad_query.arm_status) + ao_led_on(AO_LED_REMOTE_ARM); + else + ao_led_off(AO_LED_REMOTE_ARM); + + for (c = 0; c < AO_LED_CONTINUITY_NUM; c++) { + uint8_t status; + + if (ao_pad_query.channels & (1 << c)) + status = ao_pad_query.igniter_status[c]; + else + status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN; + + if (ao_lco_drag_race && (ao_lco_selected[ao_lco_box] & (1 << c))) { + uint8_t on = 0; + if (status == AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN) { + if (t) + on = 1; + } else { + if (t == 1) + on = 1; + } + if (on) + ao_led_on(continuity_led[c]); + else + ao_led_off(continuity_led[c]); + } else { + if (status == AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN) + ao_led_on(continuity_led[c]); + else + ao_led_off(continuity_led[c]); + } + } + t = (t + 1) & 3; + } +} + +uint8_t +ao_lco_pad_present(uint8_t box, uint8_t pad) +{ + /* voltage measurement is always valid */ + if (pad == AO_LCO_PAD_VOLTAGE) + return 1; + if (!ao_lco_channels[box]) + return 0; + if (pad > AO_PAD_MAX_CHANNELS) + return 0; + return (ao_lco_channels[box] >> (pad - 1)) & 1; +} + +uint8_t +ao_lco_pad_first(uint8_t box) +{ + uint8_t pad; + + for (pad = 1; pad <= AO_PAD_MAX_CHANNELS; pad++) + if (ao_lco_pad_present(box, pad)) + return pad; + return 0; +} + +static uint8_t +ao_lco_get_channels(uint8_t box, struct ao_pad_query *query) +{ + int8_t r; + + r = ao_lco_query(box, query, &ao_lco_tick_offset[box]); + if (r == AO_RADIO_CMAC_OK) { + ao_lco_channels[box] = query->channels; + ao_lco_valid[box] = AO_LCO_VALID_LAST | AO_LCO_VALID_EVER; + } else + ao_lco_valid[box] &= ~AO_LCO_VALID_LAST; + PRINTD("ao_lco_get_channels(%d) rssi %d valid %d ret %d offset %d\n", box, ao_radio_cmac_rssi, ao_lco_valid[box], r, ao_lco_tick_offset[box]); + ao_wakeup(&ao_pad_query); + return ao_lco_valid[box]; +} + +void +ao_lco_update(void) +{ + uint8_t previous_valid = ao_lco_valid[ao_lco_box]; + + if (ao_lco_get_channels(ao_lco_box, &ao_pad_query) & AO_LCO_VALID_LAST) { + if (!(previous_valid & AO_LCO_VALID_EVER)) { + if (ao_lco_pad != AO_LCO_PAD_VOLTAGE) + ao_lco_pad = ao_lco_pad_first(ao_lco_box); + ao_lco_set_display(); + } + if (ao_lco_pad == AO_LCO_PAD_VOLTAGE) + ao_lco_set_display(); + } +} + +uint8_t ao_lco_box_mask[AO_LCO_MASK_SIZE(AO_PAD_MAX_BOXES)]; + +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[AO_LCO_MASK_ID(box)] |= 1 << AO_LCO_MASK_SHIFT(box); +} + +void +ao_lco_step_pad(int8_t dir) +{ + int8_t new_pad; + + new_pad = ao_lco_pad; + do { + new_pad += dir; + if (new_pad > AO_PAD_MAX_CHANNELS) + new_pad = AO_LCO_PAD_VOLTAGE; + if (new_pad < 0) + new_pad = AO_PAD_MAX_CHANNELS; + if (new_pad == ao_lco_pad) + break; + } while (!ao_lco_pad_present(ao_lco_box, new_pad)); + if (new_pad != ao_lco_pad) { + ao_lco_pad = new_pad; + ao_lco_set_display(); + } +} + +void +ao_lco_set_armed(uint8_t armed) +{ + ao_lco_armed = armed; + PRINTD("Armed %d\n", ao_lco_armed); + if (ao_lco_armed) { + if (ao_lco_drag_race) { + uint8_t box; + + for (box = ao_lco_min_box; box <= ao_lco_max_box; box++) + if (ao_lco_selected[box]) + break; + if (box > ao_lco_max_box) + ao_lco_armed = 0; + } else { + memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); + if (ao_lco_pad != 0) + ao_lco_selected[ao_lco_box] = (1 << (ao_lco_pad - 1)); + else + ao_lco_armed = 0; + } + } + ao_wakeup(&ao_lco_armed); +} + +void +ao_lco_set_firing(uint8_t firing) +{ + ao_lco_firing = firing; + PRINTD("Firing %d\n", ao_lco_firing); + ao_wakeup(&ao_lco_armed); +} + +void +ao_lco_toggle_drag(void) +{ + if (ao_lco_drag_race && ao_lco_pad != AO_LCO_PAD_VOLTAGE) { + ao_lco_selected[ao_lco_box] ^= (1 << (ao_lco_pad - 1)); + PRINTD("Toggle box %d pad %d (pads now %x) to drag race\n", + ao_lco_pad, ao_lco_box, ao_lco_selected[ao_lco_box]); + ao_lco_drag_add_beeps(ao_lco_pad); + } +} + +void +ao_lco_search(void) +{ + int8_t r; + int8_t try; + uint8_t box; + uint8_t boxes = 0; + + ao_lco_box_reset_present(); + ao_lco_set_pad(0); + for (box = 0; box < AO_PAD_MAX_BOXES; box++) { + if ((box % 10) == 0) + ao_lco_set_box(box); + for (try = 0; try < 3; try++) { + ao_lco_tick_offset[box] = 0; + r = ao_lco_query(box, &ao_pad_query, &ao_lco_tick_offset[box]); + PRINTD("box %d result %d offset %d\n", box, r, ao_lco_tick_offset[box]); + if (r == AO_RADIO_CMAC_OK) { + ++boxes; + ao_lco_box_set_present(box); + ao_lco_set_pad(boxes % 10); + ao_delay(AO_MS_TO_TICKS(30)); + break; + } + } + } + if (ao_lco_min_box <= ao_lco_max_box) + ao_lco_box = ao_lco_min_box; + else + ao_lco_min_box = ao_lco_max_box = ao_lco_box = 0; + memset(ao_lco_valid, 0, sizeof (ao_lco_valid)); + memset(ao_lco_channels, 0, sizeof (ao_lco_channels)); + ao_lco_pad = 1; + ao_lco_set_display(); +} + +void +ao_lco_monitor(void) +{ + uint16_t delay; + uint8_t box; + + for (;;) { + PRINTD("monitor armed %d firing %d\n", + ao_lco_armed, ao_lco_firing); + + if (ao_lco_armed && ao_lco_firing) { + ao_lco_ignite(AO_PAD_FIRE); + } else { + ao_lco_update(); + if (ao_lco_armed) { + for (box = ao_lco_min_box; box <= ao_lco_max_box; box++) { + if (ao_lco_selected[box]) { + PRINTD("Arming box %d pads %x\n", + box, ao_lco_selected[box]); + if (ao_lco_valid[box] & AO_LCO_VALID_EVER) { + ao_lco_arm(box, ao_lco_selected[box], ao_lco_tick_offset[box]); + ao_delay(AO_MS_TO_TICKS(10)); + } + } + } + } + } + if (ao_lco_armed && ao_lco_firing) + delay = AO_MS_TO_TICKS(100); + else + delay = AO_SEC_TO_TICKS(1); + ao_sleep_for(&ao_lco_armed, delay); + } +} + +uint8_t ao_lco_drag_beep_count; +static uint8_t ao_lco_drag_beep_on; +static uint16_t ao_lco_drag_beep_time; +static uint16_t ao_lco_drag_warn_time; + +#define AO_LCO_DRAG_BEEP_TIME AO_MS_TO_TICKS(50) +#define AO_LCO_DRAG_WARN_TIME AO_SEC_TO_TICKS(5) + +/* Request 'beeps' additional drag race beeps */ +void +ao_lco_drag_add_beeps(uint8_t beeps) +{ + PRINTD("beep %d\n", beeps); + if (ao_lco_drag_beep_count == 0) + ao_lco_drag_beep_time = ao_time(); + ao_lco_drag_beep_count += beeps; + ao_wakeup(&ao_lco_drag_beep_count); +} + +/* Check whether it's time to change the beeper status, then either + * turn it on or off as necessary and bump the remaining beep counts + */ + +uint16_t +ao_lco_drag_beep_check(uint16_t now, uint16_t delay) +{ + PRINTD("beep check count %d delta %d\n", + ao_lco_drag_beep_count, + (int16_t) (now - ao_lco_drag_beep_time)); + if (ao_lco_drag_beep_count) { + if ((int16_t) (now - ao_lco_drag_beep_time) >= 0) { + if (ao_lco_drag_beep_on) { + ao_beep(0); + PRINTD("beep stop\n"); + ao_lco_drag_beep_on = 0; + if (ao_lco_drag_beep_count) { + --ao_lco_drag_beep_count; + if (ao_lco_drag_beep_count) + ao_lco_drag_beep_time = now + AO_LCO_DRAG_BEEP_TIME; + } + } else { + ao_beep(AO_BEEP_HIGH); + PRINTD("beep start\n"); + ao_lco_drag_beep_on = 1; + ao_lco_drag_beep_time = now + AO_LCO_DRAG_BEEP_TIME; + } + } + } + + if (ao_lco_drag_beep_count) { + uint16_t beep_delay = 0; + + if (ao_lco_drag_beep_time > now) + beep_delay = ao_lco_drag_beep_time - now; + + if (delay > beep_delay) + delay = beep_delay; + } + return delay; +} + +void +ao_lco_drag_enable(void) +{ + if (!ao_lco_drag_race) { + PRINTD("Drag enable\n"); + ao_lco_drag_race = 1; + memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); +#ifdef AO_LED_DRAG + ao_led_on(AO_LED_DRAG); +#endif + ao_lco_drag_add_beeps(5); + ao_lco_set_display(); + } +} + +void +ao_lco_drag_disable(void) +{ + if (ao_lco_drag_race) { + PRINTD("Drag disable\n"); + ao_lco_drag_race = 0; +#ifdef AO_LED_DRAG + ao_led_off(AO_LED_DRAG); +#endif + memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); + ao_lco_drag_add_beeps(2); + ao_lco_set_display(); + } +} + +/* add a beep if it's time to warn the user that drag race mode is + * active + */ + +uint16_t +ao_lco_drag_warn_check(uint16_t now, uint16_t delay) +{ + if (ao_lco_drag_race) { + uint16_t warn_delay; + + if ((int16_t) (now - ao_lco_drag_warn_time) >= 0) { + ao_lco_drag_add_beeps(1); + ao_lco_drag_warn_time = now + AO_LCO_DRAG_WARN_TIME; + } + warn_delay = ao_lco_drag_warn_time - now; + if (delay > warn_delay) + delay = warn_delay; + } + return delay; +} + +/* task function for beeping while arm is active */ +void +ao_lco_arm_warn(void) +{ + for (;;) { + while (!ao_lco_armed) { +#ifdef AO_LED_FIRE + ao_led_off(AO_LED_FIRE); +#endif + ao_sleep(&ao_lco_armed); + } +#ifdef AO_LED_FIRE + ao_led_on(AO_LED_FIRE); +#endif + ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200)); + ao_delay(AO_MS_TO_TICKS(200)); + } +} diff --git a/src/telelco-v0.2-cc1200/Makefile b/src/telelco-v0.2-cc1200/Makefile index f29e2930..4ccf494c 100644 --- a/src/telelco-v0.2-cc1200/Makefile +++ b/src/telelco-v0.2-cc1200/Makefile @@ -67,6 +67,7 @@ ALTOS_SRC = \ ao_button.c \ ao_event.c \ ao_lco.c \ + ao_lco_bits.c \ ao_lco_cmd.c \ ao_lco_func.c \ ao_radio_cmac_cmd.c diff --git a/src/telelco-v0.2/Makefile b/src/telelco-v0.2/Makefile index 8c1ced6c..8279cac1 100644 --- a/src/telelco-v0.2/Makefile +++ b/src/telelco-v0.2/Makefile @@ -64,6 +64,7 @@ ALTOS_SRC = \ ao_button.c \ ao_event.c \ ao_lco.c \ + ao_lco_bits.c \ ao_lco_cmd.c \ ao_lco_func.c \ ao_radio_cmac_cmd.c diff --git a/src/telelco-v0.3/Makefile b/src/telelco-v0.3/Makefile index 0bb0f9dc..c2592bf8 100644 --- a/src/telelco-v0.3/Makefile +++ b/src/telelco-v0.3/Makefile @@ -65,6 +65,7 @@ ALTOS_SRC = \ ao_button.c \ ao_event.c \ ao_lco.c \ + ao_lco_bits.c \ ao_lco_cmd.c \ ao_lco_func.c \ ao_radio_cmac_cmd.c diff --git a/src/telelco-v2.0/Makefile b/src/telelco-v2.0/Makefile index 4871993d..43295fd3 100644 --- a/src/telelco-v2.0/Makefile +++ b/src/telelco-v2.0/Makefile @@ -66,6 +66,7 @@ ALTOS_SRC = \ ao_quadrature.c \ ao_button.c \ ao_event.c \ + ao_lco_bits.c \ ao_lco_v2.c \ ao_lco_cmd.c \ ao_lco_func.c \ diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index 21b2c54d..9fefde3b 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -21,19 +21,9 @@ #include #include #include -#include #include #include -#define DEBUG 1 - -#if DEBUG -static uint8_t ao_lco_debug; -#define PRINTD(...) do { if (!ao_lco_debug) break; printf ("\r%5u %s: ", ao_tick_count, __func__); printf(__VA_ARGS__); flush(); } while(0) -#else -#define PRINTD(...) -#endif - #define AO_LCO_PAD_DIGIT 0 #define AO_LCO_BOX_DIGIT_1 1 #define AO_LCO_BOX_DIGIT_10 2 @@ -41,30 +31,14 @@ static uint8_t ao_lco_debug; #define AO_LCO_DRAG_RACE_START_TIME AO_SEC_TO_TICKS(5) #define AO_LCO_DRAG_RACE_STOP_TIME AO_SEC_TO_TICKS(2) -#define AO_LCO_VALID_LAST 1 -#define AO_LCO_VALID_EVER 2 - -static uint8_t ao_lco_min_box, ao_lco_max_box; -static uint8_t ao_lco_selected[AO_PAD_MAX_BOXES]; -static uint8_t ao_lco_valid[AO_PAD_MAX_BOXES]; -static uint8_t ao_lco_channels[AO_PAD_MAX_BOXES]; -static uint16_t ao_lco_tick_offset[AO_PAD_MAX_BOXES]; - /* UI values */ -static uint8_t ao_lco_armed; -static uint8_t ao_lco_firing; -static uint8_t ao_lco_drag_race; -static uint8_t ao_lco_pad; -static int16_t ao_lco_box; static uint8_t ao_lco_select_mode; #define AO_LCO_SELECT_PAD 0 #define AO_LCO_SELECT_BOX 1 -static struct ao_pad_query ao_pad_query; - static uint8_t ao_lco_display_mutex; -static void +void ao_lco_set_pad(uint8_t pad) { ao_mutex_get(&ao_lco_display_mutex); @@ -89,7 +63,7 @@ ao_lco_set_pad(uint8_t pad) (0 << 5) | \ (0 << 6)) -static void +void ao_lco_set_box(uint16_t box) { ao_mutex_get(&ao_lco_display_mutex); @@ -98,7 +72,7 @@ ao_lco_set_box(uint16_t box) ao_mutex_put(&ao_lco_display_mutex); } -static void +void ao_lco_set_voltage(uint16_t decivolts) { uint8_t tens, ones, tenths; @@ -114,7 +88,7 @@ ao_lco_set_voltage(uint16_t decivolts) ao_mutex_put(&ao_lco_display_mutex); } -static void +void ao_lco_set_display(void) { if (ao_lco_pad == 0) { @@ -125,42 +99,12 @@ ao_lco_set_display(void) } } -#define MASK_SIZE(n) (((n) + 7) >> 3) -#define MASK_ID(n) ((n) >> 3) -#define MASK_SHIFT(n) ((n) & 7) - -static uint8_t ao_lco_box_mask[MASK_SIZE(AO_PAD_MAX_BOXES)]; - -static uint8_t +uint8_t ao_lco_box_present(uint16_t box) { if (box >= AO_PAD_MAX_BOXES) return 0; - return (ao_lco_box_mask[MASK_ID(box)] >> MASK_SHIFT(box)) & 1; -} - -static uint8_t -ao_lco_pad_present(uint8_t box, uint8_t pad) -{ - /* voltage measurement is always valid */ - if (pad == 0) - return 1; - if (!ao_lco_channels[box]) - return 0; - if (pad > AO_PAD_MAX_CHANNELS) - return 0; - return (ao_lco_channels[box] >> (pad - 1)) & 1; -} - -static uint8_t -ao_lco_pad_first(uint8_t box) -{ - uint8_t pad; - - for (pad = 1; pad <= AO_PAD_MAX_CHANNELS; pad++) - if (ao_lco_pad_present(box, pad)) - return pad; - return 0; + return (ao_lco_box_mask[AO_LCO_MASK_ID(box)] >> AO_LCO_MASK_SHIFT(box)) & 1; } static void @@ -185,114 +129,29 @@ ao_lco_set_select(void) } } -static struct ao_task ao_lco_drag_task; -static uint8_t ao_lco_drag_beep_count; -static uint8_t ao_lco_drag_beep_on; -static uint16_t ao_lco_drag_beep_time; -static uint16_t ao_lco_drag_warn_time; - -#define AO_LCO_DRAG_BEEP_TIME AO_MS_TO_TICKS(50) -#define AO_LCO_DRAG_WARN_TIME AO_SEC_TO_TICKS(5) - -/* Request 'beeps' additional drag race beeps */ -static void -ao_lco_drag_add_beeps(uint8_t beeps) -{ - PRINTD("beep %d\n", beeps); - if (ao_lco_drag_beep_count == 0) - ao_lco_drag_beep_time = ao_time(); - ao_lco_drag_beep_count += beeps; - ao_wakeup(&ao_lco_drag_beep_count); -} - -/* Check whether it's time to change the beeper status, then either - * turn it on or off as necessary and bump the remaining beep counts - */ - -static uint16_t -ao_lco_drag_beep_check(uint16_t now, uint16_t delay) -{ - PRINTD("beep check count %d delta %d\n", - ao_lco_drag_beep_count, - (int16_t) (now - ao_lco_drag_beep_time)); - if (ao_lco_drag_beep_count) { - if ((int16_t) (now - ao_lco_drag_beep_time) >= 0) { - if (ao_lco_drag_beep_on) { - ao_beep(0); - PRINTD("beep stop\n"); - ao_lco_drag_beep_on = 0; - if (ao_lco_drag_beep_count) { - --ao_lco_drag_beep_count; - if (ao_lco_drag_beep_count) - ao_lco_drag_beep_time = now + AO_LCO_DRAG_BEEP_TIME; - } - } else { - ao_beep(AO_BEEP_HIGH); - PRINTD("beep start\n"); - ao_lco_drag_beep_on = 1; - ao_lco_drag_beep_time = now + AO_LCO_DRAG_BEEP_TIME; - } - } - } - - if (ao_lco_drag_beep_count) { - uint16_t beep_delay = 0; - - if (ao_lco_drag_beep_time > now) - beep_delay = ao_lco_drag_beep_time - now; - - if (delay > beep_delay) - delay = beep_delay; - } - return delay; -} - static void -ao_lco_drag_enable(void) +ao_lco_step_box(int8_t dir) { - if (!ao_lco_drag_race) { - PRINTD("Drag enable\n"); - ao_lco_drag_race = 1; - memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); - ao_led_on(AO_LED_DRAG); - ao_lco_drag_add_beeps(5); - ao_lco_set_display(); - } -} - -static void -ao_lco_drag_disable(void) -{ - if (ao_lco_drag_race) { - PRINTD("Drag disable\n"); - ao_lco_drag_race = 0; - ao_led_off(AO_LED_DRAG); - memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); - ao_lco_drag_add_beeps(2); + int16_t 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_max_box; + if (new_box == ao_lco_box) + break; + } while (!ao_lco_box_present(new_box)); + if (ao_lco_box != new_box) { + ao_lco_box = new_box; + ao_lco_pad = 1; + ao_lco_channels[ao_lco_box] = 0; ao_lco_set_display(); } } -/* add a beep if it's time to warn the user that drag race mode is - * active - */ - -static uint16_t -ao_lco_drag_warn_check(uint16_t now, uint16_t delay) -{ - if (ao_lco_drag_race) { - uint16_t warn_delay; - - if ((int16_t) (now - ao_lco_drag_warn_time) >= 0) { - ao_lco_drag_add_beeps(1); - ao_lco_drag_warn_time = now + AO_LCO_DRAG_WARN_TIME; - } - warn_delay = ao_lco_drag_warn_time - now; - if (delay > warn_delay) - delay = warn_delay; - } - return delay; -} +static struct ao_task ao_lco_drag_task; static void ao_lco_drag_monitor(void) @@ -300,6 +159,7 @@ ao_lco_drag_monitor(void) uint16_t delay = ~0; uint16_t now; + ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200)); for (;;) { PRINTD("Drag monitor count %d delay %d\n", ao_lco_drag_beep_count, delay); if (delay == (uint16_t) ~0) @@ -318,10 +178,7 @@ static void ao_lco_input(void) { static struct ao_event event; - int8_t dir, new_pad; - int16_t new_box; - ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200)); for (;;) { ao_event_get(&event); PRINTD("event type %d unit %d value %d\n", @@ -333,40 +190,10 @@ ao_lco_input(void) if (!ao_lco_armed) { switch (ao_lco_select_mode) { case AO_LCO_SELECT_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; - if (new_pad < 0) - new_pad = AO_PAD_MAX_CHANNELS; - if (new_pad == ao_lco_pad) - break; - } while (!ao_lco_pad_present(ao_lco_box, new_pad)); - if (new_pad != ao_lco_pad) { - ao_lco_pad = new_pad; - ao_lco_set_display(); - } + ao_lco_step_pad((int8_t) event.value); break; case AO_LCO_SELECT_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_max_box; - if (new_box == ao_lco_box) - break; - } while (!ao_lco_box_present(new_box)); - if (ao_lco_box != new_box) { - ao_lco_box = new_box; - ao_lco_pad = 1; - ao_lco_channels[ao_lco_box] = 0; - ao_lco_set_display(); - } + ao_lco_step_box((int8_t) event.value); break; default: break; @@ -378,34 +205,12 @@ ao_lco_input(void) case AO_EVENT_BUTTON: switch (event.unit) { case AO_BUTTON_ARM: - ao_lco_armed = event.value; - PRINTD("Armed %d\n", ao_lco_armed); - if (ao_lco_armed) { - if (ao_lco_drag_race) { - uint8_t box; - - for (box = ao_lco_min_box; box <= ao_lco_max_box; box++) - if (ao_lco_selected[box]) - break; - if (box > ao_lco_max_box) - ao_lco_armed = 0; - } else { - memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); - if (ao_lco_pad != 0) - ao_lco_selected[ao_lco_box] = (1 << (ao_lco_pad - 1)); - else - ao_lco_armed = 0; - } - } + ao_lco_set_armed(event.value); ao_lco_set_select(); - ao_wakeup(&ao_lco_armed); break; case AO_BUTTON_FIRE: - if (ao_lco_armed) { - ao_lco_firing = event.value; - PRINTD("Firing %d\n", ao_lco_firing); - ao_wakeup(&ao_lco_armed); - } + if (ao_lco_armed) + ao_lco_set_firing(event.value); break; case AO_BUTTON_DRAG_SELECT: if (event.value && ao_lco_drag_race) { @@ -437,193 +242,6 @@ ao_lco_input(void) } } -static AO_LED_TYPE continuity_led[AO_LED_CONTINUITY_NUM] = { -#ifdef AO_LED_CONTINUITY_0 - AO_LED_CONTINUITY_0, -#endif -#ifdef AO_LED_CONTINUITY_1 - AO_LED_CONTINUITY_1, -#endif -#ifdef AO_LED_CONTINUITY_2 - AO_LED_CONTINUITY_2, -#endif -#ifdef AO_LED_CONTINUITY_3 - AO_LED_CONTINUITY_3, -#endif -#ifdef AO_LED_CONTINUITY_4 - AO_LED_CONTINUITY_4, -#endif -#ifdef AO_LED_CONTINUITY_5 - AO_LED_CONTINUITY_5, -#endif -#ifdef AO_LED_CONTINUITY_6 - AO_LED_CONTINUITY_6, -#endif -#ifdef AO_LED_CONTINUITY_7 - AO_LED_CONTINUITY_7, -#endif -}; - -static uint8_t -ao_lco_get_channels(uint8_t box, struct ao_pad_query *query) -{ - int8_t r; - - r = ao_lco_query(box, query, &ao_lco_tick_offset[box]); - if (r == AO_RADIO_CMAC_OK) { - ao_lco_channels[box] = query->channels; - ao_lco_valid[box] = AO_LCO_VALID_LAST | AO_LCO_VALID_EVER; - } else - ao_lco_valid[box] &= ~AO_LCO_VALID_LAST; - PRINTD("ao_lco_get_channels(%d) rssi %d valid %d ret %d offset %d\n", box, ao_radio_cmac_rssi, ao_lco_valid[box], r, ao_lco_tick_offset[box]); - ao_wakeup(&ao_pad_query); - return ao_lco_valid[box]; -} - -static void -ao_lco_update(void) -{ - uint8_t previous_valid = ao_lco_valid[ao_lco_box]; - - if (ao_lco_get_channels(ao_lco_box, &ao_pad_query) & AO_LCO_VALID_LAST) { - if (!(previous_valid & AO_LCO_VALID_EVER)) { - if (ao_lco_pad != 0) - ao_lco_pad = ao_lco_pad_first(ao_lco_box); - ao_lco_set_display(); - } - if (ao_lco_pad == 0) - ao_lco_set_display(); - } -} - -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); -} - -static void -ao_lco_search(void) -{ - int8_t r; - int8_t try; - uint8_t box; - uint8_t boxes = 0; - - ao_lco_box_reset_present(); - ao_lco_set_pad(0); - for (box = 0; box < AO_PAD_MAX_BOXES; box++) { - if ((box % 10) == 0) - ao_lco_set_box(box); - for (try = 0; try < 3; try++) { - ao_lco_tick_offset[box] = 0; - r = ao_lco_query(box, &ao_pad_query, &ao_lco_tick_offset[box]); - PRINTD("box %d result %d offset %d\n", box, r, ao_lco_tick_offset[box]); - if (r == AO_RADIO_CMAC_OK) { - ++boxes; - ao_lco_box_set_present(box); - ao_lco_set_pad(boxes % 10); - ao_delay(AO_MS_TO_TICKS(30)); - break; - } - } - } - if (ao_lco_min_box <= ao_lco_max_box) - ao_lco_box = ao_lco_min_box; - else - ao_lco_min_box = ao_lco_max_box = ao_lco_box = 0; - memset(ao_lco_valid, 0, sizeof (ao_lco_valid)); - memset(ao_lco_channels, 0, sizeof (ao_lco_channels)); - ao_lco_pad = 1; - ao_lco_set_display(); -} - -static void -ao_lco_igniter_status(void) -{ - uint8_t c; - uint8_t t = 0; - - for (;;) { - ao_sleep(&ao_pad_query); - PRINTD("RSSI %d VALID %d\n", ao_radio_cmac_rssi, ao_lco_valid[ao_lco_box]); - if (!(ao_lco_valid[ao_lco_box] & AO_LCO_VALID_LAST)) { - ao_led_on(AO_LED_RED); - ao_led_off(AO_LED_GREEN|AO_LED_AMBER); - continue; - } - if (ao_radio_cmac_rssi < -90) { - ao_led_on(AO_LED_AMBER); - ao_led_off(AO_LED_RED|AO_LED_GREEN); - } else { - ao_led_on(AO_LED_GREEN); - ao_led_off(AO_LED_RED|AO_LED_AMBER); - } - if (ao_pad_query.arm_status) - ao_led_on(AO_LED_REMOTE_ARM); - else - ao_led_off(AO_LED_REMOTE_ARM); - - for (c = 0; c < AO_LED_CONTINUITY_NUM; c++) { - uint8_t status; - - if (ao_pad_query.channels & (1 << c)) - status = ao_pad_query.igniter_status[c]; - else - status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN; - - if (ao_lco_drag_race && (ao_lco_selected[ao_lco_box] & (1 << c))) { - uint8_t on = 0; - if (status == AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN) { - if (t) - on = 1; - } else { - if (t == 1) - on = 1; - } - if (on) - ao_led_on(continuity_led[c]); - else - ao_led_off(continuity_led[c]); - } else { - if (status == AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN) - ao_led_on(continuity_led[c]); - else - ao_led_off(continuity_led[c]); - } - } - t = (t + 1) & 3; - } -} - -static void -ao_lco_arm_warn(void) -{ - for (;;) { - while (!ao_lco_armed) { - ao_led_off(AO_LED_FIRE); - ao_sleep(&ao_lco_armed); - } - ao_led_on(AO_LED_FIRE); - ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200)); - ao_delay(AO_MS_TO_TICKS(200)); - } -} - /* * Light up everything for a second at power on to let the user * visually inspect the system for correct operation @@ -659,11 +277,8 @@ static struct ao_task ao_lco_arm_warn_task; static struct ao_task ao_lco_igniter_status_task; static void -ao_lco_monitor(void) +ao_lco_main(void) { - uint16_t delay; - uint8_t box; - ao_lco_display_test(); ao_lco_batt_voltage(); ao_lco_search(); @@ -671,33 +286,7 @@ ao_lco_monitor(void) ao_add_task(&ao_lco_arm_warn_task, ao_lco_arm_warn, "lco arm warn"); ao_add_task(&ao_lco_igniter_status_task, ao_lco_igniter_status, "lco igniter status"); ao_add_task(&ao_lco_drag_task, ao_lco_drag_monitor, "drag race"); - for (;;) { - PRINTD("monitor armed %d firing %d\n", - ao_lco_armed, ao_lco_firing); - - if (ao_lco_armed && ao_lco_firing) { - ao_lco_ignite(AO_PAD_FIRE); - } else { - ao_lco_update(); - if (ao_lco_armed) { - for (box = ao_lco_min_box; box <= ao_lco_max_box; box++) { - if (ao_lco_selected[box]) { - PRINTD("Arming box %d pads %x\n", - box, ao_lco_selected[box]); - if (ao_lco_valid[box] & AO_LCO_VALID_EVER) { - ao_lco_arm(box, ao_lco_selected[box], ao_lco_tick_offset[box]); - ao_delay(AO_MS_TO_TICKS(10)); - } - } - } - } - } - if (ao_lco_armed && ao_lco_firing) - delay = AO_MS_TO_TICKS(100); - else - delay = AO_SEC_TO_TICKS(1); - ao_sleep_for(&ao_lco_armed, delay); - } + ao_lco_monitor(); } #if DEBUG @@ -719,7 +308,7 @@ __code struct ao_cmds ao_lco_cmds[] = { void ao_lco_init(void) { - ao_add_task(&ao_lco_monitor_task, ao_lco_monitor, "lco monitor"); + ao_add_task(&ao_lco_monitor_task, ao_lco_main, "lco monitor"); #if DEBUG ao_cmd_register(&ao_lco_cmds[0]); #endif -- cgit v1.2.3 From 55f817d6ff1a524434fe41ba83e42b8008989cac Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 3 Jul 2018 01:01:05 -0700 Subject: altos: Build telelco-v2.0 by default Signed-off-by: Keith Packard --- src/Makefile | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/Makefile b/src/Makefile index 6bbb2a38..7d84efeb 100644 --- a/src/Makefile +++ b/src/Makefile @@ -39,6 +39,7 @@ ARMM3DIRS=\ telelco-v0.2 telelco-v0.2/flash-loader \ telelco-v0.2-cc1200 telelco-v0.2-cc1200/flash-loader \ telelco-v0.3 telelco-v0.3/flash-loader \ + telelco-v2.0 telelco-v2.0/flash-loader \ teledongle-v3.0 teledongle-v3.0/flash-loader \ teleballoon-v2.0 \ telebt-v3.0 telebt-v3.0/flash-loader \ -- cgit v1.2.3 From 81355cd08a22502d47637f2505b6089d226d0889 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Tue, 3 Jul 2018 01:06:09 -0700 Subject: altos: A bit more LCO code sharing Missed drag race pad toggling in ao_lco_v2.c Signed-off-by: Keith Packard --- src/drivers/ao_lco.c | 2 +- src/telelco-v2.0/ao_lco_v2.c | 12 +++--------- 2 files changed, 4 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_lco.c b/src/drivers/ao_lco.c index 6b270042..f73fb43f 100644 --- a/src/drivers/ao_lco.c +++ b/src/drivers/ao_lco.c @@ -102,7 +102,7 @@ ao_lco_set_voltage(uint16_t decivolts) void ao_lco_set_display(void) { - if (ao_lco_pad == 0 && ao_lco_box != AO_LCO_BOX_DRAG) { + if (ao_lco_pad == AO_LCO_PAD_VOLTAGE && ao_lco_box != AO_LCO_BOX_DRAG) { ao_lco_set_voltage(ao_pad_query.battery); } else { if (ao_lco_box == AO_LCO_BOX_DRAG) diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index 9fefde3b..d5d3d5e4 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -91,7 +91,7 @@ ao_lco_set_voltage(uint16_t decivolts) void ao_lco_set_display(void) { - if (ao_lco_pad == 0) { + if (ao_lco_pad == AO_LCO_PAD_VOLTAGE) { ao_lco_set_voltage(ao_pad_query.battery); } else { ao_lco_set_pad(ao_lco_pad); @@ -213,14 +213,8 @@ ao_lco_input(void) ao_lco_set_firing(event.value); break; case AO_BUTTON_DRAG_SELECT: - if (event.value && ao_lco_drag_race) { - if (ao_lco_pad != 0) { - ao_lco_selected[ao_lco_box] ^= (1 << (ao_lco_pad - 1)); - PRINTD("Toggle box %d pad %d (pads now %x) to drag race\n", - ao_lco_pad, ao_lco_box, ao_lco_selected[ao_lco_box]); - ao_lco_drag_add_beeps(ao_lco_pad); - } - } + if (event.value) + ao_lco_toggle_drag(); break; case AO_BUTTON_DRAG_MODE: if (event.value) -- cgit v1.2.3 From 71a51b4857f5bd49c0cba2578fb54b1d5b2c738b Mon Sep 17 00:00:00 2001 From: Bdale Garbee Date: Wed, 4 Jul 2018 12:13:38 -0600 Subject: add telefireeight-v1.0 to products built by default in src/Makefile --- src/Makefile | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/Makefile b/src/Makefile index 7d84efeb..0dafb5ec 100644 --- a/src/Makefile +++ b/src/Makefile @@ -46,6 +46,7 @@ ARMM3DIRS=\ telebt-v4.0 telebt-v4.0/flash-loader \ telelcotwo-v0.1 telelcotwo-v0.1/flash-loader \ telefiretwo-v0.1 telefiretwo-v0.1/flash-loader \ + telefireeight-v1.0 telefireeight-v1.0/flash-loader ARMM0DIRS=\ easymini-v1.0 easymini-v1.0/flash-loader \ -- cgit v1.2.3 From 3b53a69e47816ee987a409b05a6b0b47891ca816 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 4 Jul 2018 11:18:12 -0700 Subject: altos: Fix telelcotwo build for new ao_lco_bits.c file Lots of code motion; shouldn't have any functional changes. Signed-off-by: Keith Packard --- src/drivers/ao_lco.c | 28 ++---- src/drivers/ao_lco.h | 22 +++- src/drivers/ao_lco_bits.c | 65 ++++++++---- src/drivers/ao_lco_two.c | 226 +++--------------------------------------- src/telelco-v2.0/ao_lco_v2.c | 23 ++--- src/telelcotwo-v0.1/Makefile | 1 + src/telelcotwo-v0.1/ao_pins.h | 2 + 7 files changed, 96 insertions(+), 271 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_lco.c b/src/drivers/ao_lco.c index f73fb43f..2c8c729f 100644 --- a/src/drivers/ao_lco.c +++ b/src/drivers/ao_lco.c @@ -41,12 +41,10 @@ static uint16_t ao_lco_fire_tick; static uint8_t ao_lco_fire_down; -#define AO_LCO_BOX_DRAG 0x1000 - static uint8_t ao_lco_display_mutex; void -ao_lco_set_pad(uint8_t pad) +ao_lco_show_pad(uint8_t pad) { ao_mutex_get(&ao_lco_display_mutex); ao_seven_segment_set(AO_LCO_PAD_DIGIT, pad | (ao_lco_drag_race << 4)); @@ -71,7 +69,7 @@ ao_lco_set_pad(uint8_t pad) (0 << 6)) void -ao_lco_set_box(uint16_t box) +ao_lco_show_box(uint16_t box) { ao_mutex_get(&ao_lco_display_mutex); if (box == AO_LCO_BOX_DRAG) { @@ -85,7 +83,7 @@ ao_lco_set_box(uint16_t box) } void -ao_lco_set_voltage(uint16_t decivolts) +ao_lco_show_voltage(uint16_t decivolts) { uint8_t tens, ones, tenths; @@ -100,16 +98,16 @@ ao_lco_set_voltage(uint16_t decivolts) } void -ao_lco_set_display(void) +ao_lco_show_display(void) { if (ao_lco_pad == AO_LCO_PAD_VOLTAGE && ao_lco_box != AO_LCO_BOX_DRAG) { - ao_lco_set_voltage(ao_pad_query.battery); + ao_lco_show_voltage(ao_pad_query.battery); } else { if (ao_lco_box == AO_LCO_BOX_DRAG) - ao_lco_set_pad(ao_lco_drag_race); + ao_lco_show_pad(ao_lco_drag_race); else - ao_lco_set_pad(ao_lco_pad); - ao_lco_set_box(ao_lco_box); + ao_lco_show_pad(ao_lco_pad); + ao_lco_show_box(ao_lco_box); } } @@ -210,13 +208,7 @@ ao_lco_step_box(int8_t dir) if (new_box == ao_lco_box) break; } while (!ao_lco_box_present(new_box)); - if (ao_lco_box != new_box) { - ao_lco_box = new_box; - ao_lco_pad = 1; - if (ao_lco_box != AO_LCO_BOX_DRAG) - ao_lco_channels[ao_lco_box] = 0; - ao_lco_set_display(); - } + ao_lco_set_box(new_box); } static void @@ -299,7 +291,7 @@ ao_lco_batt_voltage(void) ao_adc_single_get(&packet); decivolt = ao_battery_decivolt(packet.v_batt); - ao_lco_set_voltage(decivolt); + ao_lco_show_voltage(decivolt); ao_delay(AO_MS_TO_TICKS(1000)); } #endif diff --git a/src/drivers/ao_lco.h b/src/drivers/ao_lco.h index d5aace0d..c1b101d2 100644 --- a/src/drivers/ao_lco.h +++ b/src/drivers/ao_lco.h @@ -21,6 +21,14 @@ #include +#ifndef AO_LCO_DRAG +#define AO_LCO_DRAG 1 +#endif + +#if AO_LCO_DRAG +#define AO_LCO_BOX_DRAG 0x1000 +#endif + #define DEBUG 1 #if DEBUG @@ -72,9 +80,15 @@ ao_lco_pad_present(uint8_t box, uint8_t pad); uint8_t ao_lco_pad_first(uint8_t box); +void +ao_lco_set_pad(uint8_t new_pad); + void ao_lco_step_pad(int8_t dir); +void +ao_lco_set_box(uint16_t new_box); + void ao_lco_set_armed(uint8_t armed); @@ -121,16 +135,16 @@ ao_lco_arm_warn(void); */ void -ao_lco_set_pad(uint8_t pad); +ao_lco_show_pad(uint8_t pad); void -ao_lco_set_box(uint16_t box); +ao_lco_show_box(uint16_t box); void -ao_lco_set_voltage(uint16_t decivolts); +ao_lco_show_voltage(uint16_t decivolts); void -ao_lco_set_display(void); +ao_lco_show_display(void); void ao_lco_init(void); diff --git a/src/drivers/ao_lco_bits.c b/src/drivers/ao_lco_bits.c index b2124d1b..9492cf59 100644 --- a/src/drivers/ao_lco_bits.c +++ b/src/drivers/ao_lco_bits.c @@ -170,11 +170,10 @@ ao_lco_update(void) if (ao_lco_get_channels(ao_lco_box, &ao_pad_query) & AO_LCO_VALID_LAST) { if (!(previous_valid & AO_LCO_VALID_EVER)) { if (ao_lco_pad != AO_LCO_PAD_VOLTAGE) - ao_lco_pad = ao_lco_pad_first(ao_lco_box); - ao_lco_set_display(); + ao_lco_set_pad(ao_lco_pad_first(ao_lco_box)); } if (ao_lco_pad == AO_LCO_PAD_VOLTAGE) - ao_lco_set_display(); + ao_lco_show_display(); } } @@ -200,6 +199,28 @@ ao_lco_box_set_present(uint8_t box) ao_lco_box_mask[AO_LCO_MASK_ID(box)] |= 1 << AO_LCO_MASK_SHIFT(box); } +void +ao_lco_set_pad(uint8_t new_pad) +{ + if (new_pad != ao_lco_pad) { + ao_lco_pad = new_pad; + ao_lco_show_display(); + } +} + +void +ao_lco_set_box(uint16_t new_box) +{ + if (ao_lco_box != new_box) { + ao_lco_box = new_box; +#if AO_LCO_DRAG + if (ao_lco_box != AO_LCO_BOX_DRAG) +#endif + ao_lco_channels[ao_lco_box] = 0; + ao_lco_set_pad(1); + } +} + void ao_lco_step_pad(int8_t dir) { @@ -215,10 +236,7 @@ ao_lco_step_pad(int8_t dir) if (new_pad == ao_lco_pad) break; } while (!ao_lco_pad_present(ao_lco_box, new_pad)); - if (new_pad != ao_lco_pad) { - ao_lco_pad = new_pad; - ao_lco_set_display(); - } + ao_lco_set_pad(new_pad); } void @@ -254,17 +272,6 @@ ao_lco_set_firing(uint8_t firing) ao_wakeup(&ao_lco_armed); } -void -ao_lco_toggle_drag(void) -{ - if (ao_lco_drag_race && ao_lco_pad != AO_LCO_PAD_VOLTAGE) { - ao_lco_selected[ao_lco_box] ^= (1 << (ao_lco_pad - 1)); - PRINTD("Toggle box %d pad %d (pads now %x) to drag race\n", - ao_lco_pad, ao_lco_box, ao_lco_selected[ao_lco_box]); - ao_lco_drag_add_beeps(ao_lco_pad); - } -} - void ao_lco_search(void) { @@ -297,8 +304,7 @@ ao_lco_search(void) ao_lco_min_box = ao_lco_max_box = ao_lco_box = 0; memset(ao_lco_valid, 0, sizeof (ao_lco_valid)); memset(ao_lco_channels, 0, sizeof (ao_lco_channels)); - ao_lco_pad = 1; - ao_lco_set_display(); + ao_lco_set_pad(1); } void @@ -336,6 +342,8 @@ ao_lco_monitor(void) } } +#if AO_LCO_DRAG + uint8_t ao_lco_drag_beep_count; static uint8_t ao_lco_drag_beep_on; static uint16_t ao_lco_drag_beep_time; @@ -355,6 +363,18 @@ ao_lco_drag_add_beeps(uint8_t beeps) ao_wakeup(&ao_lco_drag_beep_count); } +/* Toggle current pad in drag set */ +void +ao_lco_toggle_drag(void) +{ + if (ao_lco_drag_race && ao_lco_pad != AO_LCO_PAD_VOLTAGE) { + ao_lco_selected[ao_lco_box] ^= (1 << (ao_lco_pad - 1)); + PRINTD("Toggle box %d pad %d (pads now %x) to drag race\n", + ao_lco_pad, ao_lco_box, ao_lco_selected[ao_lco_box]); + ao_lco_drag_add_beeps(ao_lco_pad); + } +} + /* Check whether it's time to change the beeper status, then either * turn it on or off as necessary and bump the remaining beep counts */ @@ -408,7 +428,7 @@ ao_lco_drag_enable(void) ao_led_on(AO_LED_DRAG); #endif ao_lco_drag_add_beeps(5); - ao_lco_set_display(); + ao_lco_show_display(); } } @@ -423,7 +443,7 @@ ao_lco_drag_disable(void) #endif memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); ao_lco_drag_add_beeps(2); - ao_lco_set_display(); + ao_lco_show_display(); } } @@ -447,6 +467,7 @@ ao_lco_drag_warn_check(uint16_t now, uint16_t delay) } return delay; } +#endif /* AO_LCO_DRAG */ /* task function for beeping while arm is active */ void diff --git a/src/drivers/ao_lco_two.c b/src/drivers/ao_lco_two.c index e2f86745..2fa0a21f 100644 --- a/src/drivers/ao_lco_two.c +++ b/src/drivers/ao_lco_two.c @@ -23,64 +23,13 @@ #include #include -#define DEBUG 1 - -#if DEBUG -static uint8_t ao_lco_debug; -#define DEBUG_EVENT 1 -#define DEBUG_STATUS 2 -#define PRINTD(l, ...) do { if (!(ao_lco_debug & l)) break; printf ("\r%5u %s: ", ao_tick_count, __func__); printf(__VA_ARGS__); flush(); } while(0) -#else -#define PRINTD(l,...) -#endif - -#define AO_LCO_VALID_LAST 1 -#define AO_LCO_VALID_EVER 2 - static uint8_t ao_lco_suspended; -static uint8_t ao_lco_valid; -static uint8_t ao_lco_channels; -static uint16_t ao_lco_tick_offset; - -/* UI values */ -static uint8_t ao_lco_armed; -static uint8_t ao_lco_firing; -static uint8_t ao_lco_box; - -static struct ao_pad_query ao_pad_query; - -/* TeleFireTwo boxes have a single pad */ -#define ao_lco_pad 0 - -static void -ao_lco_set_box(int box) -{ - ao_lco_box = ao_config.pad_box + box; - ao_lco_valid = 0; - ao_lco_armed = 0; - ao_wakeup(&ao_lco_armed); -} - -static void -ao_lco_set_armed(int armed) -{ - uint8_t bit = (1 << ao_lco_pad); - - if (armed) { - ao_lco_armed = bit; - } else { - ao_lco_armed = 0; - } - PRINTD(DEBUG_EVENT, "pad %d bit 0x%x armed %d ao_lco_armed 0x%x\n", - ao_lco_pad, bit, armed, ao_lco_armed); - ao_wakeup(&ao_lco_armed); -} static void ao_lco_suspend(void) { if (!ao_lco_suspended) { - PRINTD(DEBUG_EVENT, "suspend\n"); + PRINTD("suspend\n"); ao_lco_suspended = 1; ao_lco_armed = 0; ao_wakeup(&ao_pad_query); @@ -96,6 +45,11 @@ ao_lco_wakeup(void) } } +void +ao_lco_show_display(void) +{ +} + static void ao_lco_input(void) { @@ -113,23 +67,21 @@ ao_lco_input(void) ao_event_get(&event); } ao_lco_wakeup(); - PRINTD(DEBUG_EVENT, "event type %d unit %d value %d\n", + PRINTD("event type %d unit %d value %d\n", event.type, event.unit, event.value); switch (event.type) { case AO_EVENT_BUTTON: switch (event.unit) { case AO_BUTTON_BOX: ao_lco_set_box(event.value); + ao_lco_set_armed(0); break; case AO_BUTTON_ARM: ao_lco_set_armed(event.value); break; case AO_BUTTON_FIRE: - if (ao_lco_armed) { - ao_lco_firing = event.value; - PRINTD(DEBUG_EVENT, "Firing %d\n", ao_lco_firing); - ao_wakeup(&ao_lco_armed); - } + if (ao_lco_armed) + ao_lco_set_firing(event.value); break; } break; @@ -137,140 +89,14 @@ ao_lco_input(void) } } -static AO_LED_TYPE continuity_led[AO_LED_CONTINUITY_NUM] = { -#ifdef AO_LED_CONTINUITY_0 - AO_LED_CONTINUITY_0, -#endif -#ifdef AO_LED_CONTINUITY_1 - AO_LED_CONTINUITY_1, -#endif -#ifdef AO_LED_CONTINUITY_2 - AO_LED_CONTINUITY_2, -#endif -#ifdef AO_LED_CONTINUITY_3 - AO_LED_CONTINUITY_3, -#endif -#ifdef AO_LED_CONTINUITY_4 - AO_LED_CONTINUITY_4, -#endif -#ifdef AO_LED_CONTINUITY_5 - AO_LED_CONTINUITY_5, -#endif -#ifdef AO_LED_CONTINUITY_6 - AO_LED_CONTINUITY_6, -#endif -#ifdef AO_LED_CONTINUITY_7 - AO_LED_CONTINUITY_7, -#endif -}; - -static uint8_t -ao_lco_get_channels(void) -{ - int8_t r; - - r = ao_lco_query(ao_lco_box, &ao_pad_query, &ao_lco_tick_offset); - if (r == AO_RADIO_CMAC_OK) { - ao_lco_channels = ao_pad_query.channels; - ao_lco_valid = AO_LCO_VALID_LAST | AO_LCO_VALID_EVER; - } else - ao_lco_valid &= ~AO_LCO_VALID_LAST; - PRINTD(DEBUG_STATUS, "ao_lco_get_channels() rssi %d valid %d ret %d offset %d\n", ao_radio_cmac_rssi, ao_lco_valid, r, ao_lco_tick_offset); - ao_wakeup(&ao_pad_query); - return ao_lco_valid; -} - -static void -ao_lco_igniter_status(void) -{ - uint8_t c; - uint8_t t = 0; - - for (;;) { - uint8_t all_status; - ao_sleep(&ao_pad_query); - while (ao_lco_suspended) { - ao_led_off(AO_LED_GREEN|AO_LED_AMBER|AO_LED_RED|AO_LED_REMOTE_ARM); - for (c = 0; c < AO_LED_CONTINUITY_NUM; c++) - ao_led_off(continuity_led[c]); - ao_sleep(&ao_lco_suspended); - } - PRINTD(DEBUG_STATUS, "RSSI %d VALID %d channels %d arm_status %d\n", - ao_radio_cmac_rssi, ao_lco_valid, - ao_lco_channels, ao_pad_query.arm_status); - if (!(ao_lco_valid & AO_LCO_VALID_LAST)) { - ao_led_on(AO_LED_RED); - ao_led_off(AO_LED_GREEN|AO_LED_AMBER); - memset(&ao_pad_query, '\0', sizeof (ao_pad_query)); - } else if (ao_radio_cmac_rssi < -90) { - ao_led_on(AO_LED_AMBER); - ao_led_off(AO_LED_RED|AO_LED_GREEN); - } else { - ao_led_on(AO_LED_GREEN); - ao_led_off(AO_LED_RED|AO_LED_AMBER); - } - if (ao_pad_query.arm_status) - ao_led_on(AO_LED_REMOTE_ARM); - else - ao_led_off(AO_LED_REMOTE_ARM); - - all_status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN; - for (c = 0; c < 8; c++) { - if (ao_pad_query.channels & (1 << c)) { - uint8_t status = ao_pad_query.igniter_status[c]; - if (status > all_status) - all_status = status; - PRINTD(DEBUG_STATUS, "\tchannel %d status %d\n", c, status); - } - } - for (c = 0; c < AO_LED_CONTINUITY_NUM; c++) { - uint8_t on = 0; - if (c == (ao_lco_box - ao_config.pad_box) % AO_LED_CONTINUITY_NUM) { - switch (all_status) { - case AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN: - on = 1; - break; - case AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_CLOSED: - case AO_PAD_IGNITER_STATUS_UNKNOWN: - on = t & 1; - } - } - if (on) - ao_led_on(continuity_led[c]); - else - ao_led_off(continuity_led[c]); - } - t = 1-t; - } -} - -static void -ao_lco_arm_warn(void) -{ - int i; - for (;;) { - while (ao_lco_suspended) - ao_sleep(&ao_lco_suspended); - while (!ao_lco_armed) - ao_sleep(&ao_lco_armed); - for (i = 0; i < ao_lco_armed; i++) { - ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(100)); - ao_delay(AO_MS_TO_TICKS(100)); - } - ao_delay(AO_MS_TO_TICKS(300)); - } -} - static struct ao_task ao_lco_input_task; static struct ao_task ao_lco_monitor_task; static struct ao_task ao_lco_arm_warn_task; static struct ao_task ao_lco_igniter_status_task; static void -ao_lco_monitor(void) +ao_lco_main(void) { - uint16_t delay; - ao_config_get(); ao_lco_set_box(ao_button_get(AO_BUTTON_BOX)); ao_add_task(&ao_lco_input_task, ao_lco_input, "lco input"); @@ -279,33 +105,7 @@ ao_lco_monitor(void) ao_led_on(~0); ao_beep_for(AO_BEEP_MID, AO_MS_TO_TICKS(200)); ao_led_off(~0); - for (;;) { - while (ao_lco_suspended) - ao_sleep(&ao_lco_suspended); - - PRINTD(DEBUG_STATUS, "monitor armed %d firing %d\n", - ao_lco_armed, ao_lco_firing); - - if (ao_lco_armed && ao_lco_firing) { - ao_lco_ignite(AO_PAD_FIRE); - } else { - ao_lco_get_channels(); - if (ao_lco_armed) { - PRINTD(DEBUG_STATUS, "Arming pads %x\n", - ao_lco_armed); - if (ao_lco_valid & AO_LCO_VALID_EVER) { - ao_lco_arm(ao_lco_box, ao_lco_armed, ao_lco_tick_offset); - ao_delay(AO_MS_TO_TICKS(10)); - } - } - } - if (ao_lco_armed && ao_lco_firing) - delay = AO_MS_TO_TICKS(100); - else { - delay = AO_SEC_TO_TICKS(1); - } - ao_sleep_for(&ao_lco_armed, delay); - } + ao_lco_monitor(); } #if DEBUG @@ -326,7 +126,7 @@ __code struct ao_cmds ao_lco_cmds[] = { void ao_lco_init(void) { - ao_add_task(&ao_lco_monitor_task, ao_lco_monitor, "lco monitor"); + ao_add_task(&ao_lco_monitor_task, ao_lco_main, "lco monitor"); #if DEBUG ao_cmd_register(&ao_lco_cmds[0]); #endif diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index d5d3d5e4..f64a7745 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -39,7 +39,7 @@ static uint8_t ao_lco_select_mode; static uint8_t ao_lco_display_mutex; void -ao_lco_set_pad(uint8_t pad) +ao_lco_show_pad(uint8_t pad) { ao_mutex_get(&ao_lco_display_mutex); ao_seven_segment_set(AO_LCO_PAD_DIGIT, pad | (ao_lco_drag_race << 4)); @@ -64,7 +64,7 @@ ao_lco_set_pad(uint8_t pad) (0 << 6)) void -ao_lco_set_box(uint16_t box) +ao_lco_show_box(uint16_t box) { ao_mutex_get(&ao_lco_display_mutex); ao_seven_segment_set(AO_LCO_BOX_DIGIT_1, box % 10 | (ao_lco_drag_race << 4)); @@ -73,7 +73,7 @@ ao_lco_set_box(uint16_t box) } void -ao_lco_set_voltage(uint16_t decivolts) +ao_lco_show_voltage(uint16_t decivolts) { uint8_t tens, ones, tenths; @@ -89,13 +89,13 @@ ao_lco_set_voltage(uint16_t decivolts) } void -ao_lco_set_display(void) +ao_lco_show_display(void) { if (ao_lco_pad == AO_LCO_PAD_VOLTAGE) { - ao_lco_set_voltage(ao_pad_query.battery); + ao_lco_show_voltage(ao_pad_query.battery); } else { - ao_lco_set_pad(ao_lco_pad); - ao_lco_set_box(ao_lco_box); + ao_lco_show_pad(ao_lco_pad); + ao_lco_show_box(ao_lco_box); } } @@ -143,12 +143,7 @@ ao_lco_step_box(int8_t dir) if (new_box == ao_lco_box) break; } while (!ao_lco_box_present(new_box)); - if (ao_lco_box != new_box) { - ao_lco_box = new_box; - ao_lco_pad = 1; - ao_lco_channels[ao_lco_box] = 0; - ao_lco_set_display(); - } + ao_lco_set_box(new_box); } static struct ao_task ao_lco_drag_task; @@ -261,7 +256,7 @@ ao_lco_batt_voltage(void) ao_adc_single_get(&packet); decivolt = ao_battery_decivolt(packet.v_batt); - ao_lco_set_voltage(decivolt); + ao_lco_show_voltage(decivolt); ao_delay(AO_MS_TO_TICKS(1000)); } diff --git a/src/telelcotwo-v0.1/Makefile b/src/telelcotwo-v0.1/Makefile index 42188bb2..c68f3eb5 100644 --- a/src/telelcotwo-v0.1/Makefile +++ b/src/telelcotwo-v0.1/Makefile @@ -58,6 +58,7 @@ ALTOS_SRC = \ ao_button.c \ ao_event.c \ ao_lco_two.c \ + ao_lco_bits.c \ ao_lco_func.c \ ao_lco_cmd.c \ ao_radio_cmac_cmd.c diff --git a/src/telelcotwo-v0.1/ao_pins.h b/src/telelcotwo-v0.1/ao_pins.h index 60e94c67..1941e03d 100644 --- a/src/telelcotwo-v0.1/ao_pins.h +++ b/src/telelcotwo-v0.1/ao_pins.h @@ -141,6 +141,8 @@ AO_LED_CONTINUITY_0 | \ AO_LED_REMOTE_ARM) +#define AO_LCO_DRAG 0 + /* * Use event queue for input devices */ -- cgit v1.2.3 From 2214ad7c0f268be76c0b027eef9d1dc406f23b28 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 4 Jul 2018 11:27:31 -0700 Subject: altos: Fix LCO display while searching Use lower level functions to control the display more directly Signed-off-by: Keith Packard --- src/drivers/ao_lco_bits.c | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_lco_bits.c b/src/drivers/ao_lco_bits.c index 9492cf59..6c5182f2 100644 --- a/src/drivers/ao_lco_bits.c +++ b/src/drivers/ao_lco_bits.c @@ -202,23 +202,20 @@ ao_lco_box_set_present(uint8_t box) void ao_lco_set_pad(uint8_t new_pad) { - if (new_pad != ao_lco_pad) { - ao_lco_pad = new_pad; - ao_lco_show_display(); - } + ao_lco_pad = new_pad; + ao_lco_show_display(); } void ao_lco_set_box(uint16_t new_box) { - if (ao_lco_box != new_box) { - ao_lco_box = new_box; + ao_lco_box = new_box; #if AO_LCO_DRAG - if (ao_lco_box != AO_LCO_BOX_DRAG) + if (ao_lco_box != AO_LCO_BOX_DRAG) #endif - ao_lco_channels[ao_lco_box] = 0; - ao_lco_set_pad(1); - } + ao_lco_channels[ao_lco_box] = 0; + ao_lco_pad = 1; + ao_lco_show_display(); } void @@ -283,8 +280,10 @@ ao_lco_search(void) ao_lco_box_reset_present(); ao_lco_set_pad(0); for (box = 0; box < AO_PAD_MAX_BOXES; box++) { - if ((box % 10) == 0) - ao_lco_set_box(box); + if ((box % 10) == 0) { + ao_lco_box = box; + ao_lco_show_display(); + } for (try = 0; try < 3; try++) { ao_lco_tick_offset[box] = 0; r = ao_lco_query(box, &ao_pad_query, &ao_lco_tick_offset[box]); @@ -304,7 +303,7 @@ ao_lco_search(void) ao_lco_min_box = ao_lco_max_box = ao_lco_box = 0; memset(ao_lco_valid, 0, sizeof (ao_lco_valid)); memset(ao_lco_channels, 0, sizeof (ao_lco_channels)); - ao_lco_set_pad(1); + ao_lco_set_box(ao_lco_min_box); } void -- cgit v1.2.3 From 38fb80e5e6af87f8c734448e4aa69b2c39854903 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 4 Jul 2018 11:58:40 -0700 Subject: altos: Make LCO search directly drive pad/box displays Don't use ao_lco_show as that has other logic too. Signed-off-by: Keith Packard --- src/drivers/ao_lco.c | 4 ++-- src/drivers/ao_lco.h | 5 +---- src/drivers/ao_lco_bits.c | 21 ++++++++++----------- src/drivers/ao_lco_two.c | 14 +++++++++++++- src/telelco-v2.0/ao_lco_v2.c | 4 ++-- 5 files changed, 28 insertions(+), 20 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_lco.c b/src/drivers/ao_lco.c index 2c8c729f..47752f1f 100644 --- a/src/drivers/ao_lco.c +++ b/src/drivers/ao_lco.c @@ -82,7 +82,7 @@ ao_lco_show_box(uint16_t box) ao_mutex_put(&ao_lco_display_mutex); } -void +static void ao_lco_show_voltage(uint16_t decivolts) { uint8_t tens, ones, tenths; @@ -98,7 +98,7 @@ ao_lco_show_voltage(uint16_t decivolts) } void -ao_lco_show_display(void) +ao_lco_show(void) { if (ao_lco_pad == AO_LCO_PAD_VOLTAGE && ao_lco_box != AO_LCO_BOX_DRAG) { ao_lco_show_voltage(ao_pad_query.battery); diff --git a/src/drivers/ao_lco.h b/src/drivers/ao_lco.h index c1b101d2..e528c2ad 100644 --- a/src/drivers/ao_lco.h +++ b/src/drivers/ao_lco.h @@ -141,10 +141,7 @@ void ao_lco_show_box(uint16_t box); void -ao_lco_show_voltage(uint16_t decivolts); - -void -ao_lco_show_display(void); +ao_lco_show(void); void ao_lco_init(void); diff --git a/src/drivers/ao_lco_bits.c b/src/drivers/ao_lco_bits.c index 6c5182f2..197bf8aa 100644 --- a/src/drivers/ao_lco_bits.c +++ b/src/drivers/ao_lco_bits.c @@ -173,7 +173,7 @@ ao_lco_update(void) ao_lco_set_pad(ao_lco_pad_first(ao_lco_box)); } if (ao_lco_pad == AO_LCO_PAD_VOLTAGE) - ao_lco_show_display(); + ao_lco_show(); } } @@ -203,7 +203,7 @@ void ao_lco_set_pad(uint8_t new_pad) { ao_lco_pad = new_pad; - ao_lco_show_display(); + ao_lco_show(); } void @@ -215,7 +215,7 @@ ao_lco_set_box(uint16_t new_box) #endif ao_lco_channels[ao_lco_box] = 0; ao_lco_pad = 1; - ao_lco_show_display(); + ao_lco_show(); } void @@ -278,12 +278,11 @@ ao_lco_search(void) uint8_t boxes = 0; ao_lco_box_reset_present(); - ao_lco_set_pad(0); + ao_lco_show_box(0); + ao_lco_show_pad(0); for (box = 0; box < AO_PAD_MAX_BOXES; box++) { - if ((box % 10) == 0) { - ao_lco_box = box; - ao_lco_show_display(); - } + if ((box % 10) == 0) + ao_lco_show_box(box); for (try = 0; try < 3; try++) { ao_lco_tick_offset[box] = 0; r = ao_lco_query(box, &ao_pad_query, &ao_lco_tick_offset[box]); @@ -291,7 +290,7 @@ ao_lco_search(void) if (r == AO_RADIO_CMAC_OK) { ++boxes; ao_lco_box_set_present(box); - ao_lco_set_pad(boxes % 10); + ao_lco_show_pad(boxes % 10); ao_delay(AO_MS_TO_TICKS(30)); break; } @@ -427,7 +426,7 @@ ao_lco_drag_enable(void) ao_led_on(AO_LED_DRAG); #endif ao_lco_drag_add_beeps(5); - ao_lco_show_display(); + ao_lco_show(); } } @@ -442,7 +441,7 @@ ao_lco_drag_disable(void) #endif memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); ao_lco_drag_add_beeps(2); - ao_lco_show_display(); + ao_lco_show(); } } diff --git a/src/drivers/ao_lco_two.c b/src/drivers/ao_lco_two.c index 2fa0a21f..6f2d81ff 100644 --- a/src/drivers/ao_lco_two.c +++ b/src/drivers/ao_lco_two.c @@ -46,7 +46,19 @@ ao_lco_wakeup(void) } void -ao_lco_show_display(void) +ao_lco_show_pad(uint8_t pad) +{ + (void) pad; +} + +void +ao_lco_show_box(uint16_t box) +{ + (void) box; +} + +void +ao_lco_show(void) { } diff --git a/src/telelco-v2.0/ao_lco_v2.c b/src/telelco-v2.0/ao_lco_v2.c index f64a7745..a9933d59 100644 --- a/src/telelco-v2.0/ao_lco_v2.c +++ b/src/telelco-v2.0/ao_lco_v2.c @@ -72,7 +72,7 @@ ao_lco_show_box(uint16_t box) ao_mutex_put(&ao_lco_display_mutex); } -void +static void ao_lco_show_voltage(uint16_t decivolts) { uint8_t tens, ones, tenths; @@ -89,7 +89,7 @@ ao_lco_show_voltage(uint16_t decivolts) } void -ao_lco_show_display(void) +ao_lco_show(void) { if (ao_lco_pad == AO_LCO_PAD_VOLTAGE) { ao_lco_show_voltage(ao_pad_query.battery); -- cgit v1.2.3 From ad9d6677231ccbfa09a528387f306f5364f9d608 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 4 Jul 2018 12:34:38 -0700 Subject: altos: Make fewer LCO variables public The lco common code doesn't need to share its internal values with the world; make most of them static. Signed-off-by: Keith Packard --- src/drivers/ao_lco.c | 3 +-- src/drivers/ao_lco.h | 18 +++++------------- src/drivers/ao_lco_bits.c | 42 ++++++++++++++++++++++++++++-------------- 3 files changed, 34 insertions(+), 29 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_lco.c b/src/drivers/ao_lco.c index 47752f1f..e892c8c0 100644 --- a/src/drivers/ao_lco.c +++ b/src/drivers/ao_lco.c @@ -34,8 +34,7 @@ #define AO_LCO_DRAG_RACE_START_TIME AO_SEC_TO_TICKS(5) #define AO_LCO_DRAG_RACE_STOP_TIME AO_SEC_TO_TICKS(2) -#define AO_LCO_VALID_LAST 1 -#define AO_LCO_VALID_EVER 2 +#define AO_LCO_BOX_DRAG 0x1000 /* UI values */ static uint16_t ao_lco_fire_tick; diff --git a/src/drivers/ao_lco.h b/src/drivers/ao_lco.h index e528c2ad..2958fbcc 100644 --- a/src/drivers/ao_lco.h +++ b/src/drivers/ao_lco.h @@ -25,10 +25,6 @@ #define AO_LCO_DRAG 1 #endif -#if AO_LCO_DRAG -#define AO_LCO_BOX_DRAG 0x1000 -#endif - #define DEBUG 1 #if DEBUG @@ -38,25 +34,21 @@ extern uint8_t ao_lco_debug; #define PRINTD(...) #endif +#if AO_LCO_DRAG extern uint8_t ao_lco_drag_race; /* TRUE when drag race mode enabled */ +#endif + extern uint8_t ao_lco_pad; /* Currently selected pad */ extern int16_t ao_lco_box; /* Currently selected box */ -extern uint8_t ao_lco_armed; -extern uint8_t ao_lco_firing; +extern uint8_t ao_lco_armed; /* armed mode active */ +extern uint8_t ao_lco_firing; /* fire button pressed */ extern struct ao_pad_query ao_pad_query; /* Last received QUERY from pad */ -#define AO_LCO_VALID_LAST 1 -#define AO_LCO_VALID_EVER 2 - #define AO_LCO_PAD_VOLTAGE 0 /* Pad number to show box voltage */ extern uint8_t ao_lco_min_box, ao_lco_max_box; -extern uint8_t ao_lco_selected[AO_PAD_MAX_BOXES]; -extern uint8_t ao_lco_valid[AO_PAD_MAX_BOXES]; -extern uint8_t ao_lco_channels[AO_PAD_MAX_BOXES]; -extern uint16_t ao_lco_tick_offset[AO_PAD_MAX_BOXES]; #define AO_LCO_MASK_SIZE(n) (((n) + 7) >> 3) #define AO_LCO_MASK_ID(n) ((n) >> 3) diff --git a/src/drivers/ao_lco_bits.c b/src/drivers/ao_lco_bits.c index 197bf8aa..6e50e44d 100644 --- a/src/drivers/ao_lco_bits.c +++ b/src/drivers/ao_lco_bits.c @@ -20,20 +20,28 @@ uint8_t ao_lco_debug; uint8_t ao_lco_pad; int16_t ao_lco_box; -uint8_t ao_lco_drag_race; -uint8_t ao_lco_armed; /* arm active */ -uint8_t ao_lco_firing; /* fire active */ +uint8_t ao_lco_armed; /* arm active */ +uint8_t ao_lco_firing; /* fire active */ uint8_t ao_lco_min_box, ao_lco_max_box; -uint8_t ao_lco_selected[AO_PAD_MAX_BOXES]; -uint8_t ao_lco_valid[AO_PAD_MAX_BOXES]; -uint8_t ao_lco_channels[AO_PAD_MAX_BOXES]; -uint16_t ao_lco_tick_offset[AO_PAD_MAX_BOXES]; -struct ao_pad_query ao_pad_query; +#if AO_LCO_DRAG +uint8_t ao_lco_drag_race; +#endif + +struct ao_pad_query ao_pad_query; /* latest query response */ -static AO_LED_TYPE continuity_led[AO_LED_CONTINUITY_NUM] = { +static uint8_t ao_lco_channels[AO_PAD_MAX_BOXES]; /* pad channels available on each box */ +static uint16_t ao_lco_tick_offset[AO_PAD_MAX_BOXES]; /* offset from local to remote tick count */ +static uint8_t ao_lco_selected[AO_PAD_MAX_BOXES]; /* pads selected to fire */ + +#define AO_LCO_VALID_LAST 1 +#define AO_LCO_VALID_EVER 2 + +static uint8_t ao_lco_valid[AO_PAD_MAX_BOXES]; /* AO_LCO_VALID bits per box */ + +static const AO_LED_TYPE continuity_led[AO_LED_CONTINUITY_NUM] = { #ifdef AO_LED_CONTINUITY_0 AO_LED_CONTINUITY_0, #endif @@ -68,9 +76,11 @@ ao_lco_igniter_status(void) uint8_t t = 0; for (;;) { +#if AO_LCO_DRAG if (ao_lco_drag_race) ao_sleep_for(&ao_pad_query, AO_MS_TO_TICKS(50)); else +#endif ao_sleep(&ao_pad_query); PRINTD("RSSI %d VALID %d\n", ao_radio_cmac_rssi, ao_lco_valid[ao_lco_box]); if (!(ao_lco_valid[ao_lco_box] & AO_LCO_VALID_LAST)) { @@ -98,6 +108,7 @@ ao_lco_igniter_status(void) else status = AO_PAD_IGNITER_STATUS_NO_IGNITER_RELAY_OPEN; +#if AO_LCO_DRAG if (ao_lco_drag_race && (ao_lco_selected[ao_lco_box] & (1 << c))) { uint8_t on = 0; if (status == AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN) { @@ -111,7 +122,9 @@ ao_lco_igniter_status(void) ao_led_on(continuity_led[c]); else ao_led_off(continuity_led[c]); - } else { + } else +#endif + { if (status == AO_PAD_IGNITER_STATUS_GOOD_IGNITER_RELAY_OPEN) ao_led_on(continuity_led[c]); else @@ -210,9 +223,7 @@ void ao_lco_set_box(uint16_t new_box) { ao_lco_box = new_box; -#if AO_LCO_DRAG - if (ao_lco_box != AO_LCO_BOX_DRAG) -#endif + if (ao_lco_box < AO_PAD_MAX_BOXES) ao_lco_channels[ao_lco_box] = 0; ao_lco_pad = 1; ao_lco_show(); @@ -242,6 +253,7 @@ ao_lco_set_armed(uint8_t armed) ao_lco_armed = armed; PRINTD("Armed %d\n", ao_lco_armed); if (ao_lco_armed) { +#if AO_LCO_DRAG if (ao_lco_drag_race) { uint8_t box; @@ -250,7 +262,9 @@ ao_lco_set_armed(uint8_t armed) break; if (box > ao_lco_max_box) ao_lco_armed = 0; - } else { + } else +#endif + { memset(ao_lco_selected, 0, sizeof (ao_lco_selected)); if (ao_lco_pad != 0) ao_lco_selected[ao_lco_box] = (1 << (ao_lco_pad - 1)); -- cgit v1.2.3 From 323a79e6ed3dfdd2afadfc2a464bb0610380b03b Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 4 Jul 2018 16:08:31 -0700 Subject: altos: Fetch current quadrature status at startup time This avoids having the state appear to change the first time we look at the device. Signed-off-by: Keith Packard --- src/drivers/ao_quadrature.c | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/drivers/ao_quadrature.c b/src/drivers/ao_quadrature.c index 59e84518..250e035f 100644 --- a/src/drivers/ao_quadrature.c +++ b/src/drivers/ao_quadrature.c @@ -111,7 +111,8 @@ static const struct { }; static void -_ao_quadrature_set(uint8_t q, uint8_t new) { +_ao_quadrature_set(uint8_t q, uint8_t new) +{ uint8_t old; ao_arch_block_interrupts(); @@ -136,6 +137,14 @@ ao_quadrature_isr(void) #endif } +static void +_ao_quadrature_start_one(uint8_t q, uint8_t new) +{ + ao_arch_block_interrupts(); + ao_quadrature_state[q] = new; + ao_arch_release_interrupts(); +} + int32_t ao_quadrature_poll(uint8_t q) { @@ -204,9 +213,10 @@ static const struct ao_cmds ao_quadrature_cmds[] = { { 0, NULL } }; -#define init(q) do { \ - ao_enable_input(port(q), bita(q), 0); \ - ao_enable_input(port(q), bitb(q), 0); \ +#define init(q) do { \ + ao_enable_input(port(q), bita(q), 0); \ + ao_enable_input(port(q), bitb(q), 0); \ + _ao_quadrature_start_one(q, _ao_quadrature_get(q)); \ } while (0) void -- cgit v1.2.3 From 327b765962d397efd4c45b6209c9225a4d23ba1d Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 5 Aug 2018 08:44:04 +0800 Subject: altos: Change 'after motor' pyro check to be >= instead of == This makes after motor stay valid even if further motors burn. Signed-off-by: Keith Packard --- doc/pyro-channels.inc | 4 +++- src/kernel/ao_pyro.c | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/doc/pyro-channels.inc b/doc/pyro-channels.inc index 7fd06412..68bbf910 100644 --- a/doc/pyro-channels.inc +++ b/doc/pyro-channels.inc @@ -57,7 +57,9 @@ cleared and must be reconfigured by the user. After Motor:: The flight software counts each time the rocket starts accelerating and then decelerating (presumably due to a motor or motors burning). Use this value for multi-staged or multi-airstart -launches. +launches. As of version 1.8.6 firmware, this checks to make sure at +least this many motors have burned. Before version 1.8.6, this checked +to make sure that exactly this many motors had burned. Delay:: Once the other parameters all become true, a timer is started for the specified amount of time. While the timer is running, diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index 5a556d59..3c872354 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -182,7 +182,7 @@ ao_pyro_ready(struct ao_pyro *pyro) break; case ao_pyro_after_motor: - if (ao_motor_number == pyro->motor) + if (ao_motor_number >= pyro->motor) continue; DBG("motor %d != %d\n", ao_motor_number, pyro->motor); break; -- cgit v1.2.3 From 50c6d796e0dbb69289f95c826e6b542e538fc5de Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 5 Aug 2018 08:45:30 +0800 Subject: altos/stm-demo: Set up for dumping eeprom This is used to dump an eeprom wired to the stm demo board. Signed-off-by: Keith Packard --- src/stm-demo/Makefile | 2 ++ src/stm-demo/ao_demo.c | 3 ++- src/stm-demo/ao_pins.h | 14 ++++++++++++-- 3 files changed, 16 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/stm-demo/Makefile b/src/stm-demo/Makefile index 869fb32f..d4569c1a 100644 --- a/src/stm-demo/Makefile +++ b/src/stm-demo/Makefile @@ -30,6 +30,8 @@ ALTOS_SRC = \ ao_lcd_stm.c \ ao_lcd_font.c \ ao_mutex.c \ + ao_storage.c \ + ao_m25.c \ ao_dma_stm.c \ ao_spi_stm.c \ ao_adc_stm.c \ diff --git a/src/stm-demo/ao_demo.c b/src/stm-demo/ao_demo.c index db432c2a..f6c8e3df 100644 --- a/src/stm-demo/ao_demo.c +++ b/src/stm-demo/ao_demo.c @@ -223,11 +223,12 @@ main(void) ao_cmd_init(); // ao_lcd_stm_init(); // ao_lcd_font_init(); -// ao_spi_init(); + ao_spi_init(); // ao_i2c_init(); // ao_exti_init(); // ao_quadrature_init(); // ao_button_init(); + ao_storage_init(); // ao_timer_set_adc_interval(100); diff --git a/src/stm-demo/ao_pins.h b/src/stm-demo/ao_pins.h index 233537df..2b4c5ab0 100644 --- a/src/stm-demo/ao_pins.h +++ b/src/stm-demo/ao_pins.h @@ -59,11 +59,14 @@ #define SERIAL_3_PC10_PC11 0 #define SERIAL_3_PD8_PD9 1 -#define HAS_SPI_1 1 +#define HAS_SPI_1 0 #define SPI_1_PB3_PB4_PB5 1 #define SPI_1_OSPEEDR STM_OSPEEDR_10MHz -#define HAS_SPI_2 0 +#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 HAS_USB 1 #define HAS_BEEP 0 @@ -200,4 +203,11 @@ struct ao_adc { #define AO_TICK_TYPE uint32_t #define AO_TICK_SIGNED int32_t +#define M25_MAX_CHIPS 1 +#define AO_M25_SPI_CS_PORT (&stm_gpiob) +#define AO_M25_SPI_CS_MASK (1 << 12) +#define AO_M25_SPI_BUS AO_SPI_2_PB13_PB14_PB15 + +#define AO_LOG_FORMAT AO_LOG_FORMAT_TELEMEGA + #endif /* _AO_PINS_H_ */ -- cgit v1.2.3 From 9dfbf0103a649816e98d5511b1d6bbbfc93f6632 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 5 Aug 2018 09:03:35 +0800 Subject: altos/plottest: Make linewidth bigger 1-pixel lines are harsh in 2018 Signed-off-by: Keith Packard --- src/test/plottest | 34 ++++++++++++++++++++++++---------- 1 file changed, 24 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/test/plottest b/src/test/plottest index e427604a..5b30cfa5 100755 --- a/src/test/plottest +++ b/src/test/plottest @@ -1,3 +1,7 @@ +#!/bin/bash + +LINEWIDTH=2 + gnuplot -persist << EOF set title "$1" set ylabel "altitude (m)" @@ -6,14 +10,24 @@ 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:5 with lines axes x1y2 title "raw accel",\ -"$1" using 1:7 with lines axes x1y2 title "accel speed",\ -"$1" using 1:11 with lines axes x1y1 title "height",\ -"$1" using 1:13 with lines axes x1y2 title "speed",\ -"$1" using 1:15 with lines axes x1y2 title "accel",\ -"$1" using 1:19 with lines axes x1y1 title "drogue",\ -"$1" using 1:21 with lines axes x1y1 title "main",\ -"$1" using 1:23 with lines axes x1y1 title "error",\ -"$1" using 1:9 with lines axes x1y2 title "state" +set style line 1 linewidth $LINEWIDTH +set style line 2 linewidth $LINEWIDTH +set style line 3 linewidth $LINEWIDTH +set style line 4 linewidth $LINEWIDTH +set style line 5 linewidth $LINEWIDTH +set style line 6 linewidth $LINEWIDTH +set style line 7 linewidth $LINEWIDTH dashtype 2 +set style line 8 linewidth $LINEWIDTH dashtype 2 +set style line 9 linewidth $LINEWIDTH dashtype 2 +set style line 10 linewidth $LINEWIDTH dashtype 2 +plot "$1" using 1:3 with lines axes x1y1 title "raw height" ls 1,\ +"$1" using 1:5 with lines axes x1y2 title "raw accel" ls 2,\ +"$1" using 1:7 with lines axes x1y2 title "accel speed" ls 3,\ +"$1" using 1:11 with lines axes x1y1 title "height" ls 4,\ +"$1" using 1:13 with lines axes x1y2 title "speed" ls 5,\ +"$1" using 1:15 with lines axes x1y2 title "accel" ls 6,\ +"$1" using 1:19 with lines axes x1y1 title "drogue" ls 7,\ +"$1" using 1:21 with lines axes x1y1 title "main" ls 8,\ +"$1" using 1:23 with lines axes x1y1 title "error" ls 9,\ +"$1" using 1:9 with lines axes x1y2 title "state" ls 10 EOF -- cgit v1.2.3 From 0d57c78dde3c6e61576a4769b0e0fae7e88c107d Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 5 Aug 2018 11:09:34 +0800 Subject: altos: Add separate 'ao_launch_tick'. Use in pyro and lockout. Prior to this, there was only ao_boost_tick, which got reset at each motor burn start. That meant there wasn't any way to measure total flight time for pyro channels and 'apogee lockout' was based on time since most recent motor start instead of total flight time. Now pyro channels and apogee lockout both use total flight time, while motor burn length still uses time since most recent motor burn start (as it should). Docs and UI updated to use 'launch' instead of 'boost' to try and make the change clear. Signed-off-by: Keith Packard --- altoslib/AltosPyro.java | 4 ++-- altosui/AltosConfigFCUI.java | 2 +- doc/config-device.inc | 8 +++++++- doc/pyro-channels.inc | 10 +++++++--- src/kernel/ao_flight.c | 7 ++++--- src/kernel/ao_flight.h | 1 + src/kernel/ao_pyro.c | 8 ++++---- 7 files changed, 26 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/altoslib/AltosPyro.java b/altoslib/AltosPyro.java index 18f0da56..fea4fd59 100644 --- a/altoslib/AltosPyro.java +++ b/altoslib/AltosPyro.java @@ -61,8 +61,8 @@ public class AltosPyro { public static final int pyro_time_greater = 0x00000200; public static final String pyro_time_less_string = "t<"; public static final String pyro_time_greater_string = "t>"; - public static final String pyro_time_less_name = "Time since boost less than (s)"; - public static final String pyro_time_greater_name = "Time since boost greater than (s)"; + public static final String pyro_time_less_name = "Time since launch less than (s)"; + public static final String pyro_time_greater_name = "Time since launch greater than (s)"; public static final double pyro_time_scale = 100.0; public static final int pyro_ascending = 0x00000400; diff --git a/altosui/AltosConfigFCUI.java b/altosui/AltosConfigFCUI.java index 1e875dec..9bd265f0 100644 --- a/altosui/AltosConfigFCUI.java +++ b/altosui/AltosConfigFCUI.java @@ -440,7 +440,7 @@ public class AltosConfigFCUI apogee_lockout_value.setEditable(true); apogee_lockout_value.addItemListener(this); pane.add(apogee_lockout_value, c); - apogee_lockout_value.setToolTipText("Time after boost while apogee detection is locked out"); + apogee_lockout_value.setToolTipText("Time after launch while apogee detection is locked out"); row++; /* Frequency */ diff --git a/doc/config-device.inc b/doc/config-device.inc index 99d5c008..0ca6afff 100644 --- a/doc/config-device.inc +++ b/doc/config-device.inc @@ -23,7 +23,7 @@ ifdef::altusmetrum[] ==== Apogee Lockout - Apogee lockout is the number of seconds after boost + Apogee lockout is the number of seconds after launch where the flight computer will not fire the apogee charge, even if the rocket appears to be at apogee. This is often called 'Mach Delay', as it is @@ -35,6 +35,12 @@ ifdef::altusmetrum[] pressure increase, and so this setting should be left at the default value of zero to disable it. + [WARNING] + Firmware versions older than 1.8.6 have a + bug which resets the time since launch to zero each + time a motor starts burning. Update firmware to get + the correct behavior. + endif::altusmetrum[] ifdef::radio[] diff --git a/doc/pyro-channels.inc b/doc/pyro-channels.inc index 68bbf910..ab5baef0 100644 --- a/doc/pyro-channels.inc +++ b/doc/pyro-channels.inc @@ -42,9 +42,13 @@ launch pad and initialize the system. of less than that value. ==== -Flight Time:: Time since boost was detected. Select a value and choose -whether to activate the pyro channel before or after that amount of -time. +Flight Time:: Time since launch. Select a value and choose whether to +activate the pyro channel before or after that amount of time. + +[WARNING] +Firmware versions older than 1.8.6 have a bug which resets the time +since launch to zero each time a motor starts burning. Update firmware +to get the correct behavior. Ascending:: A deprecated configuration value which was the same as setting Ascent rate > 0. Existing configurations using this will be diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index 7b3cb9fa..c2700d20 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -48,7 +48,8 @@ /* Main flight thread. */ __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_boost_tick; /* time of most recent boost detect */ +__pdata uint16_t ao_launch_tick; /* time of first boost detect */ __pdata uint16_t ao_motor_number; /* number of motors burned so far */ #if HAS_SENSOR_ERRORS @@ -199,7 +200,7 @@ ao_flight(void) ) { ao_flight_state = ao_flight_boost; - ao_boost_tick = ao_sample_tick; + ao_launch_tick = ao_boost_tick = ao_sample_tick; /* start logging data */ ao_log_start(); @@ -269,7 +270,7 @@ ao_flight(void) * number of seconds. */ if (ao_config.apogee_lockout) { - if ((int16_t) (ao_sample_tick - ao_boost_tick) < + if ((int16_t) (ao_sample_tick - ao_launch_tick) < AO_SEC_TO_TICKS(ao_config.apogee_lockout)) break; } diff --git a/src/kernel/ao_flight.h b/src/kernel/ao_flight.h index 6894fe59..005c7e84 100644 --- a/src/kernel/ao_flight.h +++ b/src/kernel/ao_flight.h @@ -40,6 +40,7 @@ enum ao_flight_state { extern __pdata enum ao_flight_state ao_flight_state; extern __pdata uint16_t ao_boost_tick; +extern __pdata uint16_t ao_launch_tick; extern __pdata uint16_t ao_motor_number; #if HAS_IMU || HAS_MMA655X diff --git a/src/kernel/ao_pyro.c b/src/kernel/ao_pyro.c index 3c872354..527112ac 100644 --- a/src/kernel/ao_pyro.c +++ b/src/kernel/ao_pyro.c @@ -160,14 +160,14 @@ ao_pyro_ready(struct ao_pyro *pyro) #endif case ao_pyro_time_less: - if ((int16_t) (ao_time() - ao_boost_tick) <= pyro->time_less) + if ((int16_t) (ao_time() - ao_launch_tick) <= pyro->time_less) continue; - DBG("time %d > %d\n", (int16_t)(ao_time() - ao_boost_tick), pyro->time_less); + DBG("time %d > %d\n", (int16_t)(ao_time() - ao_launch_tick), pyro->time_less); break; case ao_pyro_time_greater: - if ((int16_t) (ao_time() - ao_boost_tick) >= pyro->time_greater) + if ((int16_t) (ao_time() - ao_launch_tick) >= pyro->time_greater) continue; - DBG("time %d < %d\n", (int16_t)(ao_time() - ao_boost_tick), pyro->time_greater); + DBG("time %d < %d\n", (int16_t)(ao_time() - ao_launch_tick), pyro->time_greater); break; case ao_pyro_ascending: -- cgit v1.2.3