From ea539031da96df3d3eb9faadd24eb1cc71813e7f Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 1 Nov 2012 23:42:36 -0700 Subject: Cleanup and refactor of the PX4IO firmware and board support. Builds, not tested yet. --- apps/drivers/boards/px4fmu/Makefile | 1 + apps/drivers/boards/px4fmu/px4fmu_init.c | 1 - apps/drivers/boards/px4io/Makefile | 41 ++++++++++ apps/drivers/boards/px4io/px4io_init.c | 100 ++++++++++++++++++++++ apps/drivers/boards/px4io/px4io_internal.h | 100 ++++++++++++++++++++++ apps/drivers/boards/px4io/px4io_pwm_servo.c | 123 ++++++++++++++++++++++++++++ apps/drivers/drv_gpio.h | 32 ++++++++ apps/drivers/drv_led.h | 3 +- apps/drivers/px4io/px4io.cpp | 2 +- apps/drivers/stm32/drv_hrt.c | 24 +++++- apps/drivers/stm32/drv_pwm_servo.h | 7 +- 11 files changed, 425 insertions(+), 9 deletions(-) create mode 100644 apps/drivers/boards/px4io/Makefile create mode 100644 apps/drivers/boards/px4io/px4io_init.c create mode 100644 apps/drivers/boards/px4io/px4io_internal.h create mode 100644 apps/drivers/boards/px4io/px4io_pwm_servo.c (limited to 'apps/drivers') diff --git a/apps/drivers/boards/px4fmu/Makefile b/apps/drivers/boards/px4fmu/Makefile index 393e96e32..6b183d8d2 100644 --- a/apps/drivers/boards/px4fmu/Makefile +++ b/apps/drivers/boards/px4fmu/Makefile @@ -36,5 +36,6 @@ # INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common +LIBNAME = brd_px4fmu include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index 568d861c9..57ffb77d3 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -57,7 +57,6 @@ #include #include #include -#include #include "stm32_internal.h" #include "px4fmu_internal.h" diff --git a/apps/drivers/boards/px4io/Makefile b/apps/drivers/boards/px4io/Makefile new file mode 100644 index 000000000..85806fe6f --- /dev/null +++ b/apps/drivers/boards/px4io/Makefile @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# 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. +# +############################################################################ + +# +# Board-specific startup code for the PX4IO +# + +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common +LIBNAME = brd_px4io + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/boards/px4io/px4io_init.c b/apps/drivers/boards/px4io/px4io_init.c new file mode 100644 index 000000000..dc2e11c30 --- /dev/null +++ b/apps/drivers/boards/px4io/px4io_init.c @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 px4io_init.c + * + * PX4IO-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include + +#include "stm32_internal.h" +#include "px4io_internal.h" +#include "stm32_uart.h" + +#include + +#include +#include +#include + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * 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 + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure the high-resolution time/callout interface */ +#ifdef CONFIG_HRT_TIMER + hrt_init(); +#endif + + /* configure GPIOs */ + stm32_configgpio(GPIO_ACC1_PWR_EN); + stm32_configgpio(GPIO_ACC2_PWR_EN); + stm32_configgpio(GPIO_SERVO_PWR_EN); + stm32_configgpio(GPIO_RELAY1_EN); + stm32_configgpio(GPIO_RELAY2_EN); + stm32_configgpio(GPIO_LED1); + stm32_configgpio(GPIO_LED2); + stm32_configgpio(GPIO_LED3); + stm32_configgpio(GPIO_ACC_OC_DETECT); + stm32_configgpio(GPIO_SERVO_OC_DETECT); + stm32_configgpio(GPIO_BTN_SAFETY); +} diff --git a/apps/drivers/boards/px4io/px4io_internal.h b/apps/drivers/boards/px4io/px4io_internal.h new file mode 100644 index 000000000..a0342ac8a --- /dev/null +++ b/apps/drivers/boards/px4io/px4io_internal.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 PX4IO hardware definitions. + */ + +#pragma once + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include +#include + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* PX4IO GPIOs **********************************************************************/ +/* LEDs */ + +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) + +/* R/C in/out channels **************************************************************/ + +/* XXX just GPIOs for now - eventually timer pins */ + +#define GPIO_CH1_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN0) +#define GPIO_CH2_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN1) +#define GPIO_CH3_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN8) +#define GPIO_CH4_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN9) +#define GPIO_CH5_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN6) +#define GPIO_CH6_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN7) +#define GPIO_CH7_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN0) +#define GPIO_CH8_IN (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN1) + +#define GPIO_CH1_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_CH2_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_CH3_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) +#define GPIO_CH4_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) +#define GPIO_CH5_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6) +#define GPIO_CH6_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) +#define GPIO_CH7_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_CH8_OUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) + +/* Safety switch button *************************************************************/ + +#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5) + +/* Power switch controls ************************************************************/ + +#define GPIO_ACC1_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +#define GPIO_ACC2_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN14) +#define GPIO_SERVO_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15) + +#define GPIO_ACC_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN12) +#define GPIO_SERVO_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13) + +#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13) +#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) diff --git a/apps/drivers/boards/px4io/px4io_pwm_servo.c b/apps/drivers/boards/px4io/px4io_pwm_servo.c new file mode 100644 index 000000000..a2f73c429 --- /dev/null +++ b/apps/drivers/boards/px4io/px4io_pwm_servo.c @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include + +#include +#include + +#include +#include +#include + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + }, + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM2_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 2, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH4OUT, + .timer_index = 2, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH1OUT, + .timer_index = 1, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH4OUT, + .timer_index = 1, + .timer_channel = 4, + .default_value = 1000, + } +}; diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h index b8d1fa0f0..92d184326 100644 --- a/apps/drivers/drv_gpio.h +++ b/apps/drivers/drv_gpio.h @@ -67,6 +67,38 @@ #endif +#ifdef CONFIG_ARCH_BOARD_PX4IO +/* + * PX4IO GPIO numbers. + * + * XXX note that these are here for reference/future use; currently + * there is no good way to wire these up without a common STM32 GPIO + * driver, which isn't implemented yet. + */ +/* outputs */ +# define GPIO_ACC1_POWER (1<<0) /**< accessory power 1 */ +# define GPIO_ACC2_POWER (1<<1) /**< accessory power 2 */ +# define GPIO_SERVO_POWER (1<<2) /**< servo power */ +# define GPIO_RELAY1 (1<<3) /**< relay 1 */ +# define GPIO_RELAY2 (1<<4) /**< relay 2 */ +# define GPIO_LED_BLUE (1<<5) /**< blue LED */ +# define GPIO_LED_AMBER (1<<6) /**< amber/red LED */ +# define GPIO_LED_SAFETY (1<<7) /**< safety LED */ + +/* inputs */ +# define GPIO_ACC_OVERCURRENT (1<<8) /**< accessory 1/2 overcurrent detect */ +# define GPIO_SERVO_OVERCURRENT (1<<9) /**< servo overcurrent detect */ +# define GPIO_SAFETY_BUTTON (1<<10) /**< safety button pressed */ + +/** + * Default GPIO device - other devices may also support this protocol if + * they also export GPIO-like things. This is always the GPIOs on the + * main board. + */ +# define GPIO_DEVICE_PATH "/dev/px4io" + +#endif + #ifndef GPIO_DEVICE_PATH # error No GPIO support for this board. #endif diff --git a/apps/drivers/drv_led.h b/apps/drivers/drv_led.h index bf21787f2..7eb9e4b7c 100644 --- a/apps/drivers/drv_led.h +++ b/apps/drivers/drv_led.h @@ -50,6 +50,7 @@ #define LED_AMBER 0 #define LED_RED 0 /* some boards have red rather than amber */ #define LED_BLUE 1 +#define LED_SAFETY 2 #define LED_ON _IOC(_LED_BASE, 0) #define LED_OFF _IOC(_LED_BASE, 1) @@ -59,6 +60,6 @@ __BEGIN_DECLS /* * Initialise the LED driver. */ -__EXPORT extern void drv_led_start(); +__EXPORT extern void drv_led_start(void); __END_DECLS diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 995c9393f..51ad93423 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -73,7 +73,7 @@ #include #include -#include "px4io/protocol.h" +#include #include "uploader.h" diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index 1ac90b16d..0474960d0 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -276,6 +276,17 @@ static void hrt_call_invoke(void); * Specific registers and bits used by PPM sub-functions */ #ifdef CONFIG_HRT_PPM +/* + * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. + */ +# ifndef GTIM_CCER_CC1NP +# define GTIM_CCER_CC1NP 0 +# define GTIM_CCER_CC2NP 0 +# define GTIM_CCER_CC3NP 0 +# define GTIM_CCER_CC4NP 0 +# define PPM_EDGE_FLIP +# endif + # if HRT_PPM_CHANNEL == 1 # define rCCR_PPM rCCR1 /* capture register for PPM */ # define DIER_PPM GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */ @@ -284,6 +295,7 @@ static void hrt_call_invoke(void); # define CCMR1_PPM 1 /* not on TI1/TI2 */ # define CCMR2_PPM 0 /* on TI3, not on TI4 */ # define CCER_PPM (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC1P # elif HRT_PPM_CHANNEL == 2 # define rCCR_PPM rCCR2 /* capture register for PPM */ # define DIER_PPM GTIM_DIER_CC2IE /* capture interrupt (non-DMA mode) */ @@ -292,6 +304,7 @@ static void hrt_call_invoke(void); # define CCMR1_PPM 2 /* not on TI1/TI2 */ # define CCMR2_PPM 0 /* on TI3, not on TI4 */ # define CCER_PPM (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC2P # elif HRT_PPM_CHANNEL == 3 # define rCCR_PPM rCCR3 /* capture register for PPM */ # define DIER_PPM GTIM_DIER_CC3IE /* capture interrupt (non-DMA mode) */ @@ -300,6 +313,7 @@ static void hrt_call_invoke(void); # define CCMR1_PPM 0 /* not on TI1/TI2 */ # define CCMR2_PPM 1 /* on TI3, not on TI4 */ # define CCER_PPM (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC3P # elif HRT_PPM_CHANNEL == 4 # define rCCR_PPM rCCR4 /* capture register for PPM */ # define DIER_PPM GTIM_DIER_CC4IE /* capture interrupt (non-DMA mode) */ @@ -308,6 +322,7 @@ static void hrt_call_invoke(void); # define CCMR1_PPM 0 /* not on TI1/TI2 */ # define CCMR2_PPM 2 /* on TI3, not on TI4 */ # define CCER_PPM (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC4P # else # error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set # endif @@ -532,9 +547,14 @@ hrt_tim_isr(int irq, void *context) #ifdef CONFIG_HRT_PPM /* was this a PPM edge? */ - if (status & (SR_INT_PPM | SR_OVF_PPM)) - hrt_ppm_decode(status); + if (status & (SR_INT_PPM | SR_OVF_PPM)) { + /* if required, flip edge sensitivity */ +# ifdef PPM_EDGE_FLIP + rCCER ^= CCER_PPM_FLIP; +# endif + hrt_ppm_decode(status); + } #endif /* was this a timer tick? */ diff --git a/apps/drivers/stm32/drv_pwm_servo.h b/apps/drivers/stm32/drv_pwm_servo.h index 667283424..5dd4cf70c 100644 --- a/apps/drivers/stm32/drv_pwm_servo.h +++ b/apps/drivers/stm32/drv_pwm_servo.h @@ -42,7 +42,7 @@ #include /* configuration limits */ -#define PWM_SERVO_MAX_TIMERS 2 +#define PWM_SERVO_MAX_TIMERS 4 #define PWM_SERVO_MAX_CHANNELS 8 /* array of timers dedicated to PWM servo use */ @@ -53,9 +53,6 @@ struct pwm_servo_timer { uint32_t clock_freq; }; -/* supplied by board-specific code */ -__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS]; - /* array of channels in logical order */ struct pwm_servo_channel { uint32_t gpio; @@ -64,4 +61,6 @@ struct pwm_servo_channel { servo_position_t default_value; }; +/* supplied by board-specific code */ +__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS]; __EXPORT extern const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS]; -- cgit v1.2.3