mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-22 01:12:31 +00:00
Refactored pwm servo to use drv_io_timer
This commit is contained in:
David Sidrane
committed by
Lorenz Meier
Lorenz Meier
parent
2949578832
commit
f3c22d3334
@@ -98,6 +98,24 @@
|
||||
#define GTIM_SR_CCIF (GTIM_SR_CC4IF|GTIM_SR_CC3IF|GTIM_SR_CC2IF|GTIM_SR_CC1IF)
|
||||
#define GTIM_SR_CCOF (GTIM_SR_CC4OF|GTIM_SR_CC3OF|GTIM_SR_CC2OF|GTIM_SR_CC1OF)
|
||||
|
||||
#define CCMR_C1_RESET 0x00ff
|
||||
#define CCMR_C1_NUM_BITS 8
|
||||
#define CCER_C1_NUM_BITS 4
|
||||
|
||||
#define CCMR_C1_CAPTURE_INIT (GTIM_CCMR_CCS_CCIN1 << GTIM_CCMR1_CC1S_SHIFT) | \
|
||||
(GTIM_CCMR_ICPSC_NOPSC << GTIM_CCMR1_IC1PSC_SHIFT) | \
|
||||
(GTIM_CCMR_ICF_NOFILT << GTIM_CCMR1_IC1F_SHIFT)
|
||||
|
||||
#define CCMR_C1_PWMOUT_INIT (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE
|
||||
|
||||
#define CCMR_C1_PWMIN_INIT 0 // TBD
|
||||
|
||||
// NotUsed PWMOut PWMIn Capture
|
||||
io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT8_MAX, 0 , 0 , 0 };
|
||||
|
||||
typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */
|
||||
|
||||
static io_timer_allocation_t once = 0;
|
||||
|
||||
typedef struct channel_stat_t {
|
||||
uint32_t isr_cout;
|
||||
@@ -191,3 +209,515 @@ int io_timer_handler3(int irq, void *context)
|
||||
return io_timer_handler(3);
|
||||
|
||||
}
|
||||
|
||||
static inline int validate_timer_index(unsigned timer)
|
||||
{
|
||||
return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL;
|
||||
}
|
||||
|
||||
static inline int is_timer_uninitalized(unsigned timer)
|
||||
{
|
||||
int rv = 0;
|
||||
|
||||
if (once & 1 << timer) {
|
||||
rv = -EBUSY;
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
static inline void set_timer_initalized(unsigned timer)
|
||||
{
|
||||
once |= 1 << timer;
|
||||
}
|
||||
|
||||
static inline void set_timer_deinitalized(unsigned timer)
|
||||
{
|
||||
once &= ~(1 << timer);
|
||||
}
|
||||
|
||||
static inline int channels_timer(unsigned channel)
|
||||
{
|
||||
return timer_io_channels[channel].timer_index;
|
||||
}
|
||||
|
||||
static uint32_t get_timer_channels(unsigned timer)
|
||||
{
|
||||
uint32_t channels = 0;
|
||||
|
||||
if (validate_timer_index(timer) == 0) {
|
||||
const io_timers_t *tmr = &io_timers[timer];
|
||||
/* Gather the channel bit that belong to the timer */
|
||||
|
||||
for (int chan_index = tmr->first_channel_index; chan_index <= tmr->last_channel_index; chan_index++) {
|
||||
channels |= 1 << chan_index;
|
||||
}
|
||||
}
|
||||
|
||||
return channels;
|
||||
}
|
||||
|
||||
static inline int is_channels_timer_uninitalized(unsigned channel)
|
||||
{
|
||||
return is_timer_uninitalized(channels_timer(channel));
|
||||
}
|
||||
|
||||
int io_timer_is_channel_free(unsigned channel)
|
||||
{
|
||||
int rv = io_timer_validate_channel_index(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
if (0 == (channel_allocations[IOTimerChanMode_NotUsed] & (1 << channel))) {
|
||||
rv = -EBUSY;
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int io_timer_validate_channel_index(unsigned channel)
|
||||
{
|
||||
int rv = -EINVAL;
|
||||
|
||||
if (channel < MAX_TIMER_IO_CHANNELS && timer_io_channels[channel].timer_channel != 0) {
|
||||
|
||||
unsigned timer = timer_io_channels[channel].timer_index;
|
||||
|
||||
/* test timer for validity */
|
||||
|
||||
if ((io_timers[timer].base != 0) &&
|
||||
(timer_io_channels[channel].gpio_out != 0) &&
|
||||
(timer_io_channels[channel].gpio_in != 0)) {
|
||||
rv = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int io_timer_get_mode_channels(io_timer_channel_mode_t mode)
|
||||
{
|
||||
if (mode < IOTimerChanModeSize) {
|
||||
return channel_allocations[mode];
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int io_timer_get_channel_mode(unsigned channel)
|
||||
{
|
||||
io_timer_channel_allocation_t bit = 1 << channel;
|
||||
|
||||
for (int mode = IOTimerChanMode_NotUsed; mode < IOTimerChanModeSize; mode++) {
|
||||
if (bit & channel_allocations[mode]) {
|
||||
return mode;
|
||||
}
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode)
|
||||
{
|
||||
int rv = io_timer_is_channel_free(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
io_timer_channel_allocation_t bit = 1 << channel;
|
||||
channel_allocations[IOTimerChanMode_NotUsed] &= ~bit;
|
||||
channel_allocations[mode] |= bit;
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
|
||||
static inline int free_channel_resource(unsigned channel)
|
||||
{
|
||||
int mode = get_channel_mode(channel);
|
||||
|
||||
if (mode > IOTimerChanMode_NotUsed) {
|
||||
io_timer_channel_allocation_t bit = 1 << channel;
|
||||
channel_allocations[mode] &= ~bit;
|
||||
channel_allocations[IOTimerChanMode_NotUsed] |= bit;
|
||||
}
|
||||
|
||||
return mode;
|
||||
}
|
||||
|
||||
int io_timer_free_channel(unsigned channel)
|
||||
{
|
||||
if (io_timer_validate_channel_index(channel) != 0) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
int mode = get_channel_mode(channel);
|
||||
|
||||
if (mode > IOTimerChanMode_NotUsed) {
|
||||
io_timer_set_enable(false, mode, 1 << channel);
|
||||
free_channel_resource(channel);
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
|
||||
{
|
||||
int rv = -EINVAL;
|
||||
|
||||
if (mode != IOTimerChanMode_NotUsed) {
|
||||
rv = io_timer_validate_channel_index(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
rv = allocate_channel_resource(channel, mode);
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
static int 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;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static int io_timer_init_timer(unsigned timer)
|
||||
{
|
||||
/* Do this only once per timer */
|
||||
|
||||
int rv = is_timer_uninitalized(timer);
|
||||
|
||||
if (rv == 0) {
|
||||
|
||||
irqstate_t flags = irqsave();
|
||||
|
||||
set_timer_initalized(timer);
|
||||
|
||||
/* enable the timer clock before we try to talk to it */
|
||||
|
||||
modifyreg32(io_timers[timer].clock_register, 0, io_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;
|
||||
|
||||
if ((io_timers[timer].base == STM32_TIM1_BASE) || (io_timers[timer].base == STM32_TIM8_BASE)) {
|
||||
|
||||
/* master output enable = on */
|
||||
|
||||
rBDTR(timer) = ATIM_BDTR_MOE;
|
||||
}
|
||||
|
||||
/* configure the timer to free-run at 1MHz */
|
||||
|
||||
rPSC(timer) = (io_timers[timer].clock_freq / 1000000) - 1;
|
||||
|
||||
|
||||
/*
|
||||
* Note we do the Standard PWM Out init here
|
||||
* default to updating at 50Hz
|
||||
*/
|
||||
|
||||
timer_set_rate(timer, 50);
|
||||
|
||||
/*
|
||||
* Note that the timer is left disabled with IRQ subs installed
|
||||
* and active but DEIR bits are not set.
|
||||
*/
|
||||
|
||||
irq_attach(io_timers[timer].vectorno, io_timers[timer].handler);
|
||||
|
||||
up_enable_irq(io_timers[timer].vectorno);
|
||||
|
||||
irqrestore(flags);
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
|
||||
int io_timer_set_rate(unsigned timer, unsigned rate)
|
||||
{
|
||||
/* Gather the channel bit that belong to the timer */
|
||||
|
||||
uint32_t channels = get_timer_channels(timer);
|
||||
|
||||
/* Check ownership of PWM out */
|
||||
|
||||
if ((channels & channel_allocations[IOTimerChanMode_PWMOut]) != 0) {
|
||||
|
||||
/* Change only a timer that is owned by pwm */
|
||||
|
||||
timer_set_rate(timer, rate);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
|
||||
channel_handler_t channel_handler, void *context)
|
||||
{
|
||||
|
||||
uint32_t gpio = 0;
|
||||
uint32_t clearbits = CCMR_C1_RESET;
|
||||
uint32_t setbits = CCMR_C1_CAPTURE_INIT;
|
||||
uint32_t ccer_setbits = GTIM_CCER_CC1E;
|
||||
uint32_t dier_setbits = GTIM_DIER_CC1IE;
|
||||
|
||||
/* figure out the GPIO config first */
|
||||
|
||||
switch (mode) {
|
||||
case IOTimerChanMode_PWMOut:
|
||||
ccer_setbits = 0;
|
||||
dier_setbits = 0;
|
||||
setbits = CCMR_C1_PWMOUT_INIT;
|
||||
break;
|
||||
|
||||
case IOTimerChanMode_PWMIn:
|
||||
setbits = CCMR_C1_PWMIN_INIT;
|
||||
gpio = timer_io_channels[channel].gpio_in;
|
||||
break;
|
||||
|
||||
case IOTimerChanMode_Capture:
|
||||
setbits = CCMR_C1_CAPTURE_INIT;
|
||||
gpio = timer_io_channels[channel].gpio_in;
|
||||
break;
|
||||
|
||||
case IOTimerChanMode_NotUsed:
|
||||
setbits = 0;
|
||||
break;
|
||||
|
||||
default:
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
int rv = allocate_channel(channel, mode);
|
||||
|
||||
/* Valid channel should now be reserved in new mode */
|
||||
|
||||
if (rv >= 0) {
|
||||
|
||||
/* Blindly try to initialize the time - it will only do it once */
|
||||
|
||||
io_timer_init_timer(channels_timer(channel));
|
||||
|
||||
irqstate_t flags = irqsave();
|
||||
|
||||
/* Set up IO */
|
||||
if (gpio) {
|
||||
stm32_configgpio(gpio);
|
||||
}
|
||||
|
||||
|
||||
unsigned timer = channels_timer(channel);
|
||||
|
||||
|
||||
/* configure the channel */
|
||||
|
||||
uint32_t shifts = timer_io_channels[channel].timer_channel - 1;
|
||||
|
||||
/* Map shifts timer channel 1-4 to 0-3 */
|
||||
|
||||
uint32_t ccmr_offset = STM32_GTIM_CCMR1_OFFSET + ((shifts >> 1) * sizeof(uint32_t));
|
||||
uint32_t ccr_offset = STM32_GTIM_CCR1_OFFSET + (shifts * sizeof(uint32_t));
|
||||
|
||||
clearbits <<= (shifts & 1) * CCMR_C1_NUM_BITS;
|
||||
setbits <<= (shifts & 1) * CCMR_C1_NUM_BITS;
|
||||
|
||||
uint16_t rvalue = REG(timer, ccmr_offset);
|
||||
rvalue &= ~clearbits;
|
||||
rvalue |= setbits;
|
||||
REG(timer, ccmr_offset) = rvalue;
|
||||
|
||||
/*
|
||||
* The beauty here is that per DocID018909 Rev 8 18.3.5 Input capture mode
|
||||
* As soon as CCxS (in SSMRx becomes different from 00, the channel is configured
|
||||
* in input and the TIMx_CCR1 register becomes read-only.
|
||||
* so the next line does nothing
|
||||
*/
|
||||
|
||||
REG(timer, ccr_offset) = timer_io_channels[channel].default_value;
|
||||
|
||||
/* on PWM Out ccer_setbits is 0 */
|
||||
|
||||
clearbits = (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) << (shifts * CCER_C1_NUM_BITS);
|
||||
setbits = ccer_setbits << (shifts * CCER_C1_NUM_BITS);
|
||||
rvalue = rCCER(timer);
|
||||
rvalue &= ~clearbits;
|
||||
rvalue |= setbits;
|
||||
rCCER(timer) = rvalue;
|
||||
|
||||
channel_handlers[channel].callback = channel_handler;
|
||||
channel_handlers[channel].context = context;
|
||||
rDIER(timer) |= dier_setbits << shifts;
|
||||
irqrestore(flags);
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_channel_allocation_t masks)
|
||||
{
|
||||
|
||||
struct action_cache_t {
|
||||
uint32_t ccer_clearbits;
|
||||
uint32_t ccer_setbits;
|
||||
uint32_t dier_setbits;
|
||||
uint32_t dier_clearbits;
|
||||
uint32_t base;
|
||||
uint32_t gpio[MAX_CHANNELS_PER_TIMER];
|
||||
} action_cache[MAX_IO_TIMERS];
|
||||
memset(action_cache, 0, sizeof(action_cache));
|
||||
|
||||
uint32_t dier_bit = state ? GTIM_DIER_CC1IE : 0;
|
||||
uint32_t ccer_bit = state ? GTIM_CCER_CC1E : 0;
|
||||
|
||||
switch (mode) {
|
||||
case IOTimerChanMode_NotUsed:
|
||||
case IOTimerChanMode_PWMOut:
|
||||
dier_bit = 0;
|
||||
break;
|
||||
|
||||
case IOTimerChanMode_PWMIn:
|
||||
case IOTimerChanMode_Capture:
|
||||
break;
|
||||
|
||||
default:
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* Was the request for all channels in this mode ?*/
|
||||
|
||||
if (masks == IO_TIMER_ALL_MODES_CHANNELS) {
|
||||
|
||||
/* Yes - we provide them */
|
||||
|
||||
masks = channel_allocations[mode];
|
||||
|
||||
} else {
|
||||
|
||||
/* No - caller provided mask */
|
||||
|
||||
/* Only allow the channels in that mode to be affected */
|
||||
|
||||
masks &= channel_allocations[mode];
|
||||
|
||||
}
|
||||
|
||||
/* Pre calculate all the changes */
|
||||
|
||||
for (int chan_index = 0; masks != 0 && chan_index < MAX_TIMER_IO_CHANNELS; chan_index++) {
|
||||
if (masks & (1 << chan_index)) {
|
||||
masks &= ~(1 << chan_index);
|
||||
uint32_t shifts = timer_io_channels[chan_index].timer_channel - 1;
|
||||
uint32_t timer = channels_timer(chan_index);
|
||||
action_cache[timer].base = io_timers[timer].base;
|
||||
action_cache[timer].ccer_clearbits |= GTIM_CCER_CC1E << (shifts * CCER_C1_NUM_BITS);
|
||||
action_cache[timer].ccer_setbits |= ccer_bit << (shifts * CCER_C1_NUM_BITS);
|
||||
action_cache[timer].dier_clearbits |= GTIM_DIER_CC1IE << shifts;
|
||||
action_cache[timer].dier_setbits |= dier_bit << shifts;
|
||||
|
||||
if (state && mode == IOTimerChanMode_PWMOut) {
|
||||
action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
irqstate_t flags = irqsave();
|
||||
|
||||
for (int actions = 0; action_cache[actions].base != 0 && actions < arraySize(action_cache); actions++) {
|
||||
uint32_t rvalue = _REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET);
|
||||
rvalue &= ~action_cache[actions].ccer_clearbits;
|
||||
rvalue |= action_cache[actions].ccer_setbits;
|
||||
_REG32(action_cache[actions].base, STM32_GTIM_CCER_OFFSET) = rvalue;
|
||||
uint32_t after = rvalue & (GTIM_CCER_CC1E | GTIM_CCER_CC2E | GTIM_CCER_CC3E | GTIM_CCER_CC4E);
|
||||
|
||||
rvalue = _REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET);
|
||||
rvalue &= ~action_cache[actions].dier_clearbits;
|
||||
rvalue |= action_cache[actions].dier_setbits;
|
||||
_REG32(action_cache[actions].base, STM32_GTIM_DIER_OFFSET) = rvalue;
|
||||
|
||||
|
||||
/* Any On ?*/
|
||||
|
||||
if (after != 0) {
|
||||
|
||||
/* force an update to preload all registers */
|
||||
rEGR(actions) = GTIM_EGR_UG;
|
||||
|
||||
for (int chan = 0; chan < arraySize(action_cache[actions].gpio); chan++) {
|
||||
if (action_cache[actions].gpio[chan]) {
|
||||
stm32_configgpio(action_cache[actions].gpio[chan]);
|
||||
action_cache[actions].gpio[chan] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* arm requires the timer be enabled */
|
||||
rCR1(actions) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
|
||||
|
||||
} else {
|
||||
|
||||
rCR1(actions) = 0;
|
||||
}
|
||||
|
||||
irqrestore(flags);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int io_timer_set_ccr(unsigned channel, uint16_t value)
|
||||
{
|
||||
int rv = io_timer_validate_channel_index(channel);
|
||||
|
||||
if (rv == 0) {
|
||||
if (get_channel_mode(channel) != IOTimerChanMode_PWMOut) {
|
||||
|
||||
rv = -EIO;
|
||||
|
||||
} else {
|
||||
|
||||
/* configure the channel */
|
||||
|
||||
if (value > 0) {
|
||||
value--;
|
||||
}
|
||||
|
||||
REG(channels_timer(channel), timer_io_channels[channel].ccr_offset) = value;
|
||||
}
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
uint16_t io_channel_get_ccr(unsigned channel)
|
||||
{
|
||||
uint16_t value = 0;
|
||||
|
||||
if (io_timer_validate_channel_index(channel) == 0 &&
|
||||
get_channel_mode(channel) == IOTimerChanMode_PWMOut) {
|
||||
value = REG(channels_timer(channel), timer_io_channels[channel].ccr_offset) + 1;
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
uint32_t io_timer_get_group(unsigned timer)
|
||||
{
|
||||
return get_timer_channels(timer);
|
||||
|
||||
}
|
||||
|
||||
@@ -49,6 +49,16 @@
|
||||
#define MAX_TIMER_IO_CHANNELS 8
|
||||
#define IO_TIMER_ALL_MODES_CHANNELS 0
|
||||
|
||||
typedef enum io_timer_channel_mode_t {
|
||||
IOTimerChanMode_NotUsed = 0,
|
||||
IOTimerChanMode_PWMOut = 1,
|
||||
IOTimerChanMode_PWMIn = 2,
|
||||
IOTimerChanMode_Capture = 3,
|
||||
IOTimerChanModeSize
|
||||
} io_timer_channel_mode_t;
|
||||
|
||||
typedef uint8_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_IO_CHANNELS */
|
||||
|
||||
/* array of timers dedicated to PWM in and out and capture use */
|
||||
typedef struct io_timers_t {
|
||||
uint32_t base;
|
||||
@@ -80,7 +90,25 @@ typedef void (*channel_handler_t)(void *context, const io_timers_t *timer, uint3
|
||||
/* supplied by board-specific code */
|
||||
__EXPORT extern const io_timers_t io_timers[MAX_IO_TIMERS];
|
||||
__EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS];
|
||||
__EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize];
|
||||
__EXPORT int io_timer_handler0(int irq, void *context);
|
||||
__EXPORT int io_timer_handler1(int irq, void *context);
|
||||
__EXPORT int io_timer_handler2(int irq, void *context);
|
||||
__EXPORT int io_timer_handler3(int irq, void *context);
|
||||
|
||||
__EXPORT int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode,
|
||||
channel_handler_t channel_handler, void *context);
|
||||
__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate);
|
||||
__EXPORT int io_timer_set_enable(bool state, io_timer_channel_mode_t mode,
|
||||
io_timer_channel_allocation_t masks);
|
||||
__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate);
|
||||
__EXPORT uint16_t io_channel_get_ccr(unsigned channel);
|
||||
__EXPORT int io_timer_set_ccr(unsigned channel, uint16_t value);
|
||||
__EXPORT uint32_t io_timer_get_group(unsigned timer);
|
||||
__EXPORT int io_timer_validate_channel_index(unsigned channel);
|
||||
__EXPORT int io_timer_is_channel_free(unsigned channel);
|
||||
__EXPORT int io_timer_free_channel(unsigned channel);
|
||||
__EXPORT int io_timer_get_channel_mode(unsigned channel);
|
||||
__EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
@@ -59,227 +59,64 @@
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include "drv_io_timer.h"
|
||||
#include "drv_pwm_servo.h"
|
||||
|
||||
#include <chip.h>
|
||||
#include <up_internal.h>
|
||||
#include <up_arch.h>
|
||||
|
||||
#include <stm32.h>
|
||||
#include <stm32_gpio.h>
|
||||
#include <stm32_tim.h>
|
||||
|
||||
#define REG(_tmr, _reg) (*(volatile uint32_t *)(io_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)
|
||||
#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET)
|
||||
|
||||
static void pwm_timer_init(unsigned timer);
|
||||
static void pwm_timer_set_rate(unsigned timer, unsigned rate);
|
||||
static void pwm_channel_init(unsigned channel);
|
||||
|
||||
static void
|
||||
pwm_timer_init(unsigned timer)
|
||||
int up_pwm_servo_set(unsigned channel, servo_position_t value)
|
||||
{
|
||||
/* enable the timer clock before we try to talk to it */
|
||||
modifyreg32(io_timers[timer].clock_register, 0, io_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;
|
||||
rCCR1(timer) = 0;
|
||||
rCCR2(timer) = 0;
|
||||
rCCR3(timer) = 0;
|
||||
rCCR4(timer) = 0;
|
||||
rCCER(timer) = 0;
|
||||
rDCR(timer) = 0;
|
||||
|
||||
if ((io_timers[timer].base == STM32_TIM1_BASE) || (io_timers[timer].base == STM32_TIM8_BASE)) {
|
||||
/* master output enable = on */
|
||||
rBDTR(timer) = ATIM_BDTR_MOE;
|
||||
}
|
||||
|
||||
/* configure the timer to free-run at 1MHz */
|
||||
rPSC(timer) = (io_timers[timer].clock_freq / 1000000) - 1;
|
||||
|
||||
/* default to updating at 50Hz */
|
||||
pwm_timer_set_rate(timer, 50);
|
||||
|
||||
/* note that the timer is left disabled - arming is performed separately */
|
||||
return io_timer_set_ccr(channel, value);
|
||||
}
|
||||
|
||||
static void
|
||||
pwm_timer_set_rate(unsigned timer, unsigned rate)
|
||||
servo_position_t up_pwm_servo_get(unsigned channel)
|
||||
{
|
||||
/* 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;
|
||||
return io_channel_get_ccr(channel);
|
||||
}
|
||||
|
||||
static void
|
||||
pwm_channel_init(unsigned channel)
|
||||
int up_pwm_servo_init(uint32_t channel_mask)
|
||||
{
|
||||
unsigned timer = timer_io_channels[channel].timer_index;
|
||||
/* Init channels */
|
||||
uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut);
|
||||
|
||||
/* configure the channel */
|
||||
switch (timer_io_channels[channel].timer_channel) {
|
||||
case 1:
|
||||
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE;
|
||||
rCCER(timer) |= GTIM_CCER_CC1E;
|
||||
break;
|
||||
// First free the current set of PWMs
|
||||
|
||||
case 2:
|
||||
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE;
|
||||
rCCER(timer) |= GTIM_CCER_CC2E;
|
||||
break;
|
||||
|
||||
case 3:
|
||||
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE;
|
||||
rCCER(timer) |= GTIM_CCER_CC3E;
|
||||
break;
|
||||
|
||||
case 4:
|
||||
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE;
|
||||
rCCER(timer) |= GTIM_CCER_CC4E;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
up_pwm_servo_set(unsigned channel, servo_position_t value)
|
||||
{
|
||||
if (channel >= MAX_TIMER_IO_CHANNELS) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
unsigned timer = timer_io_channels[channel].timer_index;
|
||||
|
||||
/* test timer for validity */
|
||||
if ((io_timers[timer].base == 0) ||
|
||||
(timer_io_channels[channel].gpio_out == 0)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* configure the channel */
|
||||
if (value > 0) {
|
||||
value--;
|
||||
}
|
||||
|
||||
switch (timer_io_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 >= MAX_TIMER_IO_CHANNELS) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
unsigned timer = timer_io_channels[channel].timer_index;
|
||||
servo_position_t value = 0;
|
||||
|
||||
/* test timer for validity */
|
||||
if ((io_timers[timer].base == 0) ||
|
||||
(timer_io_channels[channel].timer_channel == 0)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* configure the channel */
|
||||
switch (timer_io_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 + 1;
|
||||
}
|
||||
|
||||
int
|
||||
up_pwm_servo_init(uint32_t channel_mask)
|
||||
{
|
||||
/* do basic timer initialisation first */
|
||||
for (unsigned i = 0; i < MAX_IO_TIMERS; i++) {
|
||||
if (io_timers[i].base != 0) {
|
||||
pwm_timer_init(i);
|
||||
for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
|
||||
if (current & (1 << channel)) {
|
||||
io_timer_free_channel(channel);
|
||||
current &= ~(1 << channel);
|
||||
}
|
||||
}
|
||||
|
||||
/* now init channels */
|
||||
for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; i++) {
|
||||
/* don't do init for disabled channels; this leaves the pin configs alone */
|
||||
if (((1 << i) & channel_mask) && (timer_io_channels[i].timer_channel != 0)) {
|
||||
pwm_channel_init(i);
|
||||
// Now allocate the new set
|
||||
|
||||
for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) {
|
||||
if (channel_mask & (1 << channel)) {
|
||||
|
||||
// First free any that were not PWM mode before
|
||||
|
||||
if (-EBUSY == io_timer_is_channel_free(channel)) {
|
||||
io_timer_free_channel(channel);
|
||||
}
|
||||
|
||||
io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL);
|
||||
channel_mask &= ~(1 << channel);
|
||||
}
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
up_pwm_servo_deinit(void)
|
||||
void up_pwm_servo_deinit(void)
|
||||
{
|
||||
/* disable the timers */
|
||||
up_pwm_servo_arm(false);
|
||||
}
|
||||
|
||||
int
|
||||
up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
|
||||
int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
|
||||
{
|
||||
/* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
|
||||
if (rate < 1) {
|
||||
@@ -294,13 +131,12 @@ up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
pwm_timer_set_rate(group, rate);
|
||||
io_timer_set_rate(group, rate);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
up_pwm_servo_set_rate(unsigned rate)
|
||||
int up_pwm_servo_set_rate(unsigned rate)
|
||||
{
|
||||
for (unsigned i = 0; i < MAX_IO_TIMERS; i++) {
|
||||
up_pwm_servo_set_rate_group_update(i, rate);
|
||||
@@ -309,48 +145,13 @@ up_pwm_servo_set_rate(unsigned rate)
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint32_t
|
||||
up_pwm_servo_get_rate_group(unsigned group)
|
||||
uint32_t up_pwm_servo_get_rate_group(unsigned group)
|
||||
{
|
||||
unsigned channels = 0;
|
||||
|
||||
for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; i++) {
|
||||
if ((timer_io_channels[i].gpio_out != 0) && (timer_io_channels[i].timer_index == group)) {
|
||||
channels |= (1 << i);
|
||||
}
|
||||
}
|
||||
|
||||
return channels;
|
||||
return io_timer_get_group(group);
|
||||
}
|
||||
|
||||
void
|
||||
up_pwm_servo_arm(bool armed)
|
||||
{
|
||||
/* iterate timers and arm/disarm appropriately */
|
||||
for (unsigned i = 0; i < MAX_IO_TIMERS; i++) {
|
||||
if (io_timers[i].base != 0) {
|
||||
if (armed) {
|
||||
|
||||
/* force an update to preload all registers */
|
||||
rEGR(i) = GTIM_EGR_UG;
|
||||
|
||||
for (unsigned c = 0; c < MAX_TIMER_IO_CHANNELS; c++) {
|
||||
if (timer_io_channels[c].gpio_out) {
|
||||
stm32_configgpio(timer_io_channels[c].gpio_out);
|
||||
}
|
||||
}
|
||||
|
||||
/* arm requires the timer be enabled */
|
||||
rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
|
||||
|
||||
} else {
|
||||
// XXX This leads to FMU PWM being still active
|
||||
// but uncontrollable. Just disable the timer
|
||||
// and risk a runt.
|
||||
///* on disarm, just stop auto-reload so we don't generate runts */
|
||||
//rCR1(i) &= ~GTIM_CR1_ARPE;
|
||||
rCR1(i) = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
io_timer_set_enable(armed, IOTimerChanMode_PWMOut, IO_TIMER_ALL_MODES_CHANNELS);
|
||||
}
|
||||
|
||||
43
src/drivers/stm32/drv_pwm_servo.h
Normal file
43
src/drivers/stm32/drv_pwm_servo.h
Normal file
@@ -0,0 +1,43 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.h
|
||||
*
|
||||
* stm32-specific PWM output data.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
Reference in New Issue
Block a user