From 8eeefcce057012400d94ec855c9103d390b6365b Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Apr 2013 22:46:50 -0700 Subject: Add GPIO driver access to the power supply control/monitoring GPIOs for FMUv2 --- apps/drivers/boards/px4fmuv2/px4fmu_init.c | 19 ++++++++++++------- apps/drivers/boards/px4fmuv2/px4fmu_internal.h | 6 ++++++ apps/drivers/drv_gpio.h | 4 ++++ src/device/px4fmu/fmu.cpp | 15 ++++++++++++--- 4 files changed, 34 insertions(+), 10 deletions(-) diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_init.c b/apps/drivers/boards/px4fmuv2/px4fmu_init.c index c9364c2a4..1d99f15bf 100644 --- a/apps/drivers/boards/px4fmuv2/px4fmu_init.c +++ b/apps/drivers/boards/px4fmuv2/px4fmu_init.c @@ -145,7 +145,17 @@ __EXPORT int matherr(struct exception *e) __EXPORT int nsh_archinitialize(void) { - int result; + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN10); + stm32_configgpio(GPIO_ADC1_IN11); + stm32_configgpio(GPIO_ADC1_IN12); + + /* configure power supply control pins */ + stm32_configgpio(GPIO_VDD_5V_PERIPH_EN); + stm32_configgpio(GPIO_VDD_2V8_SENSORS_EN); + stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC); + stm32_configgpio(GPIO_VDD_5V_PERIPH_OC); /* configure the high-resolution time/callout interface */ hrt_init(); @@ -201,7 +211,7 @@ __EXPORT int nsh_archinitialize(void) /* Get the SPI port for the FRAM */ message("[boot] Initializing SPI port 2\n"); - spi2 = up_spiinitialize(3); + spi2 = up_spiinitialize(2); if (!spi2) { message("[boot] FAILED to initialize SPI port 2\n"); @@ -215,10 +225,5 @@ __EXPORT int nsh_archinitialize(void) //message("[boot] Successfully bound SPI port 2 to the FRAM driver\n"); - /* configure ADC pins */ - stm32_configgpio(GPIO_ADC1_IN10); - stm32_configgpio(GPIO_ADC1_IN11); - stm32_configgpio(GPIO_ADC1_IN12); - return OK; } diff --git a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h index 235b193f3..cc4e9529d 100644 --- a/apps/drivers/boards/px4fmuv2/px4fmu_internal.h +++ b/apps/drivers/boards/px4fmuv2/px4fmu_internal.h @@ -88,6 +88,12 @@ __BEGIN_DECLS #define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) #define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) +/* Power supply control and monitoring GPIOs */ +#define GPIO_VDD_5V_PERIPH_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) +#define GPIO_VDD_2V8_SENSORS_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_VDD_5V_HIPOWER_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN10) +#define GPIO_VDD_5V_PERIPH_OC (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) + /* USB OTG FS * * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h index d21fc5c33..a4c59d218 100644 --- a/apps/drivers/drv_gpio.h +++ b/apps/drivers/drv_gpio.h @@ -80,6 +80,10 @@ # define GPIO_SERVO_5 (1<<4) /**< servo 5 output */ # define GPIO_SERVO_6 (1<<5) /**< servo 6 output */ +# define GPIO_5V_PERIPH_EN (1<<6) /**< PA8 - VDD_5V_PERIPH_EN */ +# define GPIO_5V_HIPOWER_OC (1<<7) /**< PE10 - !VDD_5V_HIPOWER_OC */ +# define GPIO_5V_PERIPH_OC (1<<8) /**< PE15 - !VDD_5V_PERIPH_OC */ + /** * Default GPIO device - other devices may also support this protocol if * they also export GPIO-like things. This is always the GPIOs on the diff --git a/src/device/px4fmu/fmu.cpp b/src/device/px4fmu/fmu.cpp index 7b8ca9bbe..d3865f053 100644 --- a/src/device/px4fmu/fmu.cpp +++ b/src/device/px4fmu/fmu.cpp @@ -175,6 +175,9 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + {0, GPIO_VDD_5V_PERIPH_EN, 0}, + {GPIO_5V_HIPOWER_OC, 0, 0}, + {GPIO_5V_PERIPH_OC, 0, 0}, #endif }; @@ -850,10 +853,16 @@ void PX4FMU::gpio_reset(void) { /* - * Setup default GPIO config - all pins as GPIOs. + * Setup default GPIO config - all pins as GPIOs, input if + * possible otherwise output if possible. */ - for (unsigned i = 0; i < _ngpio; i++) - stm32_configgpio(_gpio_tab[i].input); + for (unsigned i = 0; i < _ngpio; i++) { + if (_gpio_tab[i].input != 0) { + stm32_configgpio(_gpio_tab[i].input); + } else if (_gpio_tab[i].output != 0) { + stm32_configgpio(_gpio_tab[i].output); + } + } #if defined(CONFIG_ARCH_BOARD_PX4FMU) /* if we have a GPIO direction control, set it to zero (input) */ -- cgit v1.2.3