diff options
Diffstat (limited to 'nuttx/configs/px4io/src/drv_pwm_servo.c')
| -rw-r--r-- | nuttx/configs/px4io/src/drv_pwm_servo.c | 316 |
1 files changed, 316 insertions, 0 deletions
diff --git a/nuttx/configs/px4io/src/drv_pwm_servo.c b/nuttx/configs/px4io/src/drv_pwm_servo.c new file mode 100644 index 000000000..7f3238da3 --- /dev/null +++ b/nuttx/configs/px4io/src/drv_pwm_servo.c @@ -0,0 +1,316 @@ +/**************************************************************************** + * + * 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 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 <sys/types.h> +#include <stdbool.h> + +#include <debug.h> +#include <errno.h> + +#include <arch/board/board.h> +#include <arch/board/drv_pwm_servo.h> + +#include "stm32_gpio.h" +#include "stm32_tim.h" + +#ifdef CONFIG_PWM_SERVO + +static const struct pwm_servo_config *cfg; + +#define REG(_tmr, _reg) (*(volatile uint32_t *)(cfg->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(cfg->timers[timer].clock_register, 0, cfg->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) = cfg->timers[timer].clock_freq / 1000000; + + /* and update at the desired rate */ + rARR(timer) = 1000000 / cfg->update_rate; + + /* 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_servos_arm(bool armed) +{ + /* iterate timers and arm/disarm appropriately */ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { + if (cfg->timers[i].base != 0) + rCR1(i) = armed ? GTIM_CR1_CEN : 0; + } +} + +static void +pwm_channel_init(unsigned channel) +{ + unsigned timer = cfg->channels[channel].timer_index; + + /* configure the GPIO first */ + stm32_configgpio(cfg->channels[channel].gpio); + + /* configure the channel */ + switch (cfg->channels[channel].timer_channel) { + case 1: + rCCMR1(timer) |= (6 << 4); + rCCR1(timer) = cfg->channels[channel].default_value; + rCCER(timer) |= (1 << 0); + break; + case 2: + rCCMR1(timer) |= (6 << 12); + rCCR2(timer) = cfg->channels[channel].default_value; + rCCER(timer) |= (1 << 4); + break; + case 3: + rCCMR2(timer) |= (6 << 4); + rCCR3(timer) = cfg->channels[channel].default_value; + rCCER(timer) |= (1 << 8); + break; + case 4: + rCCMR2(timer) |= (6 << 12); + rCCR4(timer) = cfg->channels[channel].default_value; + rCCER(timer) |= (1 << 12); + break; + } +} + +static void +pwm_channel_set(unsigned channel, servo_position_t value) +{ + if (channel >= PWM_SERVO_MAX_CHANNELS) { + lldbg("pwm_channel_set: bogus channel %u\n", channel); + return; + } + + unsigned timer = cfg->channels[channel].timer_index; + + /* test timer for validity */ + if ((cfg->timers[timer].base == 0) || + (cfg->channels[channel].gpio == 0)) + return; + + /* configure the channel */ + switch (cfg->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; + } +} + +static servo_position_t +pwm_channel_get(unsigned channel) +{ + if (channel >= PWM_SERVO_MAX_CHANNELS) { + lldbg("pwm_channel_get: bogus channel %u\n", channel); + return 0; + } + + unsigned timer = cfg->channels[channel].timer_index; + servo_position_t value = 0; + + /* test timer for validity */ + if ((cfg->timers[timer].base == 0) || + (cfg->channels[channel].gpio == 0)) + return 0; + + /* configure the channel */ + switch (cfg->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; +} + +static int pwm_servo_write(struct file *filp, const char *buffer, size_t len); +static int pwm_servo_read(struct file *filp, char *buffer, size_t len); +static int pwm_servo_ioctl(struct file *filep, int cmd, unsigned long arg); + +static const struct file_operations pwm_servo_fops = { + .write = pwm_servo_write, + .read = pwm_servo_read, + .ioctl = pwm_servo_ioctl +}; + +static int +pwm_servo_write(struct file *filp, const char *buffer, size_t len) +{ + unsigned channels = len / sizeof(servo_position_t); + servo_position_t *pdata = (servo_position_t *)buffer; + unsigned i; + + if (channels > PWM_SERVO_MAX_CHANNELS) + return -EIO; + + for (i = 0; i < channels; i++) + pwm_channel_set(i, pdata[i]); + + return i * sizeof(servo_position_t); +} + +static int +pwm_servo_read(struct file *filp, char *buffer, size_t len) +{ + unsigned channels = len / sizeof(servo_position_t); + servo_position_t *pdata = (servo_position_t *)buffer; + unsigned i; + + if (channels > PWM_SERVO_MAX_CHANNELS) + return -EIO; + + for (i = 0; i < channels; i++) + pdata[i] = pwm_channel_get(i); + + return i * sizeof(servo_position_t); +} + +static int +pwm_servo_ioctl(struct file *filep, int cmd, unsigned long arg) +{ + /* regular ioctl? */ + switch (cmd) { + case PWM_SERVO_ARM: + pwm_servos_arm(true); + return 0; + + case PWM_SERVO_DISARM: + pwm_servos_arm(false); + return 0; + } + + /* channel set? */ + if ((cmd >= PWM_SERVO_SET(0)) && (cmd < PWM_SERVO_SET(PWM_SERVO_MAX_CHANNELS))) { + /* XXX sanity-check value? */ + pwm_channel_set(cmd - PWM_SERVO_SET(0), (servo_position_t)arg); + return 0; + } + + /* channel get? */ + if ((cmd >= PWM_SERVO_GET(0)) && (cmd < PWM_SERVO_GET(PWM_SERVO_MAX_CHANNELS))) { + /* XXX sanity-check value? */ + *(servo_position_t *)arg = pwm_channel_get(cmd - PWM_SERVO_GET(0)); + return 0; + } + + /* not a recognised value */ + return -ENOTTY; +} + + +int +pwm_servo_init(const struct pwm_servo_config *config) +{ + /* save a pointer to the configuration */ + cfg = config; + + /* do basic timer initialisation first */ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { + if (cfg->timers[i].base != 0) + pwm_timer_init(i); + } + + /* now init channels */ + for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { + if (cfg->channels[i].gpio != 0) + pwm_channel_init(i); + } + + /* register the device */ + return register_driver("/dev/pwm_servo", &pwm_servo_fops, 0666, NULL); +} + +#endif /* CONFIG_PWM_SERVO */
\ No newline at end of file |