diff --git a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h index a1e35446b8..79d463f89d 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h @@ -64,6 +64,9 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Capture = 3, IOTimerChanMode_OneShot = 4, IOTimerChanMode_Trigger = 5, + IOTimerChanMode_Dshot = 6, + IOTimerChanMode_LED = 7, + IOTimerChanMode_Other = 8, IOTimerChanModeSize } io_timer_channel_mode_t; @@ -122,21 +125,29 @@ __EXPORT extern io_timer_channel_allocation_t allocations[IOTimerChanModeSize]; __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_init_timer(unsigned timer); +__EXPORT int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode); -__EXPORT int io_timer_set_rate(unsigned timer, unsigned rate); +__EXPORT int io_timer_set_pwm_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_allocate_channel(unsigned channel, io_timer_channel_mode_t mode); +__EXPORT int io_timer_unallocate_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); __EXPORT extern void io_timer_trigger(void); + +/** + * Reserve a timer + * @return 0 on success (if not used yet, or already set to the mode) + */ +__EXPORT int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode); + +__EXPORT int io_timer_unallocate_timer(unsigned timer); + /** * Returns the pin configuration for a specific channel, to be used as GPIO output. * 0 is returned if the channel is not valid. @@ -148,5 +159,4 @@ __EXPORT uint32_t io_timer_channel_get_gpio_output(unsigned channel); */ __EXPORT uint32_t io_timer_channel_get_as_pwm_input(unsigned channel); - __END_DECLS diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/input_capture.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/input_capture.c index a1e3490771..69f3a3c0a3 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/input_capture.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/input_capture.c @@ -104,11 +104,11 @@ static void input_capture_chan_handler(void *context, const io_timers_t *timer, { channel_stats[chan_index].last_edge = px4_arch_gpioread(chan->gpio_in); - if ((isrs_rcnt - capture) > channel_stats[chan_index].latnecy) { - channel_stats[chan_index].latnecy = (isrs_rcnt - capture); + if ((isrs_rcnt - capture) > channel_stats[chan_index].latency) { + channel_stats[chan_index].latency = (isrs_rcnt - capture); } - channel_stats[chan_index].chan_in_edges_out++; + channel_stats[chan_index].edges++; channel_stats[chan_index].last_time = isrs_time - (isrs_rcnt - capture); uint32_t overflow = _REG32(timer, KINETIS_FTM_CSC_OFFSET(chan->timer_channel - 1)) & FTM_CSC_CHF; @@ -169,10 +169,6 @@ int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filt } else { - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } - input_capture_bind(channel, callback, context); rv = io_timer_channel_init(channel, IOTimerChanMode_Capture, input_capture_chan_handler, context); diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c index 393920d39d..4a8470fc9e 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/io_timer.c @@ -152,12 +152,14 @@ static int io_timer_handler7(int irq, void *context, void *arg); #define CnSC_PWMIN_INIT 0 // TBD -// NotUsed PWMOut PWMIn Capture OneShot Trigger -io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0 }; +// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot LED Other +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT16_MAX, 0, 0, 0, 0, 0, 0, 0, 0 }; typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ -static io_timer_allocation_t once = 0; +io_timer_channel_allocation_t timer_allocations[MAX_IO_TIMERS] = { }; + +/* Stats and handlers are only useful for Capture */ typedef struct channel_stat_t { uint32_t isr_cout; @@ -269,25 +271,34 @@ 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 io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode) { - int rv = 0; + int ret = -EINVAL; + + if (validate_timer_index(timer) == 0) { + // check if timer is unused or already set to the mode we want + if (timer_allocations[timer] == IOTimerChanMode_NotUsed || timer_allocations[timer] == mode) { + timer_allocations[timer] = mode; + ret = 0; - if (once & 1 << timer) { - rv = -EBUSY; + } else { + ret = -EBUSY; + } } - return rv; + return ret; } -static inline void set_timer_initalized(unsigned timer) +int io_timer_unallocate_timer(unsigned timer) { - once |= 1 << timer; -} + int ret = -EINVAL; -static inline void set_timer_deinitalized(unsigned timer) -{ - once &= ~(1 << timer); + if (validate_timer_index(timer) == 0) { + timer_allocations[timer] = IOTimerChanMode_NotUsed; + ret = 0; + } + + return ret; } static inline int channels_timer(unsigned channel) @@ -324,24 +335,6 @@ static uint32_t get_timer_channels(unsigned timer) return channels_cache[timer]; } -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; @@ -380,7 +373,6 @@ uint32_t io_timer_channel_get_as_pwm_input(unsigned channel) return timer_io_channels[channel].gpio_in; } - int io_timer_get_mode_channels(io_timer_channel_mode_t mode) { if (mode < IOTimerChanModeSize) { @@ -429,21 +421,26 @@ static int reallocate_channel_resources(uint32_t channels, io_timer_channel_mode return before ^ channels; } -static inline int allocate_channel_resource(unsigned channel, io_timer_channel_mode_t mode) +__EXPORT int io_timer_allocate_channel(unsigned channel, io_timer_channel_mode_t mode) { - int rv = io_timer_is_channel_free(channel); + irqstate_t flags = px4_enter_critical_section(); + int existing_mode = io_timer_get_channel_mode(channel); + int ret = -EBUSY; - if (rv == 0) { + if (existing_mode <= IOTimerChanMode_NotUsed || existing_mode == mode) { io_timer_channel_allocation_t bit = 1 << channel; channel_allocations[IOTimerChanMode_NotUsed] &= ~bit; channel_allocations[mode] |= bit; + ret = 0; } - return rv; + px4_leave_critical_section(flags); + + return ret; } -static inline int free_channel_resource(unsigned channel) +int io_timer_unallocate_channel(unsigned channel) { int mode = io_timer_get_channel_mode(channel); @@ -456,24 +453,6 @@ static inline int free_channel_resource(unsigned channel) return mode; } -int io_timer_free_channel(unsigned channel) -{ - if (io_timer_validate_channel_index(channel) != 0) { - return -EINVAL; - } - - int mode = io_timer_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; @@ -482,7 +461,7 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) rv = io_timer_validate_channel_index(channel); if (rv == 0) { - rv = allocate_channel_resource(channel, mode); + rv = io_timer_allocate_channel(channel, mode); } } @@ -568,18 +547,20 @@ void io_timer_trigger(void) px4_leave_critical_section(flags); } -int io_timer_init_timer(unsigned timer) +int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode) { - /* Do this only once per timer */ + if (validate_timer_index(timer) != 0) { + return -EINVAL; + } - int rv = is_timer_uninitalized(timer); + io_timer_channel_mode_t previous_mode = timer_allocations[timer]; + int rv = io_timer_allocate_timer(timer, mode); - if (rv == 0) { + /* Do this only once per timer */ + if (rv == 0 && previous_mode == IOTimerChanMode_NotUsed) { irqstate_t flags = px4_enter_critical_section(); - set_timer_initalized(timer); - /* enable the timer clock before we try to talk to it */ uint32_t regval = _REG(io_timers[timer].clock_register); @@ -650,62 +631,54 @@ int io_timer_init_timer(unsigned timer) } -int io_timer_set_rate(unsigned timer, unsigned rate) +int io_timer_set_pwm_rate(unsigned timer, unsigned rate) { - int rv = EBUSY; - - /* Get the channel bits that belong to the timer */ - - uint32_t channels = get_timer_channels(timer); - - /* Check that all channels are either in PWM or Oneshot */ + /* Change only a timer that is owned by pwm or one shot */ + if (timer_allocations[timer] != IOTimerChanMode_PWMOut && timer_allocations[timer] != IOTimerChanMode_OneShot) { + return -EINVAL; + } - if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] | - channel_allocations[IOTimerChanMode_OneShot] | - channel_allocations[IOTimerChanMode_NotUsed])) == - channels) { + /* Get the channel bits that belong to the timer and are in PWM or OneShot mode */ - /* Change only a timer that is owned by pwm or one shot */ + uint32_t channels = get_timer_channels(timer) & (io_timer_get_mode_channels(IOTimerChanMode_OneShot) | + io_timer_get_mode_channels(IOTimerChanMode_PWMOut)); - /* Request to use OneShot ?*/ + /* Request to use OneShot ?*/ - if (rate == 0) { + if (PWM_RATE_ONESHOT == rate) { - /* Request to use OneShot - * - * We are here because ALL these channels were either PWM or Oneshot - * Now they need to be Oneshot - */ - - /* Did the allocation change */ - if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) { - io_timer_set_oneshot_mode(timer); - } + /* Request to use OneShot + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot); - } else { + /* Did the allocation change */ + if (changed_channels) { + io_timer_set_oneshot_mode(timer); + } - /* Request to use PWM - * - * We are here because ALL these channels were either PWM or Oneshot - * Now they need to be PWM - */ + } else { - if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) { - io_timer_set_PWM_mode(timer); - } + /* Request to use PWM + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut); - timer_set_rate(timer, rate); + if (changed_channels) { + io_timer_set_PWM_mode(timer); } - rv = OK; + timer_set_rate(timer, rate); } - return rv; + return OK; } int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handler_t channel_handler, void *context) { + if (io_timer_validate_channel_index(channel) != 0) { + return -EINVAL; + } + uint32_t gpio = timer_io_channels[channel].gpio_in; uint32_t clearbits = CnSC_RESET; uint32_t setbits = CnSC_CAPTURE_INIT; @@ -733,25 +706,33 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, return -EINVAL; } + irqstate_t flags = px4_enter_critical_section(); // atomic channel allocation and hw config + + int previous_mode = io_timer_get_channel_mode(channel); int rv = allocate_channel(channel, mode); + unsigned timer = channels_timer(channel); - /* Valid channel should now be reserved in new mode */ + if (rv == 0) { + /* Try to reserve & initialize the timer - it will only do it once */ - if (rv >= 0) { + rv = io_timer_init_timer(timer, mode); - /* Blindly try to initialize the timer - it will only do it once */ + if (rv != 0 && previous_mode == IOTimerChanMode_NotUsed) { + /* free the channel if it was not used before */ + io_timer_unallocate_channel(channel); + } + } - io_timer_init_timer(channels_timer(channel)); + /* Valid channel should now be reserved in new mode */ - irqstate_t flags = px4_enter_critical_section(); + if (rv == 0) { /* Set up IO */ if (gpio) { px4_arch_configgpio(gpio); } - - unsigned timer = channels_timer(channel); + /* configure the channel */ /* configure the channel */ @@ -767,9 +748,10 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, channel_handlers[channel].callback = channel_handler; channel_handlers[channel].context = context; - px4_leave_critical_section(flags); } + px4_leave_critical_section(flags); + return rv; } diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c index d44ee5457c..73926711a7 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_servo.c @@ -75,34 +75,42 @@ servo_position_t up_pwm_servo_get(unsigned channel) int up_pwm_servo_init(uint32_t channel_mask) { /* Init channels */ - uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut); + uint32_t current = io_timer_get_mode_channels(IOTimerChanMode_PWMOut) | + io_timer_get_mode_channels(IOTimerChanMode_OneShot); // First free the current set of PWMs for (unsigned channel = 0; current != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { if (current & (1 << channel)) { - io_timer_free_channel(channel); + io_timer_set_enable(false, IOTimerChanMode_PWMOut, 1 << channel); + io_timer_unallocate_channel(channel); current &= ~(1 << channel); } } - // Now allocate the new set + + /* Now allocate the new set */ + + int ret_val = OK; + int channels_init_mask = 0; 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 + ret_val = io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); + channel_mask &= ~(1 << channel); + + if (OK == ret_val) { + channels_init_mask |= 1 << channel; - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; } - - io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); - channel_mask &= ~(1 << channel); } } - return OK; + return ret_val == OK ? channels_init_mask : ret_val; } void up_pwm_servo_deinit(uint32_t channel_mask) @@ -132,7 +140,7 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) } } - return io_timer_set_rate(group, rate); + return io_timer_set_pwm_rate(group, rate); } void up_pwm_update(void) @@ -140,18 +148,12 @@ void up_pwm_update(void) io_timer_trigger(); } -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); - } - - return 0; -} - uint32_t up_pwm_servo_get_rate_group(unsigned group) { - return io_timer_get_group(group); + /* only return the set of channels in the group which we own */ + return (io_timer_get_mode_channels(IOTimerChanMode_PWMOut) | + io_timer_get_mode_channels(IOTimerChanMode_OneShot)) & + io_timer_get_group(group); } void diff --git a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_trigger.c b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_trigger.c index f2f002aa75..5386919d38 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_trigger.c +++ b/platforms/nuttx/src/px4/nxp/kinetis/io_pins/pwm_trigger.c @@ -64,23 +64,31 @@ int up_pwm_trigger_set(unsigned channel, uint16_t value) int up_pwm_trigger_init(uint32_t channel_mask) { /* Init channels */ - for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { - if (channel_mask & (1 << channel)) { + int ret_val = OK; + int channels_init_mask = 0; - // First free any that were not trigger mode before - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } + for (unsigned channel = 0; channel_mask != 0 && channel < MAX_TIMER_IO_CHANNELS; channel++) { + if (channel_mask & (1 << channel)) { - io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL); + ret_val = io_timer_channel_init(channel, IOTimerChanMode_Trigger, NULL, NULL); channel_mask &= ~(1 << channel); + + if (OK == ret_val) { + channels_init_mask |= 1 << channel; + + } else if (ret_val == -EBUSY) { + /* either timer or channel already used - this is not fatal */ + ret_val = 0; + } } } /* Enable the timers */ - up_pwm_trigger_arm(true); + if (ret_val == OK) { + up_pwm_trigger_arm(true); + } - return OK; + return ret_val == OK ? channels_init_mask : ret_val; } void up_pwm_trigger_deinit()