From da96144e80acd57266188464c0591dd19390eddc Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 8 Jul 2016 12:17:34 -1000 Subject: [PATCH] Added Power button and cleanup sdio --- nuttx-configs/tap-v1/include/board.h | 32 ++--- src/drivers/boards/tap-v1/CMakeLists.txt | 2 + src/drivers/boards/tap-v1/board_config.h | 53 ++++++- src/drivers/boards/tap-v1/tap_init.c | 24 +--- src/drivers/boards/tap-v1/tap_led.c | 14 +- src/drivers/boards/tap-v1/tap_pwr.c | 167 +++++++++++++++++++++++ src/drivers/boards/tap-v1/tap_sdio.c | 122 +++++++++++++++++ src/drivers/boards/tap-v1/tap_spi.c | 2 +- 8 files changed, 359 insertions(+), 57 deletions(-) create mode 100644 src/drivers/boards/tap-v1/tap_pwr.c create mode 100644 src/drivers/boards/tap-v1/tap_sdio.c diff --git a/nuttx-configs/tap-v1/include/board.h b/nuttx-configs/tap-v1/include/board.h index 8df4099124..5c7467f9d0 100644 --- a/nuttx-configs/tap-v1/include/board.h +++ b/nuttx-configs/tap-v1/include/board.h @@ -167,7 +167,7 @@ * PC4 BLUE_LED D4 Blue LED cathode * PC5 RED_LED D5 Red LED cathode */ -#define BOARD_RED 0 +#define BOARD_LED1 0 #define BOARD_LED2 1 #define BOARD_NLEDS 2 @@ -336,13 +336,13 @@ /* * GPIO - * todo:Revisit - cannnot tell from schematic - some could be ADC + * * Port Signal Name CONN - * PA4 POWER JP1-23,24 - * PB4 TEMP_CONT J2-2,11,14,23 MPU6050 TEMP - * PC0 VOLTAGE JP2-13,14 - * PC1 KEY_AD JP1-31,32 - * PC2 SD_SW SD-9 SW + * PA4 POWER JP1-23, - Must be held High to run w/o USB + * PB4 TEMP_CONT J2-2,11,14,23 - Gyro Heater + * PC0 VOLTAGE JP2-13,14 - 1.84 @16.66 1.67 @15.12 Scale 0.1105 + * PC1 KEY_AD JP1-31,32 - Low when Power button is depressed + * PC2 SD_SW SD-9 SW - Card Present * PC3 PCON_RADIO JP1-29,30 * PC13 S2 U8-9 74HCT151 * PC14 S1 U8-10 74HCT151 @@ -416,29 +416,13 @@ extern "C" { * * Description: * All STM32 architectures must provide the following entry point. This entry point - * is called early in the intitialization -- after all memory has been configured + * is called early in the initialization -- after all memory has been configured * and mapped but before any devices have been initialized. * ************************************************************************************/ EXTERN void stm32_boardinitialize(void); -/************************************************************************************ - * Name: stm32_ledinit, stm32_setled, and stm32_setleds - * - * Description: - * If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs. If - * CONFIG_ARCH_LEDS is not defined, then the following interfacesare available to - * control the LEDs from user applications. - * - ************************************************************************************/ - -#ifndef CONFIG_ARCH_LEDS -EXTERN void stm32_ledinit(void); -EXTERN void stm32_setled(int led, bool ledon); -EXTERN void stm32_setleds(uint8_t ledset); -#endif - #undef EXTERN #if defined(__cplusplus) } diff --git a/src/drivers/boards/tap-v1/CMakeLists.txt b/src/drivers/boards/tap-v1/CMakeLists.txt index ca3baa3e31..b44014b0d7 100644 --- a/src/drivers/boards/tap-v1/CMakeLists.txt +++ b/src/drivers/boards/tap-v1/CMakeLists.txt @@ -38,6 +38,8 @@ px4_add_module( SRCS ../common/board_name.c tap_init.c + tap_pwr.c + tap_sdio.c tap_timer_config.c tap_spi.c tap_usb.c diff --git a/src/drivers/boards/tap-v1/board_config.h b/src/drivers/boards/tap-v1/board_config.h index 0111e20114..26395d4908 100644 --- a/src/drivers/boards/tap-v1/board_config.h +++ b/src/drivers/boards/tap-v1/board_config.h @@ -71,7 +71,7 @@ __BEGIN_DECLS #define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN4) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) #define GPIO_BLUE_LED GPIO_LED1 -#define GPIO_RED_LED GPIO_LED1 +#define GPIO_RED_LED GPIO_LED2 /* * SPI * @@ -128,6 +128,8 @@ __BEGIN_DECLS * ADC channels * * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver + * PC0 VOLTAGE JP2-13,14 - 1.84 @16.66 1.67 @15.12 Scale 0.1105 + * */ #define ADC_CHANNELS (1 << 10) /* todo:Revisit - cannnot tell from schematic - some could be ADC */ @@ -223,6 +225,11 @@ __BEGIN_DECLS {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, \ {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, } + +#define MS_PWR_BUTTON_DOWN 750 +#define KEY_AD_GPIO (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTC|GPIO_PIN1) +#define POWER_ON_GPIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) +#define POWER_OFF_GPIO (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN4) /**************************************************************************************************** * Public Types ****************************************************************************************************/ @@ -252,6 +259,16 @@ extern void stm32_spiinitialize(void); extern void stm32_usbinitialize(void); +/************************************************************************************ + * Name: board_sdio_initialize + * + * Description: + * Called to configure SDIO. + * + ************************************************************************************/ + +extern int board_sdio_initialize(void); + /**************************************************************************** * Name: nsh_archinitialize * @@ -271,6 +288,38 @@ extern void stm32_usbinitialize(void); int nsh_archinitialize(void); #endif -#endif /* __ASSEMBLY__ */ +/************************************************************************************ + * Name: board_pwr_init() + * + * Description: + * Called to configure power control for the tap-v1 board. + * + * Input Parameters: + * stage- 0 for boot, 1 for board init + * + ************************************************************************************/ + +void board_pwr_init(int stage); + +/**************************************************************************** + * Name: board_pwr_button_down + * + * Description: + * Called to Read the logical state of the power button + ****************************************************************************/ + +bool board_pwr_button_down(void); + +/**************************************************************************** + * Name: board_pwr + * + * Description: + * Called to turn on or off the TAP + * + ****************************************************************************/ + +void board_pwr(bool on_not_off); + +#endif /* __ASSEMBLY__ */ __END_DECLS diff --git a/src/drivers/boards/tap-v1/tap_init.c b/src/drivers/boards/tap-v1/tap_init.c index 9bcfcdabc2..648e465beb 100644 --- a/src/drivers/boards/tap-v1/tap_init.c +++ b/src/drivers/boards/tap-v1/tap_init.c @@ -54,9 +54,7 @@ #include #include -#include #include -#include #include #include "stm32.h" @@ -128,6 +126,8 @@ __EXPORT void stm32_boardinitialize(void) /* configure always-on ADC pins */ stm32_configgpio(GPIO_ADC1_IN10); + board_pwr_init(0); + /* configure SPI interfaces */ stm32_spiinitialize(); @@ -143,10 +143,6 @@ __EXPORT void stm32_boardinitialize(void) * ****************************************************************************/ -static struct spi_dev_s *spi; - -#include - __EXPORT int nsh_archinitialize(void) { int result; @@ -176,26 +172,18 @@ __EXPORT int nsh_archinitialize(void) (hrt_callout)stm32_serial_dma_poll, NULL); + board_pwr_init(1); + /* initial LED state */ drv_led_start(); led_off(LED_AMBER); led_off(LED_BLUE); - /* Get the SPI port for the microSD slot */ + /* Init the microSD slot */ - spi = up_spiinitialize(CONFIG_NSH_MMCSDSPIPORTNO); - - if (!spi) { - message("[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* Now bind the SPI interface to the MMCSD driver */ - result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi); + result = board_sdio_initialize(); if (result != OK) { - message("[boot] FAILED to bind SPI port 2 to the MMCSD driver\n"); up_ledon(LED_AMBER); return -ENODEV; } diff --git a/src/drivers/boards/tap-v1/tap_led.c b/src/drivers/boards/tap-v1/tap_led.c index d39e8f4a84..b09d04216c 100644 --- a/src/drivers/boards/tap-v1/tap_led.c +++ b/src/drivers/boards/tap-v1/tap_led.c @@ -98,20 +98,10 @@ __EXPORT void led_off(int led) __EXPORT void led_toggle(int led) { if (led == 0) { - if (stm32_gpioread(GPIO_BLUE_LED)) { - stm32_gpiowrite(GPIO_BLUE_LED, false); - - } else { - stm32_gpiowrite(GPIO_BLUE_LED, true); - } + stm32_gpiowrite(GPIO_BLUE_LED, !stm32_gpioread(GPIO_BLUE_LED)); } if (led == 1) { - if (stm32_gpioread(GPIO_RED_LED)) { - stm32_gpiowrite(GPIO_RED_LED, false); - - } else { - stm32_gpiowrite(GPIO_RED_LED, true); - } + stm32_gpiowrite(GPIO_RED_LED, !stm32_gpioread(GPIO_RED_LED)); } } diff --git a/src/drivers/boards/tap-v1/tap_pwr.c b/src/drivers/boards/tap-v1/tap_pwr.c new file mode 100644 index 0000000000..04b232a223 --- /dev/null +++ b/src/drivers/boards/tap-v1/tap_pwr.c @@ -0,0 +1,167 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tap-v1_pwr.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "up_arch.h" +#include "board_config.h" +#include + +extern void led_on(int led); +extern void led_off(int led); + +/************************************************************************************ + * Private Data + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +static int board_button_irq(int irq, FAR void *context) +{ + static struct timespec time_down; + + if (board_pwr_button_down()) { + + led_on(BOARD_LED_RED); + + clock_gettime(CLOCK_REALTIME, &time_down); + + } else { + + led_off(BOARD_LED_RED); + + struct timespec now; + + clock_gettime(CLOCK_REALTIME, &now); + + uint64_t tdown_ms = time_down.tv_sec * 1000 + time_down.tv_nsec / 1000000; + + uint64_t tnow_ms = now.tv_sec * 1000 + now.tv_nsec / 1000000; + + if (tdown_ms != 0 && (tnow_ms - tdown_ms) >= MS_PWR_BUTTON_DOWN) { + + led_on(BOARD_LED_BLUE); + + up_mdelay(750); + stm32_pwr_enablebkp(); + /* XXX wow, this is evil - write a magic number into backup register zero */ + *(uint32_t *)0x40002850 = 0xdeaddead; + up_mdelay(750); + up_systemreset(); + + while (1); + } + } + + return OK; +} + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: board_pwr_init() + * + * Description: + * Called to configure power control for the tap-v1 board. + * + * Input Parameters: + * stage- 0 for boot, 1 for board init + * + ************************************************************************************/ + +void board_pwr_init(int stage) +{ + if (stage == 0) { + stm32_configgpio(POWER_ON_GPIO); + stm32_configgpio(KEY_AD_GPIO); + } + + if (stage == 1) { + stm32_gpiosetevent(KEY_AD_GPIO, true, true, true, board_button_irq); + } +} + +/**************************************************************************** + * Name: board_pwr_button_down + * + * Description: + * Called to Read the logical state of the active low power button. + * + ****************************************************************************/ + +bool board_pwr_button_down(void) +{ + return 0 == stm32_gpioread(KEY_AD_GPIO); +} + +/**************************************************************************** + * Name: board_pwr + * + * Description: + * Called to turn on or off the TAP + * + ****************************************************************************/ + +void board_pwr(bool on_not_off) +{ + if (on_not_off) { + stm32_configgpio(POWER_ON_GPIO); + + } else { + + stm32_configgpio(POWER_OFF_GPIO); + } +} diff --git a/src/drivers/boards/tap-v1/tap_sdio.c b/src/drivers/boards/tap-v1/tap_sdio.c new file mode 100644 index 0000000000..4367134c63 --- /dev/null +++ b/src/drivers/boards/tap-v1/tap_sdio.c @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tap-v1_sdio.c + * + * Board-specific SDIOfunctions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "up_arch.h" +#include "chip.h" +#include "stm32.h" +#include "board_config.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/************************************************************************************ + * Private Data + ************************************************************************************/ +static struct spi_dev_s *spi; + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: board_sdio_initialize + * + * Description: + * Called to configure SDIO. + * + ************************************************************************************/ + +__EXPORT int board_sdio_initialize(void) +{ + /* Get the SPI port for the microSD slot */ + + spi = up_spiinitialize(CONFIG_NSH_MMCSDSPIPORTNO); + + if (!spi) { + message("[boot] FAILED to initialize SPI port %d\n", CONFIG_NSH_MMCSDSPIPORTNO); + return -ENODEV; + } + + /* Now bind the SPI interface to the MMCSD driver */ + int result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi); + + if (result != OK) { + message("[boot] FAILED to bind SPI port 2 to the MMCSD driver\n"); + return -ENODEV; + } + + return OK; +} + diff --git a/src/drivers/boards/tap-v1/tap_spi.c b/src/drivers/boards/tap-v1/tap_spi.c index 87f4df5110..b7a6c2c59e 100644 --- a/src/drivers/boards/tap-v1/tap_spi.c +++ b/src/drivers/boards/tap-v1/tap_spi.c @@ -83,6 +83,6 @@ __EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) { - return stm32_gpioread(GPIO_SPI_SD_SW); + return !stm32_gpioread(GPIO_SPI_SD_SW); }