mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-29 20:53:33 +08: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);
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user