diff --git a/src/drivers/stm32/drv_io_timer.c b/src/drivers/stm32/drv_io_timer.c index 887024ddde..b9968b274f 100644 --- a/src/drivers/stm32/drv_io_timer.c +++ b/src/drivers/stm32/drv_io_timer.c @@ -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); + +} diff --git a/src/drivers/stm32/drv_io_timer.h b/src/drivers/stm32/drv_io_timer.h index 8876291467..ded546e556 100644 --- a/src/drivers/stm32/drv_io_timer.h +++ b/src/drivers/stm32/drv_io_timer.h @@ -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 diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c index 5dee4e5853..df76267045 100644 --- a/src/drivers/stm32/drv_pwm_servo.c +++ b/src/drivers/stm32/drv_pwm_servo.c @@ -59,227 +59,64 @@ #include #include "drv_io_timer.h" +#include "drv_pwm_servo.h" #include #include #include #include -#include #include -#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); } diff --git a/src/drivers/stm32/drv_pwm_servo.h b/src/drivers/stm32/drv_pwm_servo.h new file mode 100644 index 0000000000..ecd42687f6 --- /dev/null +++ b/src/drivers/stm32/drv_pwm_servo.h @@ -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 +