/****************************************************************************
*
* 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.
*
****************************************************************************/
/*
* 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 <arch/board/up_pwm_servo.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;
/* and update at the desired rate */
rARR(timer) = 1000000 / pwm_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_timer_set_rate(unsigned timer, unsigned rate)
{
/* configure the timer to update at the desired rate */
rARR(timer) = 1000000 / pwm_update_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 */
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;
}
}
