diff --git a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h index 8d55042f75..df878380ca 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h @@ -60,6 +60,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; @@ -123,27 +126,32 @@ __EXPORT extern const timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNEL __EXPORT extern const io_timers_t led_pwm_timers[MAX_LED_TIMERS]; __EXPORT extern const timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS]; -__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. diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/input_capture.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/input_capture.c index a5b0e3ce4c..6fb285c583 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/input_capture.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/input_capture.c @@ -87,11 +87,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 = 0;//_REG32(timer, KINETIS_FTM_CSC_OFFSET(chan->timer_channel - 1)) & FTM_CSC_CHF; @@ -132,7 +132,6 @@ int up_input_capture_set(unsigned channel, input_capture_edge edge, capture_filt int rv = io_timer_validate_channel_index(channel); if (rv == 0) { - if (edge == Disabled) { io_timer_set_enable(false, IOTimerChanMode_Capture, 1 << channel); @@ -140,10 +139,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/imxrt/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c index e7de2d859f..bec852a8af 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c @@ -149,12 +149,14 @@ static int io_timer_handler7(int irq, void *context, void *arg); #define rFCTRL20(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FCTRL20_OFFSET) /* Fault Control 2 Register */ -// 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; @@ -213,22 +215,40 @@ int io_timer_handler7(int irq, void *context, void *arg) return io_timer_handler(7); } -static inline int is_timer_uninitalized(unsigned timer) +static inline int validate_timer_index(unsigned timer) { - int rv = 0; + return (timer < MAX_IO_TIMERS && io_timers[timer].base != 0) ? 0 : -EINVAL; +} - if (once & 1 << timer) { - rv = -EBUSY; +int io_timer_allocate_timer(unsigned timer, io_timer_channel_mode_t mode) +{ + 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; + + } 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; + if (validate_timer_index(timer) == 0) { + timer_allocations[timer] = IOTimerChanMode_NotUsed; + ret = 0; + } + + return ret; +} static inline int channels_timer(unsigned channel) { @@ -240,19 +260,6 @@ static uint32_t get_channel_mask(unsigned channel) return io_timer_validate_channel_index(channel) == 0 ? 1 << channel : 0; } -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; @@ -337,21 +344,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); @@ -364,24 +376,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; @@ -390,7 +384,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); } } @@ -479,18 +473,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) { + if (validate_timer_index(timer) != 0) { + return -EINVAL; + } + + io_timer_channel_mode_t previous_mode = timer_allocations[timer]; + int rv = io_timer_allocate_timer(timer, mode); + /* Do this only once per timer */ - - int rv = is_timer_uninitalized(timer); - - if (rv == 0) { + 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 */ switch (io_timers[timer].base) { @@ -545,62 +541,54 @@ int io_timer_init_timer(unsigned timer) } -int io_timer_set_rate(unsigned channel, unsigned rate) +int io_timer_set_pwm_rate(unsigned timer, unsigned rate) { - int rv = EBUSY; - - /* Get the channel bits that belong to the channel */ - - uint32_t channels = get_channel_mask(channel); - - /* Check that all channels are either in PWM or Oneshot */ - - if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] | - channel_allocations[IOTimerChanMode_OneShot] | - channel_allocations[IOTimerChanMode_NotUsed])) == - channels) { - - /* Change only a timer that is owned by pwm or one shot */ - - /* Request to use OneShot ?*/ - - if (rate == 0) { - - /* 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(channel); - } - - } else { - - /* Request to use PWM - * - * We are here because ALL these channels were either PWM or Oneshot - * Now they need to be PWM - */ - - if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) { - io_timer_set_PWM_mode(channel); - } - - timer_set_rate(channel, rate); - } - - rv = OK; + /* 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; } - return rv; + /* Get the channel bits that belong to the timer and are in PWM or OneShot mode */ + + uint32_t channels = get_channel_mask(timer) & (io_timer_get_mode_channels(IOTimerChanMode_OneShot) | + io_timer_get_mode_channels(IOTimerChanMode_PWMOut)); + + /* Request to use OneShot ?*/ + + if (PWM_RATE_ONESHOT == rate) { + + /* Request to use OneShot + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot); + + /* Did the allocation change */ + if (changed_channels) { + io_timer_set_oneshot_mode(timer); + } + + } else { + + /* Request to use PWM + */ + int changed_channels = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut); + + if (changed_channels) { + io_timer_set_PWM_mode(timer); + } + + timer_set_rate(timer, rate); + } + + 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 = 0; /* figure out the GPIO config first */ @@ -625,19 +613,26 @@ 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); + + if (rv == 0) { + /* Try to reserve & initialize the timer - it will only do it once */ + + rv = io_timer_init_timer(timer, mode); + + if (rv != 0 && previous_mode == IOTimerChanMode_NotUsed) { + /* free the channel if it was not used before */ + io_timer_unallocate_channel(channel); + } + } /* Valid channel should now be reserved in new mode */ - if (rv >= 0) { - - unsigned timer = channels_timer(channel); - - /* Blindly try to initialize the timer - it will only do it once */ - - io_timer_init_timer(timer); - - irqstate_t flags = px4_enter_critical_section(); + if (rv == 0) { /* Set up IO */ if (gpio) { @@ -650,9 +645,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/imxrt/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c index bfe33ee8a7..541e94c845 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_servo.c @@ -74,34 +74,44 @@ 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 - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(channel); - } - - io_timer_channel_init(channel, IOTimerChanMode_PWMOut, NULL, NULL); + ret_val = io_timer_channel_init(channel, IOTimerChanMode_PWMOut, 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; + } } } - return OK; + return ret_val == OK ? channels_init_mask : ret_val; } void up_pwm_servo_deinit(uint32_t channel_mask) @@ -131,7 +141,7 @@ int up_pwm_servo_set_rate_group_update(unsigned channel, unsigned rate) } } - return io_timer_set_rate(channel, rate); + return io_timer_set_pwm_rate(channel, rate); } void up_pwm_update(void) @@ -139,18 +149,12 @@ void up_pwm_update(void) io_timer_trigger(); } -int up_pwm_servo_set_rate(unsigned rate) -{ - for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; 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/imxrt/io_pins/pwm_trigger.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_trigger.c index 7e9869d0d3..4f2d6d9750 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/pwm_trigger.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/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++) { + 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 trigger mode before - if (-EBUSY == io_timer_is_channel_free(channel)) { - io_timer_free_channel(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()