From e2c027405e3d806a6eec02b5c48df6b6a4750db6 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Tue, 12 Dec 2023 15:12:34 +0100 Subject: [PATCH] px4_fmu-v6xrt: Add dshot support --- .../output_groups_from_timer_config.py | 2 + boards/px4/fmu-v6xrt/default.px4board | 1 + .../px4/fmu-v6xrt/nuttx-config/nsh/defconfig | 1 + boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c | 4 +- boards/px4/fmu-v6xrt/src/timer_config.cpp | 16 +- .../src/px4/nxp/imxrt/dshot/CMakeLists.txt | 37 +++ .../nuttx/src/px4/nxp/imxrt/dshot/dshot.c | 230 ++++++++++++++++++ .../px4/nxp/imxrt/include/px4_arch/dshot.h | 45 ++++ .../px4/nxp/imxrt/include/px4_arch/io_timer.h | 2 + .../nuttx/src/px4/nxp/rt117x/CMakeLists.txt | 1 + .../px4/nxp/rt117x/include/px4_arch/dshot.h | 36 +++ .../px4_arch/io_timer_hw_description.h | 12 + 12 files changed, 377 insertions(+), 10 deletions(-) create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/dshot/CMakeLists.txt create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c create mode 100644 platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/dshot.h create mode 100644 platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/dshot.h diff --git a/Tools/module_config/output_groups_from_timer_config.py b/Tools/module_config/output_groups_from_timer_config.py index 561c71fd84..ce16545a7d 100755 --- a/Tools/module_config/output_groups_from_timer_config.py +++ b/Tools/module_config/output_groups_from_timer_config.py @@ -80,6 +80,8 @@ def get_timer_groups(timer_config_file, verbose=False): 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)} + for i in range(8): # First 8 channels support dshot + dshot_support[str(i)] = True break if timer: diff --git a/boards/px4/fmu-v6xrt/default.px4board b/boards/px4/fmu-v6xrt/default.px4board index 97ce228b95..b5e2e23af3 100644 --- a/boards/px4/fmu-v6xrt/default.px4board +++ b/boards/px4/fmu-v6xrt/default.px4board @@ -12,6 +12,7 @@ CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y +CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y diff --git a/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig index 03fbbbc25c..4769e41b32 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig @@ -83,6 +83,7 @@ CONFIG_IMXRT_ENET=y CONFIG_IMXRT_FLEXCAN1=y CONFIG_IMXRT_FLEXCAN2=y CONFIG_IMXRT_FLEXCAN3=y +CONFIG_IMXRT_FLEXIO1=y CONFIG_IMXRT_FLEXSPI2=y CONFIG_IMXRT_GPIO13_IRQ=y CONFIG_IMXRT_GPIO1_0_15_IRQ=y diff --git a/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c b/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c index e482fe0803..d83265ce8f 100644 --- a/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c +++ b/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c @@ -117,8 +117,8 @@ const struct clock_configuration_s g_initial_clkconfig = { .flexio1_clk_root = { .enable = 1, - .div = 1, - .mux = FLEXIO1_CLK_ROOT_OSC_RC_48M_DIV2, + .div = 2, + .mux = FLEXIO1_CLK_ROOT_SYS_PLL3_DIV2, }, .flexio2_clk_root = { diff --git a/boards/px4/fmu-v6xrt/src/timer_config.cpp b/boards/px4/fmu-v6xrt/src/timer_config.cpp index c0258a34b3..1b1497c101 100644 --- a/boards/px4/fmu-v6xrt/src/timer_config.cpp +++ b/boards/px4/fmu-v6xrt/src/timer_config.cpp @@ -91,14 +91,14 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { - initIOPWM(PWM::FlexPWM1, PWM::Submodule0), - initIOPWM(PWM::FlexPWM1, PWM::Submodule1), - initIOPWM(PWM::FlexPWM1, PWM::Submodule2), - initIOPWM(PWM::FlexPWM2, PWM::Submodule0), - initIOPWM(PWM::FlexPWM2, PWM::Submodule1), - initIOPWM(PWM::FlexPWM2, PWM::Submodule2), - initIOPWM(PWM::FlexPWM2, PWM::Submodule3), - initIOPWM(PWM::FlexPWM3, PWM::Submodule0), + initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule0, GPIO_FLEXIO1_FLEXIO23_1, 23), + initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule1, GPIO_FLEXIO1_FLEXIO25_1, 25), + initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule2, GPIO_FLEXIO1_FLEXIO27_1, 27), + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule0, GPIO_FLEXIO1_FLEXIO06_1, 6), + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule1, GPIO_FLEXIO1_FLEXIO08_1, 8), + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule2, GPIO_FLEXIO1_FLEXIO10_1, 10), + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule3, GPIO_FLEXIO1_FLEXIO19_1, 19), + initIOPWMDshot(PWM::FlexPWM3, PWM::Submodule0, GPIO_FLEXIO1_FLEXIO29_1, 29), initIOPWM(PWM::FlexPWM3, PWM::Submodule1), initIOPWM(PWM::FlexPWM3, PWM::Submodule3), initIOPWM(PWM::FlexPWM4, PWM::Submodule0), diff --git a/platforms/nuttx/src/px4/nxp/imxrt/dshot/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/dshot/CMakeLists.txt new file mode 100644 index 0000000000..d8a7900d40 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2023 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. +# +############################################################################ + +px4_add_library(arch_dshot + dshot.c +) +target_compile_options(arch_dshot PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c new file mode 100644 index 0000000000..ab3408c12c --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c @@ -0,0 +1,230 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * Author: Peter van der Perk + * + * 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. + * + ****************************************************************************/ +#include +#include +#include +#include +#include +#include +#include + +#include "arm_internal.h" + +#define DSHOT_TIMERS FLEXIO_SHIFTBUFNIS_COUNT +#define DSHOT_THROTTLE_POSITION 5u +#define DSHOT_TELEMETRY_POSITION 4u +#define NIBBLES_SIZE 4u +#define DSHOT_NUMBER_OF_NIBBLES 3u + +typedef struct dshot_handler_t { + bool init; + uint32_t data_seg1; + uint32_t irq_data; +} dshot_handler_t; + +static dshot_handler_t dshot_inst[DSHOT_TIMERS] = {}; + +struct flexio_dev_s *flexio_s; + +static int flexio_irq_handler(int irq, void *context, void *arg) +{ + + uint32_t flags = flexio_s->ops->get_shifter_status_flags(flexio_s); + uint32_t instance; + + for (instance = 0; flags && instance < DSHOT_TIMERS; instance++) { + if (flags & (1 << instance)) { + flexio_s->ops->disable_shifter_status_interrupts(flexio_s, (1 << instance)); + flexio_s->ops->disable_timer_status_interrupts(flexio_s, (1 << instance)); + + if (dshot_inst[instance].irq_data != 0) { + uint32_t buf_adr = flexio_s->ops->get_shifter_buffer_address(flexio_s, FLEXIO_SHIFTER_BUFFER, instance); + putreg32(dshot_inst[instance].irq_data, IMXRT_FLEXIO1_BASE + buf_adr); + dshot_inst[instance].irq_data = 0; + } + } + } + + return OK; +} + +int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) +{ + uint32_t timer_compare; + + if (dshot_pwm_freq == 150000) { + timer_compare = 0x2F8A; + + } else if (dshot_pwm_freq == 300000) { + timer_compare = 0x2F45; + + } else if (dshot_pwm_freq == 600000) { + timer_compare = 0x2F22; + + } else if (dshot_pwm_freq == 1200000) { + timer_compare = 0x2F11; + + } else { + // Not supported Dshot frequency + return 0; + } + + /* Init FlexIO peripheral */ + + flexio_s = imxrt_flexio_initialize(1); + up_enable_irq(IMXRT_IRQ_FLEXIO1); + irq_attach(IMXRT_IRQ_FLEXIO1, flexio_irq_handler, flexio_s); + + for (unsigned channel = 0; (channel_mask != 0) && (channel < DSHOT_TIMERS); channel++) { + if (channel_mask & (1 << channel)) { + uint8_t timer = timer_io_channels[channel].timer_index; + + if (io_timers[timer].dshot.pinmux == 0) { // board does not configure dshot on this pin + continue; + } + + imxrt_config_gpio(io_timers[timer].dshot.pinmux); + + struct flexio_shifter_config_s shft_cfg; + shft_cfg.timer_select = channel; + shft_cfg.timer_polarity = FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE; + shft_cfg.pin_config = FLEXIO_PIN_CONFIG_OUTPUT; + shft_cfg.pin_select = io_timers[timer].dshot.flexio_pin; + shft_cfg.pin_polarity = FLEXIO_PIN_ACTIVE_HIGH; + shft_cfg.shifter_mode = FLEXIO_SHIFTER_MODE_TRANSMIT; + shft_cfg.parallel_width = 0; + shft_cfg.input_source = FLEXIO_SHIFTER_INPUT_FROM_PIN; + shft_cfg.shifter_stop = FLEXIO_SHIFTER_STOP_BIT_LOW; + shft_cfg.shifter_start = FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_ENABLE; + + flexio_s->ops->set_shifter_config(flexio_s, channel, &shft_cfg); + + struct flexio_timer_config_s tmr_cfg; + tmr_cfg.trigger_select = (4 * channel) + 1; + tmr_cfg.trigger_polarity = FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_LOW; + tmr_cfg.trigger_source = FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL; + tmr_cfg.pin_config = FLEXIO_PIN_CONFIG_OUTPUT_DISABLED; + tmr_cfg.pin_select = 0; + tmr_cfg.pin_polarity = FLEXIO_PIN_ACTIVE_LOW; + tmr_cfg.timer_mode = FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT; + tmr_cfg.timer_output = FLEXIO_TIMER_OUTPUT_ONE_NOT_AFFECTED_BY_RESET; + tmr_cfg.timer_decrement = FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT; + tmr_cfg.timer_reset = FLEXIO_TIMER_RESET_NEVER; + tmr_cfg.timer_disable = FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE; + tmr_cfg.timer_enable = FLEXIO_TIMER_ENABLE_ON_TRIGGER_HIGH; + tmr_cfg.timer_stop = FLEXIO_TIMER_STOP_BIT_DISABLED; + tmr_cfg.timer_start = FLEXIO_TIMER_START_BIT_DISABLED; + tmr_cfg.timer_compare = timer_compare; + flexio_s->ops->set_timer_config(flexio_s, channel, &tmr_cfg); + + dshot_inst[channel].init = true; + } + } + + flexio_s->ops->enable(flexio_s, true); + + return channel_mask; +} + +void up_dshot_trigger(void) +{ + uint32_t buf_adr; + + for (uint8_t motor_number = 0; (motor_number < DSHOT_TIMERS); motor_number++) { + if (dshot_inst[motor_number].init && dshot_inst[motor_number].data_seg1 != 0) { + buf_adr = flexio_s->ops->get_shifter_buffer_address(flexio_s, FLEXIO_SHIFTER_BUFFER, motor_number); + putreg32(dshot_inst[motor_number].data_seg1, IMXRT_FLEXIO1_BASE + buf_adr); + } + } + + flexio_s->ops->clear_timer_status_flags(flexio_s, 0xFF); + flexio_s->ops->enable_shifter_status_interrupts(flexio_s, 0xFF); +} + +uint64_t dshot_expand_data(uint16_t packet) +{ + unsigned int mask; + unsigned int index = 0; + uint64_t expanded = 0x0; + + for (mask = 0x8000; mask != 0; mask >>= 1) { + if (packet & mask) { + expanded = expanded | ((uint64_t)0x3 << index); + + } else { + expanded = expanded | ((uint64_t)0x1 << index); + } + + index = index + 3; + } + + return expanded; +} + +/** +* bits 1-11 - throttle value (0-47 are reserved, 48-2047 give 2000 steps of throttle resolution) +* bit 12 - dshot telemetry enable/disable +* bits 13-16 - XOR checksum +**/ +void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry) +{ + if (motor_number < DSHOT_TIMERS && dshot_inst[motor_number].init) { + uint16_t packet = 0; + uint16_t checksum = 0; + + packet |= throttle << DSHOT_THROTTLE_POSITION; + packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION; + + uint16_t csum_data = packet; + + /* XOR checksum calculation */ + csum_data >>= NIBBLES_SIZE; + + for (unsigned i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) { + checksum ^= (csum_data & 0x0F); // XOR data by nibbles + csum_data >>= NIBBLES_SIZE; + } + + packet |= (checksum & 0x0F); + + uint64_t dshot_expanded = dshot_expand_data(packet); + dshot_inst[motor_number].data_seg1 = (uint32_t)(dshot_expanded & 0xFFFFFF); + dshot_inst[motor_number].irq_data = (uint32_t)(dshot_expanded >> 24); + } +} + +int up_dshot_arm(bool armed) +{ + return io_timer_set_enable(armed, IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); +} diff --git a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/dshot.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/dshot.h new file mode 100644 index 0000000000..19d7c4d2f7 --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/dshot.h @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * Author: Peter van der Perk + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + + +/* The structure which contains configuration for DShot + */ +typedef struct dshot_conf_t { + uint32_t pinmux; + uint32_t flexio_pin; +} dshot_conf_t; 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 bd7cb88054..4cffe9c858 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 @@ -41,6 +41,7 @@ #include #include +#include "dshot.h" #pragma once __BEGIN_DECLS @@ -86,6 +87,7 @@ typedef struct io_timers_t { uint32_t clock_register; /* SIM_SCGCn */ uint32_t clock_bit; /* SIM_SCGCn bit pos */ uint32_t vectorno; /* IRQ number */ + dshot_conf_t dshot; } io_timers_t; typedef struct io_timers_channel_mapping_element_t { diff --git a/platforms/nuttx/src/px4/nxp/rt117x/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/rt117x/CMakeLists.txt index 83d3c17367..8875ab56b7 100644 --- a/platforms/nuttx/src/px4/nxp/rt117x/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/rt117x/CMakeLists.txt @@ -43,6 +43,7 @@ add_subdirectory(../imxrt/io_pins io_pins) add_subdirectory(../imxrt/tone_alarm tone_alarm) add_subdirectory(../imxrt/version version) add_subdirectory(../imxrt/spi spi) +add_subdirectory(../imxrt/dshot dshot) add_subdirectory(px4io_serial) add_subdirectory(ssarc) diff --git a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/dshot.h b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/dshot.h new file mode 100644 index 0000000000..b6aaca410a --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/dshot.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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. + * + ****************************************************************************/ +#pragma once + + +#include "../../../imxrt/include/px4_arch/dshot.h" diff --git a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/io_timer_hw_description.h index 438045b0ad..77dbfe9184 100644 --- a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/io_timer_hw_description.h +++ b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/io_timer_hw_description.h @@ -698,3 +698,15 @@ static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubm ret.submodle = sub; return ret; } + +static inline constexpr io_timers_t initIOPWMDshot(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub, uint32_t pinmux, + uint32_t flexio_pin) +{ + io_timers_t ret{}; + + ret.base = getFlexPWMBaseRegister(pwm); + ret.submodle = sub; + ret.dshot.pinmux = pinmux; + ret.dshot.flexio_pin = flexio_pin; + return ret; +}