From 366e2b9dc939ee6413c7ce0b31e01e9432558aba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 28 Jan 2020 19:25:55 +0100 Subject: [PATCH] airmind/mindpx-v2: use hw description methods for timer configuration --- boards/airmind/mindpx-v2/src/CMakeLists.txt | 2 +- boards/airmind/mindpx-v2/src/board_config.h | 55 +------ boards/airmind/mindpx-v2/src/init.c | 13 +- boards/airmind/mindpx-v2/src/timer_config.c | 147 ------------------ boards/airmind/mindpx-v2/src/timer_config.cpp | 54 +++++++ 5 files changed, 61 insertions(+), 210 deletions(-) delete mode 100644 boards/airmind/mindpx-v2/src/timer_config.c create mode 100644 boards/airmind/mindpx-v2/src/timer_config.cpp diff --git a/boards/airmind/mindpx-v2/src/CMakeLists.txt b/boards/airmind/mindpx-v2/src/CMakeLists.txt index f0500a3369..ca5759063b 100644 --- a/boards/airmind/mindpx-v2/src/CMakeLists.txt +++ b/boards/airmind/mindpx-v2/src/CMakeLists.txt @@ -36,7 +36,7 @@ add_library(drivers_board init.c led.c spi.c - timer_config.c + timer_config.cpp usb.c ) diff --git a/boards/airmind/mindpx-v2/src/board_config.h b/boards/airmind/mindpx-v2/src/board_config.h index ceaa70840f..a3cea3a654 100644 --- a/boards/airmind/mindpx-v2/src/board_config.h +++ b/boards/airmind/mindpx-v2/src/board_config.h @@ -190,63 +190,10 @@ #define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF9|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN7) /* AUX PWMs - * - * 8 PWM outputs are configured. - * - * Pins: - * - * CH1 : PE9 : TIM1_CH1 - * CH2 : PE11 : TIM1_CH2 - * CH3 : PE13 : TIM1_CH3 - * CH4 : PE14 : TIM1_CH4 - * CH5 : PD12 : TIM4_CH1 - * CH6 : PD13 : TIM4_CH2 - * CH7 : PD14 : TIM4_CH3 - * CH8 : PD15 : TIM4_CH4 */ -#define GPIO_TIM1_CH1OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN9) -#define GPIO_TIM1_CH2OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN11) -#define GPIO_TIM1_CH3OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN13) -#define GPIO_TIM1_CH4OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN14) -#define GPIO_TIM4_CH1OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTD|GPIO_PIN12) -#define GPIO_TIM4_CH2OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTD|GPIO_PIN13) -#define GPIO_TIM4_CH3OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTD|GPIO_PIN14) -#define GPIO_TIM4_CH4OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTD|GPIO_PIN15) #define DIRECT_PWM_OUTPUT_CHANNELS 8 - -#define GPIO_TIM1_CH1IN GPIO_TIM1_CH1IN_2 -#define GPIO_TIM1_CH2IN GPIO_TIM1_CH2IN_2 -#define GPIO_TIM1_CH3IN GPIO_TIM1_CH3IN_2 -#define GPIO_TIM1_CH4IN GPIO_TIM1_CH4IN_2 -#define GPIO_TIM4_CH1IN GPIO_TIM4_CH1IN_2 -#define GPIO_TIM4_CH2IN GPIO_TIM4_CH2IN_2 -#define GPIO_TIM4_CH3IN GPIO_TIM4_CH3IN_2 -#define GPIO_TIM4_CH4IN GPIO_TIM4_CH4IN_2 - #define DIRECT_INPUT_TIMER_CHANNELS 8 - -/* PWMs for main output */ -/* TODO: implement a new driver - /dev/pwm_output0 */ -#define GPIO_TIM2_CH1OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) -#define GPIO_TIM2_CH2OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTB|GPIO_PIN3) -#define GPIO_TIM2_CH3OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN2) -#define GPIO_TIM2_CH4OUT (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN3) -#define GPIO_TIM3_CH1OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTB|GPIO_PIN4) -#define GPIO_TIM3_CH2OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN7) -#define GPIO_TIM3_CH3OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTB|GPIO_PIN0) -#define GPIO_TIM3_CH4OUT (GPIO_ALT|GPIO_AF2|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTB|GPIO_PIN1) - -#define GPIO_TIM2_CH1IN GPIO_TIM2_CH1IN_2 -#define GPIO_TIM2_CH2IN GPIO_TIM2_CH2IN_2 -#define GPIO_TIM2_CH3IN GPIO_TIM2_CH3IN_1 -#define GPIO_TIM2_CH4IN GPIO_TIM2_CH4IN_1 -#define GPIO_TIM3_CH1IN GPIO_TIM3_CH1IN_2 -#define GPIO_TIM3_CH2IN GPIO_TIM3_CH2IN_3 -#define GPIO_TIM3_CH3IN GPIO_TIM3_CH3IN_1 -#define GPIO_TIM3_CH4IN GPIO_TIM3_CH4IN_1 - - /* USB OTG FS * * PA9 OTG_FS_VBUS VBUS sensing @@ -265,7 +212,7 @@ /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 1 */ #define PWMIN_TIMER 4 #define PWMIN_TIMER_CHANNEL 1 -#define GPIO_PWM_IN GPIO_TIM4_CH1IN +#define GPIO_PWM_IN GPIO_TIM1_CH1IN_2 #define RC_SERIAL_PORT "/dev/ttyS0" diff --git a/boards/airmind/mindpx-v2/src/init.c b/boards/airmind/mindpx-v2/src/init.c index 13a3e64b85..f0680ad24e 100644 --- a/boards/airmind/mindpx-v2/src/init.c +++ b/boards/airmind/mindpx-v2/src/init.c @@ -73,6 +73,8 @@ #include #include +#include +#include /**************************************************************************** * Pre-Processor Definitions @@ -108,14 +110,9 @@ __END_DECLS __EXPORT void board_on_reset(int status) { /* configure the GPIO pins to outputs and keep them low */ - stm32_configgpio(GPIO_GPIO0_OUTPUT); - stm32_configgpio(GPIO_GPIO1_OUTPUT); - stm32_configgpio(GPIO_GPIO2_OUTPUT); - stm32_configgpio(GPIO_GPIO3_OUTPUT); - stm32_configgpio(GPIO_GPIO4_OUTPUT); - stm32_configgpio(GPIO_GPIO5_OUTPUT); - stm32_configgpio(GPIO_GPIO6_OUTPUT); - stm32_configgpio(GPIO_GPIO7_OUTPUT); + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(io_timer_channel_get_gpio_output(i)); + } /** * On resets invoked from system (not boot) insure we establish a low diff --git a/boards/airmind/mindpx-v2/src/timer_config.c b/boards/airmind/mindpx-v2/src/timer_config.c deleted file mode 100644 index 7c0b7025ca..0000000000 --- a/boards/airmind/mindpx-v2/src/timer_config.c +++ /dev/null @@ -1,147 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015, 2016 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 mindpx_timer_config.c - * - * Configuration data for the stm32 pwm_servo, input capture and pwm input driver. - * - * Note that these arrays must always be fully-sized. - */ - -#include - -#include -#include -#include - -#include -#include - -#include "board_config.h" - -__EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { - { - .base = STM32_TIM1_BASE, - .clock_register = STM32_RCC_APB2ENR, - .clock_bit = RCC_APB2ENR_TIM1EN, - .clock_freq = STM32_APB2_TIM1_CLKIN, - .vectorno = STM32_IRQ_TIM1CC, - }, - { - .base = STM32_TIM4_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM4EN, - .clock_freq = STM32_APB1_TIM4_CLKIN, - .vectorno = STM32_IRQ_TIM4 - } -}; -__EXPORT const io_timers_channel_mapping_t io_timers_channel_mapping = { - .element = { - { - .first_channel_index = 0, - .channel_count = 4, - }, - { - .first_channel_index = 4, - .channel_count = 4, - } - } -}; - -__EXPORT const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { - { - .gpio_out = GPIO_TIM1_CH1OUT, - .gpio_in = GPIO_TIM1_CH1IN, - .timer_index = 0, - .timer_channel = 1, - .ccr_offset = STM32_GTIM_CCR1_OFFSET, - .masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF - }, - { - .gpio_out = GPIO_TIM1_CH2OUT, - .gpio_in = GPIO_TIM1_CH2IN, - .timer_index = 0, - .timer_channel = 2, - .ccr_offset = STM32_GTIM_CCR2_OFFSET, - .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF - }, - { - .gpio_out = GPIO_TIM1_CH3OUT, - .gpio_in = GPIO_TIM1_CH3IN, - .timer_index = 0, - .timer_channel = 3, - .ccr_offset = STM32_GTIM_CCR3_OFFSET, - .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF - }, - { - .gpio_out = GPIO_TIM1_CH4OUT, - .gpio_in = GPIO_TIM1_CH4IN, - .timer_index = 0, - .timer_channel = 4, - .ccr_offset = STM32_GTIM_CCR4_OFFSET, - .masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF - }, - { - .gpio_out = GPIO_TIM4_CH1OUT, - .gpio_in = GPIO_TIM4_CH1IN, - .timer_index = 1, - .timer_channel = 1, - .ccr_offset = STM32_GTIM_CCR1_OFFSET, - .masks = GTIM_SR_CC1IF | GTIM_SR_CC1OF - }, - { - .gpio_out = GPIO_TIM4_CH2OUT, - .gpio_in = GPIO_TIM4_CH2IN, - .timer_index = 1, - .timer_channel = 2, - .ccr_offset = STM32_GTIM_CCR2_OFFSET, - .masks = GTIM_SR_CC2IF | GTIM_SR_CC2OF - }, - { - .gpio_out = GPIO_TIM4_CH3OUT, - .gpio_in = GPIO_TIM4_CH3IN, - .timer_index = 1, - .timer_channel = 3, - .ccr_offset = STM32_GTIM_CCR3_OFFSET, - .masks = GTIM_SR_CC3IF | GTIM_SR_CC3OF - }, - { - .gpio_out = GPIO_TIM4_CH4OUT, - .gpio_in = GPIO_TIM4_CH4IN, - .timer_index = 1, - .timer_channel = 4, - .ccr_offset = STM32_GTIM_CCR4_OFFSET, - .masks = GTIM_SR_CC4IF | GTIM_SR_CC4OF - } -}; diff --git a/boards/airmind/mindpx-v2/src/timer_config.cpp b/boards/airmind/mindpx-v2/src/timer_config.cpp new file mode 100644 index 0000000000..016bec8a45 --- /dev/null +++ b/boards/airmind/mindpx-v2/src/timer_config.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2015, 2016 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. + * + ****************************************************************************/ + +#include + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1), + initIOTimer(Timer::Timer4), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannelOutputClear(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), + initIOTimerChannelOutputClear(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannelOutputClear(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannelOutputClear(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannelOutputClear(io_timers, {Timer::Timer4, Timer::Channel1}, {GPIO::PortD, GPIO::Pin12}), + initIOTimerChannelOutputClear(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannelOutputClear(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannelOutputClear(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); +