diff --git a/Tools/module_config/output_groups_from_timer_config.py b/Tools/module_config/output_groups_from_timer_config.py index ce16545a7d..62ea7c8214 100755 --- a/Tools/module_config/output_groups_from_timer_config.py +++ b/Tools/module_config/output_groups_from_timer_config.py @@ -34,24 +34,23 @@ def extract_timer(line): if search: return search.group(1), 'generic' - # nxp rt1062 format: initIOPWM(PWM::FlexPWM2), - search = re.search('PWM::Flex([0-9a-zA-Z_]+)[,\)]', line, re.IGNORECASE) + # NXP FlexPWM format format: initIOPWM(PWM::FlexPWM2), + search = re.search('PWM::Flex([0-9a-zA-Z_]+)..PWM::Submodule([0-9])[,\)]', line, re.IGNORECASE) if search: - return search.group(1), 'imxrt' + return (search.group(1) + '_' + search.group(2)), 'imxrt' return None, 'unknown' -def extract_timer_from_channel(line, num_channels_already_found): +def extract_timer_from_channel(line, timer_names): # Try format: initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), search = re.search('Timer::([0-9a-zA-Z_]+), ', line, re.IGNORECASE) if search: return search.group(1) - # nxp rt1062 format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06), - search = re.search('PWM::(PWM[0-9]+)[_,\)]', line, re.IGNORECASE) + # NXP FlexPWM format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06), + search = re.search('PWM::(PWM[0-9]+).*PWM::Submodule([0-9])', line, re.IGNORECASE) if search: - # imxrt uses a 1:1 timer group to channel association - return str(num_channels_already_found) + return str(timer_names.index((search.group(1) + '_' + search.group(2)))) return None @@ -69,6 +68,7 @@ def get_timer_groups(timer_config_file, verbose=False): open_idx, close_idx = find_matching_brackets(('{', '}'), timer_config, verbose) timers_str = timer_config[open_idx:close_idx] timers = [] + timer_names = [] for line in timers_str.splitlines(): line = line.strip() if len(line) == 0 or line.startswith('//'): @@ -77,14 +77,12 @@ def get_timer_groups(timer_config_file, verbose=False): if timer_type == 'imxrt': if verbose: print('imxrt timer found') - max_num_channels = 16 # Just add a fixed number of timers - timers = [str(i) for i in range(max_num_channels)] - dshot_support = {str(i): False for i in range(max_num_channels)} + timer_names.append(timer) + timers.append(str(len(timers))) + dshot_support = {str(i): False for i in range(16)} for i in range(8): # First 8 channels support dshot dshot_support[str(i)] = True - break - - if timer: + elif timer: if verbose: print('found timer def: {:}'.format(timer)) dshot_support[timer] = 'DMA' in line timers.append(timer) @@ -111,7 +109,7 @@ def get_timer_groups(timer_config_file, verbose=False): continue if verbose: print('--'+line+'--') - timer = extract_timer_from_channel(line, len(channel_timers)) + timer = extract_timer_from_channel(line, timer_names) if timer: if verbose: print('Found timer: {:} in channel line {:}'.format(timer, line)) 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 240c62d062..77534c35ff 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 @@ -255,6 +255,34 @@ 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; + static uint32_t channels_cache[MAX_IO_TIMERS] = {0}; + + if (validate_timer_index(timer) < 0) { + return channels; + + } else { + if (channels_cache[timer] == 0) { + /* Gather the channel bits that belong to the timer */ + + uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index; + uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count; + + for (unsigned chan_index = first_channel_index; chan_index < last_channel_index; chan_index++) { + channels |= 1 << chan_index; + } + + /* cache them */ + + channels_cache[timer] = channels; + } + } + + return channels_cache[timer]; +} + static uint32_t get_channel_mask(unsigned channel) { return io_timer_validate_channel_index(channel) == 0 ? 1 << channel : 0; @@ -391,41 +419,60 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) return rv; } -static int timer_set_rate(unsigned channel, unsigned rate) +static int timer_set_rate(unsigned timer, unsigned rate) { + int channels = get_timer_channels(timer); + irqstate_t flags = px4_enter_critical_section(); - rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT - ; - rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1; - rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) { + if ((1 << channel) & channels) { + rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT + ; + rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1; + rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + + } + } px4_leave_critical_section(flags); return 0; } -static inline void io_timer_set_oneshot_mode(unsigned channel) +static inline void io_timer_set_oneshot_mode(unsigned timer) { + int channels = get_timer_channels(timer); + irqstate_t flags = px4_enter_critical_section(); - uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); - rvalue &= ~SMCTRL_PRSC_MASK; - rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD; - rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT - ; - rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; - rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) { + if ((1 << channel) & channels) { + uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); + rvalue &= ~SMCTRL_PRSC_MASK; + rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD; + rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT + ; + rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; + rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + } + } px4_leave_critical_section(flags); } -static inline void io_timer_set_PWM_mode(unsigned channel) +static inline void io_timer_set_PWM_mode(unsigned timer) { + int channels = get_timer_channels(timer); + irqstate_t flags = px4_enter_critical_section(); - uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); - rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD); - rvalue |= SMCTRL_PRSC_DIV16; - rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT - ; - rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; - rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) { + if ((1 << channel) & channels) { + uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); + rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD); + rvalue |= SMCTRL_PRSC_DIV16; + rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT + ; + rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; + rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + } + } px4_leave_critical_section(flags); } @@ -530,33 +577,35 @@ int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode) break; } - uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index; - uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count; + int channels = get_timer_channels(timer); - for (uint32_t chan = first_channel_index; chan < last_channel_index; chan++) { + for (uint32_t chan = 0; chan < DIRECT_PWM_OUTPUT_CHANNELS; ++chan) { + if ((1 << chan) & channels) { - /* Clear all Faults */ - rFSTS0(timer) = FSTS_FFLAG_MASK; + /* Clear all Faults */ + rFSTS0(timer) = FSTS_FFLAG_MASK; - rCTRL2(timer, timer_io_channels[chan].sub_module) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP; - rCTRL(timer, timer_io_channels[chan].sub_module) = SMCTRL_PRSC_DIV16 | SMCTRL_FULL; - /* Edge aligned at 0 */ - rINIT(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; - rVAL0(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; - rVAL2(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; - rVAL4(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; - rFFILT0(timer) &= ~FFILT_FILT_PER_MASK; - rDISMAP0(timer, timer_io_channels[chan].sub_module) = 0xf000; - rDISMAP1(timer, timer_io_channels[chan].sub_module) = 0xf000; - rOUTEN(timer) |= timer_io_channels[chan].val_offset == PWMA_VAL ? OUTEN_PWMA_EN(1 << timer_io_channels[chan].sub_module) - : OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module); - rDTSRCSEL(timer) = 0; - rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module); - rMCTRL(timer) = timer_io_channels[chan].sub_module_bits; - io_timer_set_PWM_mode(chan); - timer_set_rate(chan, 50); + rCTRL2(timer, timer_io_channels[chan].sub_module) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP; + rCTRL(timer, timer_io_channels[chan].sub_module) = SMCTRL_PRSC_DIV16 | SMCTRL_FULL; + /* Edge aligned at 0 */ + rINIT(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; + rVAL0(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; + rVAL2(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; + rVAL4(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; + rFFILT0(timer) &= ~FFILT_FILT_PER_MASK; + rDISMAP0(timer, timer_io_channels[chan].sub_module) = 0xf000; + rDISMAP1(timer, timer_io_channels[chan].sub_module) = 0xf000; + rOUTEN(timer) |= timer_io_channels[chan].val_offset == PWMA_VAL ? OUTEN_PWMA_EN(1 << timer_io_channels[chan].sub_module) + : OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module); + rDTSRCSEL(timer) = 0; + rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module); + rMCTRL(timer) = timer_io_channels[chan].sub_module_bits; + } } + io_timer_set_PWM_mode(timer); + timer_set_rate(timer, 50); + px4_leave_critical_section(flags); } @@ -818,8 +867,7 @@ uint16_t io_channel_get_ccr(unsigned channel) return value; } -// The rt has 1:1 group to channel -uint32_t io_timer_get_group(unsigned group) +uint32_t io_timer_get_group(unsigned timer) { - return get_channel_mask(group); + return get_timer_channels(timer); }