diff options
Diffstat (limited to 'apps/drivers')
-rw-r--r-- | apps/drivers/boards/px4fmu/px4fmu_init.c | 3 | ||||
-rw-r--r-- | apps/drivers/drv_pwm_output.h | 66 | ||||
-rw-r--r-- | apps/drivers/stm32/Makefile | 42 | ||||
-rw-r--r-- | apps/drivers/stm32/drv_hrt.c (renamed from apps/drivers/boards/px4fmu/drv_hrt.c) | 0 | ||||
-rw-r--r-- | apps/drivers/stm32/up_pwm_servo.c | 348 |
5 files changed, 455 insertions, 4 deletions
diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index c35290589..bb30087e0 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -63,7 +63,6 @@ #include "px4fmu_internal.h" #include "stm32_uart.h" -#include <arch/board/up_cpuload.h> #include <arch/board/up_adc.h> #include <arch/board/board.h> #include <arch/board/drv_led.h> @@ -71,6 +70,8 @@ #include <drivers/drv_hrt.h> +#include <systemlib/cpuload.h> + /**************************************************************************** * Pre-Processor Definitions ****************************************************************************/ diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 340175a4b..fc458e9b0 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -41,14 +41,15 @@ * channel. */ -#ifndef _DRV_PWM_OUTPUT_H -#define _DRV_PWM_OUTPUT_H +#pragma once #include <stdint.h> #include <sys/ioctl.h> #include "drv_orb_dev.h" +__BEGIN_DECLS + /** * Path for the default PWM output device. * @@ -109,4 +110,63 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo) -#endif /* _DRV_PWM_OUTPUT_H */ +/* + * Low-level PWM output interface. + * + * This is the low-level API to the platform-specific PWM driver. + */ + +/** + * Intialise the PWM servo outputs using the specified configuration. + * + * @param channel_mask Bitmask of channels (LSB = channel 0) to enable. + * This allows some of the channels to remain configured + * as GPIOs or as another function. + * @return OK on success. + */ +__EXPORT extern int up_pwm_servo_init(uint32_t channel_mask); + +/** + * De-initialise the PWM servo outputs. + */ +__EXPORT extern void up_pwm_servo_deinit(void); + +/** + * Arm or disarm servo outputs. + * + * When disarmed, servos output no pulse. + * + * @bug This function should, but does not, guarantee that any pulse + * currently in progress is cleanly completed. + * + * @param armed If true, outputs are armed; if false they + * are disarmed. + */ +__EXPORT extern void up_pwm_servo_arm(bool armed); + +/** + * Set the servo update rate + * + * @param rate The update rate in Hz to set. + * @return OK on success, -ERANGE if an unsupported update rate is set. + */ +__EXPORT extern int up_pwm_servo_set_rate(unsigned rate); + +/** + * Set the current output value for a channel. + * + * @param channel The channel to set. + * @param value The output pulse width in microseconds. + */ +__EXPORT extern int up_pwm_servo_set(unsigned channel, servo_position_t value); + +/** + * Get the current output value for a channel. + * + * @param channel The channel to read. + * @return The output pulse width in microseconds, or zero if + * outputs are not armed or not configured. + */ +__EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel); + +__END_DECLS diff --git a/apps/drivers/stm32/Makefile b/apps/drivers/stm32/Makefile new file mode 100644 index 000000000..4ad57f413 --- /dev/null +++ b/apps/drivers/stm32/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# STM32 driver support code +# +# Modules in this directory are compiled for all STM32 targets. +# + +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/boards/px4fmu/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index 11b98fd1b..11b98fd1b 100644 --- a/apps/drivers/boards/px4fmu/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c diff --git a/apps/drivers/stm32/up_pwm_servo.c b/apps/drivers/stm32/up_pwm_servo.c new file mode 100644 index 000000000..708aa8884 --- /dev/null +++ b/apps/drivers/stm32/up_pwm_servo.c @@ -0,0 +1,348 @@ +/**************************************************************************** + * + * 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 drv_pwm_servo.c + * + * Servo driver supporting PWM servos connected to STM32 timer blocks. + * + * Works with any of the 'generic' or 'advanced' STM32 timers that + * have output pins, does not require an interrupt. + */ + +#include <nuttx/config.h> +#include <nuttx/arch.h> +#include <nuttx/irq.h> + +#include <sys/types.h> +#include <stdbool.h> + +#include <assert.h> +#include <debug.h> +#include <time.h> +#include <queue.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> + +#include <arch/board/board.h> +#include <drivers/drv_pwm_output.h> + +#include "chip.h" +#include "up_internal.h" +#include "up_arch.h" + +#include "stm32_internal.h" +#include "stm32_gpio.h" +#include "stm32_tim.h" + +/* configuration limits */ +#define PWM_SERVO_MAX_TIMERS 2 +#define PWM_SERVO_MAX_CHANNELS 8 + +/* default rate (in Hz) of PWM updates */ +static uint32_t pwm_update_rate = 50; + +/* + * Servo configuration for all of the pins that can be used as + * PWM outputs on FMU. + */ + +/* array of timers dedicated to PWM servo use */ +static const struct pwm_servo_timer { + uint32_t base; + uint32_t clock_register; + uint32_t clock_bit; + uint32_t clock_freq; +} pwm_timers[] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + } +}; + +/* array of channels in logical order */ +static const struct pwm_servo_channel { + uint32_t gpio; + uint8_t timer_index; + uint8_t timer_channel; + servo_position_t default_value; +} pwm_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_TIM2_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1000, + } +}; + + +#define REG(_tmr, _reg) (*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg)) + +#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) +#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET) +#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET) +#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET) +#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET) +#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET) +#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET) +#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET) +#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET) +#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET) +#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET) +#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET) +#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET) +#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET) +#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) +#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) +#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) + +static void +pwm_timer_init(unsigned timer) +{ + /* enable the timer clock before we try to talk to it */ + modifyreg32(pwm_timers[timer].clock_register, 0, pwm_timers[timer].clock_bit); + + /* disable and configure the timer */ + rCR1(timer) = 0; + rCR2(timer) = 0; + rSMCR(timer) = 0; + rDIER(timer) = 0; + rCCER(timer) = 0; + rCCMR1(timer) = 0; + rCCMR2(timer) = 0; + rCCER(timer) = 0; + rDCR(timer) = 0; + + /* configure the timer to free-run at 1MHz */ + rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1; + + /* and update at the desired rate */ + rARR(timer) = (1000000 / pwm_update_rate) - 1; + + /* generate an update event; reloads the counter and all registers */ + rEGR(timer) = GTIM_EGR_UG; + + /* note that the timer is left disabled - arming is performed separately */ +} + +static void +pwm_timer_set_rate(unsigned timer, unsigned rate) +{ + /* configure the timer to update at the desired rate */ + rARR(timer) = 1000000 / rate; + + /* generate an update event; reloads the counter and all registers */ + rEGR(timer) = GTIM_EGR_UG; +} + +static void +pwm_channel_init(unsigned channel) +{ + unsigned timer = pwm_channels[channel].timer_index; + + /* configure the GPIO first */ + stm32_configgpio(pwm_channels[channel].gpio); + + /* configure the channel */ + switch (pwm_channels[channel].timer_channel) { + case 1: + rCCMR1(timer) |= (6 << 4); + rCCR1(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 0); + break; + case 2: + rCCMR1(timer) |= (6 << 12); + rCCR2(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 4); + break; + case 3: + rCCMR2(timer) |= (6 << 4); + rCCR3(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 8); + break; + case 4: + rCCMR2(timer) |= (6 << 12); + rCCR4(timer) = pwm_channels[channel].default_value; + rCCER(timer) |= (1 << 12); + break; + } +} + +int +up_pwm_servo_set(unsigned channel, servo_position_t value) +{ + if (channel >= PWM_SERVO_MAX_CHANNELS) { + lldbg("pwm_channel_set: bogus channel %u\n", channel); + return -1; + } + + unsigned timer = pwm_channels[channel].timer_index; + + /* test timer for validity */ + if ((pwm_timers[timer].base == 0) || + (pwm_channels[channel].gpio == 0)) + return -1; + + /* configure the channel */ + if (value > 0) + value--; + switch (pwm_channels[channel].timer_channel) { + case 1: + rCCR1(timer) = value; + break; + case 2: + rCCR2(timer) = value; + break; + case 3: + rCCR3(timer) = value; + break; + case 4: + rCCR4(timer) = value; + break; + default: + return -1; + } + return 0; +} + +servo_position_t +up_pwm_servo_get(unsigned channel) +{ + if (channel >= PWM_SERVO_MAX_CHANNELS) { + lldbg("pwm_channel_get: bogus channel %u\n", channel); + return 0; + } + + unsigned timer = pwm_channels[channel].timer_index; + servo_position_t value = 0; + + /* test timer for validity */ + if ((pwm_timers[timer].base == 0) || + (pwm_channels[channel].gpio == 0)) + return 0; + + /* configure the channel */ + switch (pwm_channels[channel].timer_channel) { + case 1: + value = rCCR1(timer); + break; + case 2: + value = rCCR2(timer); + break; + case 3: + value = rCCR3(timer); + break; + case 4: + value = rCCR4(timer); + break; + } + return value; +} + +int +up_pwm_servo_init(uint32_t channel_mask) +{ + /* do basic timer initialisation first */ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { + if (pwm_timers[i].base != 0) + pwm_timer_init(i); + } + + /* now init channels */ + for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { + /* don't do init for disabled channels; this leaves the pin configs alone */ + if (((1<<i) & channel_mask) && (pwm_channels[i].gpio != 0)) + pwm_channel_init(i); + } + return OK; +} + +void +up_pwm_servo_deinit(void) +{ + /* disable the timers */ + up_pwm_servo_arm(false); +} + +int +up_pwm_servo_set_rate(unsigned rate) +{ + if ((rate < 50) || (rate > 400)) + return -ERANGE; + + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { + if (pwm_timers[i].base != 0) + pwm_timer_set_rate(i, rate); + } + return OK; +} + +void +up_pwm_servo_arm(bool armed) +{ + /* + * XXX this is inelgant and in particular will either jam outputs at whatever level + * they happen to be at at the time the timers stop or generate runts. + * The right thing is almost certainly to kill auto-reload on the timers so that + * they just stop at the end of their count for disable, and to reset/restart them + * for enable. + */ + + /* iterate timers and arm/disarm appropriately */ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { + if (pwm_timers[i].base != 0) + rCR1(i) = armed ? GTIM_CR1_CEN : 0; + } +} |