diff --git a/Tools/module_config/output_groups_from_timer_config.py b/Tools/module_config/output_groups_from_timer_config.py index 62ea7c8214c6..8a932ec9dea7 100755 --- a/Tools/module_config/output_groups_from_timer_config.py +++ b/Tools/module_config/output_groups_from_timer_config.py @@ -54,12 +54,21 @@ def extract_timer_from_channel(line, timer_names): return None +def imxrt_is_dshot(line): + + # NXP FlexPWM format format: initIOPWM(PWM::FlexPWM2), + search = re.search('(initIOPWMDshot)', line, re.IGNORECASE) + if search: + return True + + return False + def get_timer_groups(timer_config_file, verbose=False): with open(timer_config_file, 'r') as f: timer_config = f.read() # timers - dshot_support = {} # key: timer + dshot_support = {str(i): False for i in range(16)} timers_start_marker = 'io_timers_t io_timers' timers_start = timer_config.find(timers_start_marker) if timers_start == -1: @@ -78,10 +87,9 @@ def get_timer_groups(timer_config_file, verbose=False): if timer_type == 'imxrt': if verbose: print('imxrt timer found') timer_names.append(timer) + if imxrt_is_dshot(line): + dshot_support[str(len(timers))] = True 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 elif timer: if verbose: print('found timer def: {:}'.format(timer)) dshot_support[timer] = 'DMA' in line diff --git a/boards/px4/fmu-v6xrt/nuttx-config/include/board.h b/boards/px4/fmu-v6xrt/nuttx-config/include/board.h index 6b704139dc0c..83832f228da1 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6xrt/nuttx-config/include/board.h @@ -267,6 +267,10 @@ // This is the ENET_1G interface. +/* Dshot Disambiguation *******************************************************/ + +#define IOMUX_DSHOT_DEFAULT (IOMUX_DRIVE_HIGHSTRENGTH | IOMUX_SLEW_FAST) + // Compile time selection #if defined(CONFIG_ETH0_PHY_TJA1103) # define BOARD_PHY_ADDR (18) diff --git a/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig index ff0a42fdfdec..28df156d1bb2 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig @@ -87,7 +87,6 @@ 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/nuttx-config/scripts/itcm_static_functions.ld b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_static_functions.ld index 5d9cd8de738b..4968488245e5 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_static_functions.ld +++ b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_static_functions.ld @@ -6,6 +6,7 @@ *(.text.board_autoled_on) *(.text.clock_timer) *(.text.exception_common) +*(.text.flexio_irq_handler) *(.text.hrt_absolute_time) *(.text.hrt_call_enter) *(.text.hrt_tim_isr) diff --git a/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c b/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c index d83265ce8f55..db591baf81c9 100644 --- a/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c +++ b/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c @@ -114,7 +114,7 @@ const struct clock_configuration_s g_initial_clkconfig = { .div = 1, .mux = ACMP_CLK_ROOT_OSC_RC_48M_DIV2, }, - .flexio1_clk_root = + .flexio1_clk_root = /* 240 / 2 = 120Mhz */ { .enable = 1, .div = 2, diff --git a/boards/px4/fmu-v6xrt/src/timer_config.cpp b/boards/px4/fmu-v6xrt/src/timer_config.cpp index 1b1497c10137..04be42b4d8ed 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] = { - 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), + initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule0), + initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule1), + initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule2), + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule0), + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule1), + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule2), + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule3), + initIOPWMDshot(PWM::FlexPWM3, PWM::Submodule0), initIOPWM(PWM::FlexPWM3, PWM::Submodule1), initIOPWM(PWM::FlexPWM3, PWM::Submodule3), initIOPWM(PWM::FlexPWM4, PWM::Submodule0), @@ -106,14 +106,14 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { }; constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { - /* FMU_CH1 */ initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_23), - /* FMU_CH2 */ initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_25), - /* FMU_CH3 */ initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_27), - /* FMU_CH4 */ initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_06), - /* FMU_CH5 */ initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_08), - /* FMU_CH6 */ initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_10), - /* FMU_CH7 */ initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_19), - /* FMU_CH8 */ initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_29), + /* FMU_CH1 */ initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_23, GPIO_FLEXIO1_FLEXIO23_1 | IOMUX_DSHOT_DEFAULT, 23), + /* FMU_CH2 */ initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_25, GPIO_FLEXIO1_FLEXIO25_1 | IOMUX_DSHOT_DEFAULT, 25), + /* FMU_CH3 */ initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_27, GPIO_FLEXIO1_FLEXIO27_1 | IOMUX_DSHOT_DEFAULT, 27), + /* FMU_CH4 */ initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_06, GPIO_FLEXIO1_FLEXIO06_1 | IOMUX_DSHOT_DEFAULT, 6), + /* FMU_CH5 */ initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_08, GPIO_FLEXIO1_FLEXIO08_1 | IOMUX_DSHOT_DEFAULT, 8), + /* FMU_CH6 */ initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_10, GPIO_FLEXIO1_FLEXIO10_1 | IOMUX_DSHOT_DEFAULT, 10), + /* FMU_CH7 */ initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_19, GPIO_FLEXIO1_FLEXIO19_1 | IOMUX_DSHOT_DEFAULT, 19), + /* FMU_CH8 */ initIOTimerChannelDshot(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_29, GPIO_FLEXIO1_FLEXIO29_1 | IOMUX_DSHOT_DEFAULT, 29), /* FMU_CH9 */ initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_31), /* FMU_CH10 */ initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_21), /* FMU_CH11 */ initIOTimerChannel(io_timers, {PWM::PWM4_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_00), diff --git a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c index ab3408c12ca8..cb92dd1209a3 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c @@ -33,145 +33,454 @@ ****************************************************************************/ #include #include +#include #include +#include +#include #include #include #include #include +#include "barriers.h" #include "arm_internal.h" +#define FLEXIO_BASE IMXRT_FLEXIO1_BASE +#define FLEXIO_PREQ 120000000 #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 +#if defined(IOMUX_PULL_UP_47K) +#define IOMUX_PULL_UP IOMUX_PULL_UP_47K +#endif + +static const uint32_t gcr_decode[32] = { + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x9, 0xA, 0xB, 0x0, 0xD, 0xE, 0xF, + 0x0, 0x0, 0x2, 0x3, 0x0, 0x5, 0x6, 0x7, + 0x0, 0x0, 0x8, 0x1, 0x0, 0x4, 0xC, 0x0 +}; + +uint32_t erpms[DSHOT_TIMERS]; + +typedef enum { + DSHOT_START = 0, + DSHOT_12BIT_FIFO, + DSHOT_12BIT_TRANSFERRED, + DSHOT_TRANSMIT_COMPLETE, + BDSHOT_RECEIVE, + BDSHOT_RECEIVE_COMPLETE, +} dshot_state; + typedef struct dshot_handler_t { bool init; uint32_t data_seg1; uint32_t irq_data; + dshot_state state; + bool bdshot; + uint32_t raw_response; + uint16_t erpm; + uint32_t crc_error_cnt; + uint32_t frame_error_cnt; + uint32_t no_response_cnt; + uint32_t last_no_response_cnt; } dshot_handler_t; +#define BDSHOT_OFFLINE_COUNT 400 // If there are no responses for 400 setpoints ESC is offline + static dshot_handler_t dshot_inst[DSHOT_TIMERS] = {}; -struct flexio_dev_s *flexio_s; +static uint32_t dshot_tcmp; +static uint32_t bdshot_tcmp; +static uint32_t dshot_mask; +static uint32_t bdshot_recv_mask; +static uint32_t bdshot_parsed_recv_mask; -static int flexio_irq_handler(int irq, void *context, void *arg) +static inline uint32_t flexio_getreg32(uint32_t offset) { + return getreg32(FLEXIO_BASE + offset); +} - uint32_t flags = flexio_s->ops->get_shifter_status_flags(flexio_s); - uint32_t instance; +static inline void flexio_modifyreg32(unsigned int offset, + uint32_t clearbits, + uint32_t setbits) +{ + modifyreg32(FLEXIO_BASE + offset, clearbits, setbits); +} - 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)); +static inline void flexio_putreg32(uint32_t value, uint32_t offset) +{ + putreg32(value, FLEXIO_BASE + offset); +} - 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; - } - } - } +static inline void enable_shifter_status_interrupts(uint32_t mask) +{ + flexio_modifyreg32(IMXRT_FLEXIO_SHIFTSIEN_OFFSET, 0, mask); +} - return OK; +static inline void disable_shifter_status_interrupts(uint32_t mask) +{ + flexio_modifyreg32(IMXRT_FLEXIO_SHIFTSIEN_OFFSET, mask, 0); } -int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) +static inline uint32_t get_shifter_status_flags(void) { - uint32_t timer_compare; + return flexio_getreg32(IMXRT_FLEXIO_SHIFTSTAT_OFFSET); +} - if (dshot_pwm_freq == 150000) { - timer_compare = 0x2F8A; +static inline void clear_shifter_status_flags(uint32_t mask) +{ + flexio_putreg32(mask, IMXRT_FLEXIO_SHIFTSTAT_OFFSET); +} - } else if (dshot_pwm_freq == 300000) { - timer_compare = 0x2F45; +static inline void enable_timer_status_interrupts(uint32_t mask) +{ + flexio_modifyreg32(IMXRT_FLEXIO_TIMIEN_OFFSET, 0, mask); +} - } else if (dshot_pwm_freq == 600000) { - timer_compare = 0x2F22; +static inline void disable_timer_status_interrupts(uint32_t mask) +{ + flexio_modifyreg32(IMXRT_FLEXIO_TIMIEN_OFFSET, mask, 0); +} - } else if (dshot_pwm_freq == 1200000) { - timer_compare = 0x2F11; +static inline uint32_t get_timer_status_flags(void) +{ + return flexio_getreg32(IMXRT_FLEXIO_TIMSTAT_OFFSET); +} - } else { - // Not supported Dshot frequency - return 0; +static inline void clear_timer_status_flags(uint32_t mask) +{ + flexio_putreg32(mask, IMXRT_FLEXIO_TIMSTAT_OFFSET); +} + +static void flexio_dshot_output(uint32_t channel, uint32_t pin, uint32_t timcmp, bool inverted) +{ + /* Disable Shifter */ + flexio_putreg32(0, IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4); + + /* No start bit, stop bit low */ + flexio_putreg32(FLEXIO_SHIFTCFG_INSRC(FLEXIO_SHIFTER_INPUT_FROM_PIN) | + FLEXIO_SHIFTCFG_PWIDTH(0) | + FLEXIO_SHIFTCFG_SSTOP(FLEXIO_SHIFTER_STOP_BIT_LOW) | + FLEXIO_SHIFTCFG_SSTART(FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_ENABLE), + IMXRT_FLEXIO_SHIFTCFG0_OFFSET + channel * 0x4); + + /* Transmit mode, output to FXIO pin, inverted output for bdshot */ + flexio_putreg32(FLEXIO_SHIFTCTL_TIMSEL(channel) | + FLEXIO_SHIFTCTL_TIMPOL(FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE) | + FLEXIO_SHIFTCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT) | + FLEXIO_SHIFTCTL_PINSEL(pin) | + FLEXIO_SHIFTCTL_PINPOL(inverted) | + FLEXIO_SHIFTCTL_SMOD(FLEXIO_SHIFTER_MODE_TRANSMIT), + IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4); + + /* Start transmitting on trigger, disable on compare */ + flexio_putreg32(FLEXIO_TIMCFG_TIMOUT(FLEXIO_TIMER_OUTPUT_ONE_NOT_AFFECTED_BY_RESET) | + FLEXIO_TIMCFG_TIMDEC(FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT) | + FLEXIO_TIMCFG_TIMRST(FLEXIO_TIMER_RESET_NEVER) | + FLEXIO_TIMCFG_TIMDIS(FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE) | + FLEXIO_TIMCFG_TIMENA(FLEXIO_TIMER_ENABLE_ON_TRIGGER_HIGH) | + FLEXIO_TIMCFG_TSTOP(FLEXIO_TIMER_STOP_BIT_DISABLED) | + FLEXIO_TIMCFG_TSTART(FLEXIO_TIMER_START_BIT_DISABLED), + IMXRT_FLEXIO_TIMCFG0_OFFSET + channel * 0x4); + + flexio_putreg32(timcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4); + + /* Baud mode, Trigger on shifter write */ + flexio_putreg32(FLEXIO_TIMCTL_TRGSEL((4 * channel) + 1) | + FLEXIO_TIMCTL_TRGPOL(FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_LOW) | + FLEXIO_TIMCTL_TRGSRC(FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL) | + FLEXIO_TIMCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) | + FLEXIO_TIMCTL_PINSEL(0) | + FLEXIO_TIMCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) | + FLEXIO_TIMCTL_TIMOD(FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT), + IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4); + +} + +static int flexio_irq_handler(int irq, void *context, void *arg) +{ + uint32_t flags = get_shifter_status_flags(); + uint32_t channel; + + for (channel = 0; flags && channel < DSHOT_TIMERS; channel++) { + if (flags & (1 << channel)) { + disable_shifter_status_interrupts(1 << channel); + + if (dshot_inst[channel].state == DSHOT_START) { + dshot_inst[channel].state = DSHOT_12BIT_FIFO; + flexio_putreg32(dshot_inst[channel].irq_data, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4); + + } else if (dshot_inst[channel].state == BDSHOT_RECEIVE) { + dshot_inst[channel].state = BDSHOT_RECEIVE_COMPLETE; + dshot_inst[channel].raw_response = flexio_getreg32(IMXRT_FLEXIO_SHIFTBUFBIS0_OFFSET + channel * 0x4); + + bdshot_recv_mask |= (1 << channel); + + if (bdshot_recv_mask == dshot_mask) { + // Received telemetry on all channels + // Schedule workqueue? + } + } + } } - /* Init FlexIO peripheral */ + flags = get_timer_status_flags(); + + for (channel = 0; flags; (channel = (channel + 1) % DSHOT_TIMERS)) { + flags = get_timer_status_flags(); + + if (flags & (1 << channel)) { + clear_timer_status_flags(1 << channel); + + if (dshot_inst[channel].state == DSHOT_12BIT_FIFO) { + dshot_inst[channel].state = DSHOT_12BIT_TRANSFERRED; + + } else if (!dshot_inst[channel].bdshot && dshot_inst[channel].state == DSHOT_12BIT_TRANSFERRED) { + dshot_inst[channel].state = DSHOT_TRANSMIT_COMPLETE; + + } else if (dshot_inst[channel].bdshot && dshot_inst[channel].state == DSHOT_12BIT_TRANSFERRED) { + disable_shifter_status_interrupts(1 << channel); + dshot_inst[channel].state = BDSHOT_RECEIVE; + + /* Transmit done, disable timer and reconfigure to receive*/ + flexio_putreg32(0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4); + + /* Input data from pin, no start/stop bit*/ + flexio_putreg32(FLEXIO_SHIFTCFG_INSRC(FLEXIO_SHIFTER_INPUT_FROM_PIN) | + FLEXIO_SHIFTCFG_PWIDTH(0) | + FLEXIO_SHIFTCFG_SSTOP(FLEXIO_SHIFTER_STOP_BIT_DISABLE) | + FLEXIO_SHIFTCFG_SSTART(FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_SHIFT), + IMXRT_FLEXIO_SHIFTCFG0_OFFSET + channel * 0x4); + + /* Shifter receive mdoe, on FXIO pin input */ + flexio_putreg32(FLEXIO_SHIFTCTL_TIMSEL(channel) | + FLEXIO_SHIFTCTL_TIMPOL(FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE) | + FLEXIO_SHIFTCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) | + FLEXIO_SHIFTCTL_PINSEL(timer_io_channels[channel].dshot.flexio_pin) | + FLEXIO_SHIFTCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) | + FLEXIO_SHIFTCTL_SMOD(FLEXIO_SHIFTER_MODE_RECEIVE), + IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4); + + /* Make sure there no shifter flags high from transmission */ + clear_shifter_status_flags(1 << channel); + + /* Enable on pin transition, resychronize through reset on rising edge */ + flexio_putreg32(FLEXIO_TIMCFG_TIMOUT(FLEXIO_TIMER_OUTPUT_ONE_AFFECTED_BY_RESET) | + FLEXIO_TIMCFG_TIMDEC(FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT) | + FLEXIO_TIMCFG_TIMRST(FLEXIO_TIMER_RESET_ON_TIMER_PIN_RISING_EDGE) | + FLEXIO_TIMCFG_TIMDIS(FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE) | + FLEXIO_TIMCFG_TIMENA(FLEXIO_TIMER_ENABLE_ON_TRIGGER_BOTH_EDGE) | + FLEXIO_TIMCFG_TSTOP(FLEXIO_TIMER_STOP_BIT_ENABLE_ON_TIMER_DISABLE) | + FLEXIO_TIMCFG_TSTART(FLEXIO_TIMER_START_BIT_ENABLED), + IMXRT_FLEXIO_TIMCFG0_OFFSET + channel * 0x4); + + /* Enable on pin transition, resychronize through reset on rising edge */ + flexio_putreg32(bdshot_tcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4); + + /* Trigger on FXIO pin transition, Baud mode */ + flexio_putreg32(FLEXIO_TIMCTL_TRGSEL(2 * timer_io_channels[channel].dshot.flexio_pin) | + FLEXIO_TIMCTL_TRGPOL(FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_HIGH) | + FLEXIO_TIMCTL_TRGSRC(FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL) | + FLEXIO_TIMCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) | + FLEXIO_TIMCTL_PINSEL(0) | + FLEXIO_TIMCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) | + FLEXIO_TIMCTL_TIMOD(FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT), + IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4); + + /* Enable shifter interrupt for receiving data */ + enable_shifter_status_interrupts(1 << channel); + } + } + + } - flexio_s = imxrt_flexio_initialize(1); + return OK; +} + + +int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) +{ + /* Calculate dshot timings based on dshot_pwm_freq */ + dshot_tcmp = 0x2F00 | (((FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2)) & 0xFF); + bdshot_tcmp = 0x2900 | (((FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 1) & 0xFF); + + /* Clock FlexIO peripheral */ + imxrt_clockall_flexio1(); + + /* Reset FlexIO peripheral */ + flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0, + FLEXIO_CTRL_SWRST_MASK); + flexio_putreg32(0, IMXRT_FLEXIO_CTRL_OFFSET); + + /* Initialize FlexIO peripheral */ + flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, + (FLEXIO_CTRL_DOZEN_MASK | + FLEXIO_CTRL_DBGE_MASK | + FLEXIO_CTRL_FASTACC_MASK | + FLEXIO_CTRL_FLEXEN_MASK), + (FLEXIO_CTRL_DBGE(1) | + FLEXIO_CTRL_FASTACC(1) | + FLEXIO_CTRL_FLEXEN(0))); + + /* FlexIO IRQ handling */ up_enable_irq(IMXRT_IRQ_FLEXIO1); - irq_attach(IMXRT_IRQ_FLEXIO1, flexio_irq_handler, flexio_s); + irq_attach(IMXRT_IRQ_FLEXIO1, flexio_irq_handler, 0); + + dshot_mask = 0x0; 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 + if (timer_io_channels[channel].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); + imxrt_config_gpio(timer_io_channels[channel].dshot.pinmux | IOMUX_PULL_UP); + + dshot_inst[channel].bdshot = enable_bidirectional_dshot; + + flexio_dshot_output(channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot); dshot_inst[channel].init = true; + + // Mask channel to be active on dshot + dshot_mask |= (1 << channel); } } - flexio_s->ops->enable(flexio_s, true); + flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0, + FLEXIO_CTRL_FLEXEN_MASK); return channel_mask; } +void up_bdshot_erpm(void) +{ + uint32_t value; + uint32_t data; + uint32_t csum_data; + uint8_t exponent; + uint16_t period; + uint16_t erpm; + + bdshot_parsed_recv_mask = 0; + + // Decode each individual channel + for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) { + if (bdshot_recv_mask & (1 << channel)) { + value = ~dshot_inst[channel].raw_response & 0xFFFFF; + + /* if lowest significant isn't 1 we've got a framing error */ + if (value & 0x1) { + /* Decode RLL */ + value = (value ^ (value >> 1)); + + /* Decode GCR */ + data = gcr_decode[value & 0x1fU]; + data |= gcr_decode[(value >> 5U) & 0x1fU] << 4U; + data |= gcr_decode[(value >> 10U) & 0x1fU] << 8U; + data |= gcr_decode[(value >> 15U) & 0x1fU] << 12U; + + /* Calculate checksum */ + csum_data = data; + csum_data = csum_data ^ (csum_data >> 8U); + csum_data = csum_data ^ (csum_data >> NIBBLES_SIZE); + + if ((csum_data & 0xFU) != 0xFU) { + dshot_inst[channel].crc_error_cnt++; + + } else { + data = (data >> 4) & 0xFFF; + + if (data == 0xFFF) { + erpm = 0; + + } else { + exponent = ((data >> 9U) & 0x7U); /* 3 bit: exponent */ + period = (data & 0x1ffU); /* 9 bit: period base */ + period = period << exponent; /* Period in usec */ + erpm = ((1000000U * 60U / 100U + period / 2U) / period); + } + + dshot_inst[channel].erpm = erpm; + bdshot_parsed_recv_mask |= (1 << channel); + dshot_inst[channel].last_no_response_cnt = dshot_inst[channel].no_response_cnt; + } + + } else { + dshot_inst[channel].frame_error_cnt++; + } + } + } +} + + + +int up_bdshot_get_erpm(uint8_t channel, int *erpm) +{ + if (bdshot_parsed_recv_mask & (1 << channel)) { + *erpm = (int)dshot_inst[channel].erpm; + return 0; + } + + return -1; +} + +int up_bdshot_channel_status(uint8_t channel) +{ + if (channel < DSHOT_TIMERS) { + return ((dshot_inst[channel].no_response_cnt - dshot_inst[channel].last_no_response_cnt) < BDSHOT_OFFLINE_COUNT); + } + + return -1; +} + +void up_bdshot_status(void) +{ + + for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) { + + if (dshot_inst[channel].init) { + PX4_INFO("Channel %i %s Last erpm %i value", channel, up_bdshot_channel_status(channel) ? "online" : "offline", + dshot_inst[channel].erpm); + PX4_INFO("CRC errors Frame error No response"); + PX4_INFO("%10lu %11lu %11lu", dshot_inst[channel].crc_error_cnt, dshot_inst[channel].frame_error_cnt, + dshot_inst[channel].no_response_cnt); + } + } +} + void up_dshot_trigger(void) { - uint32_t buf_adr; + // Calc data now since we're not event driven + if (bdshot_recv_mask != 0x0) { + up_bdshot_erpm(); + } + + clear_timer_status_flags(0xFF); - 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); + for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) { + if (dshot_inst[channel].bdshot && (bdshot_recv_mask & (1 << channel)) == 0) { + dshot_inst[channel].no_response_cnt++; + } + + if (dshot_inst[channel].init && dshot_inst[channel].data_seg1 != 0) { + flexio_putreg32(dshot_inst[channel].data_seg1, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4); } } - flexio_s->ops->clear_timer_status_flags(flexio_s, 0xFF); - flexio_s->ops->enable_shifter_status_interrupts(flexio_s, 0xFF); + bdshot_recv_mask = 0x0; + + clear_timer_status_flags(0xFF); + enable_shifter_status_interrupts(0xFF); + enable_timer_status_interrupts(0xFF); } +/* Expand packet from 16 bits 48 to get T0H and T1H timing */ uint64_t dshot_expand_data(uint16_t packet) { unsigned int mask; @@ -197,16 +506,22 @@ uint64_t dshot_expand_data(uint16_t packet) * bit 12 - dshot telemetry enable/disable * bits 13-16 - XOR checksum **/ -void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry) +void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) { - if (motor_number < DSHOT_TIMERS && dshot_inst[motor_number].init) { + if (channel < DSHOT_TIMERS && dshot_inst[channel].init) { + uint16_t csum_data; 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; + if (dshot_inst[channel].bdshot) { + csum_data = ~packet; + + } else { + csum_data = packet; + } /* XOR checksum calculation */ csum_data >>= NIBBLES_SIZE; @@ -219,8 +534,20 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet 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); + + dshot_inst[channel].data_seg1 = (uint32_t)(dshot_expanded & 0xFFFFFF); + dshot_inst[channel].irq_data = (uint32_t)(dshot_expanded >> 24); + dshot_inst[channel].state = DSHOT_START; + + if (dshot_inst[channel].bdshot) { + + flexio_putreg32(0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4); + disable_shifter_status_interrupts(1 << channel); + + flexio_dshot_output(channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot); + + clear_timer_status_flags(0xFF); + } } } 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 4cffe9c858a5..1fba2e04494f 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 @@ -87,7 +87,6 @@ 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 { @@ -112,6 +111,7 @@ typedef struct timer_io_channels_t { uint8_t sub_module; /* 0 based sub module offset */ uint8_t sub_module_bits; /* LDOK and CLDOK bits */ uint8_t timer_channel; /* Unused */ + dshot_conf_t dshot; } timer_io_channels_t; #define SM0 0 diff --git a/platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt index d94ea8d902f0..f487b031c7fe 100644 --- a/platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt @@ -36,7 +36,7 @@ add_subdirectory(../imxrt/adc adc) add_subdirectory(../imxrt/board_critmon board_critmon) add_subdirectory(../imxrt/board_hw_info board_hw_info) add_subdirectory(../imxrt/board_reset board_reset) -#add_subdirectory(../imxrt/dshot dshot) +add_subdirectory(../imxrt/dshot dshot) add_subdirectory(../imxrt/hrt hrt) add_subdirectory(../imxrt/led_pwm led_pwm) add_subdirectory(../imxrt/io_pins io_pins) diff --git a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/dshot.h b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/dshot.h new file mode 100644 index 000000000000..b6aaca410a9d --- /dev/null +++ b/platforms/nuttx/src/px4/nxp/rt106x/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/rt106x/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h index 613ac1b19777..83938d918bfe 100644 --- a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h @@ -41,6 +41,7 @@ #include #include +#include "dshot.h" #pragma once __BEGIN_DECLS @@ -110,6 +111,7 @@ typedef struct timer_io_channels_t { uint8_t sub_module; /* 0 based sub module offset */ uint8_t sub_module_bits; /* LDOK and CLDOK bits */ uint8_t timer_channel; /* Unused */ + dshot_conf_t dshot; } timer_io_channels_t; #define SM0 0 diff --git a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer_hw_description.h index e371b6de83d6..5cbc33fdc3a9 100644 --- a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer_hw_description.h +++ b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer_hw_description.h @@ -601,6 +601,16 @@ static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t return ret; } +static inline constexpr timer_io_channels_t initIOTimerChannelDshot(const io_timers_t io_timers_conf[MAX_IO_TIMERS], + PWM::FlexPWMConfig pwm_config, IOMUX::Pad pad, uint32_t dshot_pinmux, uint32_t flexio_pin) +{ + timer_io_channels_t ret = initIOTimerChannel(io_timers_conf, pwm_config, pad); + + ret.dshot.pinmux = dshot_pinmux; + ret.dshot.flexio_pin = flexio_pin; + return ret; +} + static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub) { io_timers_t ret{}; @@ -609,3 +619,14 @@ 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) +{ + io_timers_t ret{}; + + ret.base = getFlexPWMBaseRegister(pwm); + ret.submodle = sub; + return ret; +} 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 77dbfe918417..0bcf5c302ab8 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 @@ -690,6 +690,16 @@ static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t return ret; } +static inline constexpr timer_io_channels_t initIOTimerChannelDshot(const io_timers_t io_timers_conf[MAX_IO_TIMERS], + PWM::FlexPWMConfig pwm_config, IOMUX::Pad pad, uint32_t dshot_pinmux, uint32_t flexio_pin) +{ + timer_io_channels_t ret = initIOTimerChannel(io_timers_conf, pwm_config, pad); + + ret.dshot.pinmux = dshot_pinmux; + ret.dshot.flexio_pin = flexio_pin; + return ret; +} + static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub) { io_timers_t ret{}; @@ -699,14 +709,13 @@ static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubm return ret; } -static inline constexpr io_timers_t initIOPWMDshot(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub, uint32_t pinmux, - uint32_t flexio_pin) + + +static inline constexpr io_timers_t initIOPWMDshot(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub) { io_timers_t ret{}; ret.base = getFlexPWMBaseRegister(pwm); ret.submodle = sub; - ret.dshot.pinmux = pinmux; - ret.dshot.flexio_pin = flexio_pin; return ret; } diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 5b501850668a..00491e976831 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -82,7 +82,7 @@ static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(M px4_cache_aligned_data() = {}; static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {}; -int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) +int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) { unsigned buffer_offset = 0; @@ -152,6 +152,22 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) return ret_val == OK ? channels_init_mask : ret_val; } +int up_bdshot_get_erpm(uint8_t channel, int *erpm) +{ + // Not implemented + return -1; +} + +int up_bdshot_channel_status(uint8_t channel) +{ + // Not implemented + return -1; +} + +void up_bdshot_status(void) +{ +} + void up_dshot_trigger(void) { for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) { diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 2e8dab2d6db9..81c92b4d50ee 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -91,7 +91,7 @@ typedef enum { * @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200 * @return <0 on error, the initialized channels mask. */ -__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq); +__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot); /** * Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method) @@ -137,4 +137,28 @@ __EXPORT extern void up_dshot_trigger(void); */ __EXPORT extern int up_dshot_arm(bool armed); +/** + * Print bidrectional dshot status + */ +__EXPORT extern void up_bdshot_status(void); + + +/** + * Get bidrectional dshot erpm for a channel + * @param channel Dshot channel + * @param erpm pointer to write the erpm value + * @return <0 on error, OK on succes + */ +__EXPORT extern int up_bdshot_get_erpm(uint8_t channel, int *erpm); + + +/** + * Get bidrectional dshot status for a channel + * @param channel Dshot channel + * @param erpm pointer to write the erpm value + * @return <0 on error / not supported, 0 on offline, 1 on online + */ +__EXPORT extern int up_bdshot_channel_status(uint8_t channel); + + __END_DECLS diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 5ae0150efba9..940019d9bf04 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -144,7 +144,9 @@ void DShot::enable_dshot_outputs(const bool enabled) } } - int ret = up_dshot_init(_output_mask, dshot_frequency); + _bidirectional_dshot_enabled = _param_bidirectional_enable.get(); + + int ret = up_dshot_init(_output_mask, dshot_frequency, _bidirectional_dshot_enabled); if (ret < 0) { PX4_ERR("up_dshot_init failed (%i)", ret); @@ -157,6 +159,7 @@ void DShot::enable_dshot_outputs(const bool enabled) for (unsigned i = 0; i < _num_outputs; ++i) { if (((1 << i) & _output_mask) == 0) { _mixing_output.disableFunction(i); + } } @@ -167,6 +170,10 @@ void DShot::enable_dshot_outputs(const bool enabled) } _outputs_initialized = true; + + if (_bidirectional_dshot_enabled) { + init_telemetry(NULL); + } } if (_outputs_initialized) { @@ -206,17 +213,20 @@ void DShot::init_telemetry(const char *device) _telemetry->esc_status_pub.advertise(); - int ret = _telemetry->handler.init(device); + if (device != NULL) { + int ret = _telemetry->handler.init(device); - if (ret != 0) { - PX4_ERR("telemetry init failed (%i)", ret); + if (ret != 0) { + PX4_ERR("telemetry init failed (%i)", ret); + } } update_telemetry_num_motors(); } -void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data) +int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data) { + int ret = 0; // fill in new motor data esc_status_s &esc_status = _telemetry->esc_status_pub.get(); @@ -239,18 +249,83 @@ void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTele esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; esc_status.esc_count = _telemetry->handler.numMotors(); ++esc_status.counter; - // FIXME: mark all ESC's as online, otherwise commander complains even for a single dropout - esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1; - esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1; - - _telemetry->esc_status_pub.update(); - // reset esc data (in case a motor times out, so we won't send stale data) - memset(&esc_status.esc, 0, sizeof(_telemetry->esc_status_pub.get().esc)); - esc_status.esc_online_flags = 0; + ret = 1; // Indicate we wrapped, so we publish data } _telemetry->last_telemetry_index = telemetry_index; + + return ret; +} + +void DShot::publish_esc_status(void) +{ + esc_status_s &esc_status = _telemetry->esc_status_pub.get(); + int telemetry_index = 0; + + // clear data of the esc that are offline + for (int index = 0; (index < _telemetry->last_telemetry_index); index++) { + if ((esc_status.esc_online_flags & (1 << index)) == 0) { + memset(&esc_status.esc[index], 0, sizeof(struct esc_report_s)); + } + } + + // FIXME: mark all UART Telemetry ESC's as online, otherwise commander complains even for a single dropout + esc_status.esc_count = _telemetry->handler.numMotors(); + esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1; + esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1; + + if (_bidirectional_dshot_enabled) { + for (unsigned i = 0; i < _num_outputs; i++) { + if (_mixing_output.isFunctionSet(i)) { + if (up_bdshot_channel_status(i)) { + esc_status.esc_online_flags |= 1 << i; + + } else { + esc_status.esc_online_flags &= ~(1 << i); + } + + ++telemetry_index; + } + } + } + + // ESC telem wrap around or bdshot update + _telemetry->esc_status_pub.update(); + + // reset esc online flags + esc_status.esc_online_flags = 0; +} + +int DShot::handle_new_bdshot_erpm(void) +{ + int num_erpms = 0; + int telemetry_index = 0; + int erpm; + esc_status_s &esc_status = _telemetry->esc_status_pub.get(); + + esc_status.timestamp = hrt_absolute_time(); + esc_status.counter = _esc_status_counter++; + esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; + esc_status.esc_armed_flags = _outputs_on; + + for (unsigned i = 0; i < _num_outputs; i++) { + if (_mixing_output.isFunctionSet(i)) { + if (up_bdshot_get_erpm(i, &erpm) == 0) { + num_erpms++; + esc_status.esc_online_flags |= 1 << telemetry_index; + esc_status.esc[telemetry_index].timestamp = hrt_absolute_time(); + esc_status.esc[telemetry_index].esc_rpm = (erpm * 100) / (_param_mot_pole_count.get() / 2); + esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index]; + } + + ++telemetry_index; + } + + + } + + return num_erpms; } int DShot::send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index) @@ -463,6 +538,7 @@ void DShot::Run() if (_telemetry) { int telem_update = _telemetry->handler.update(); + int need_to_publish = 0; // Are we waiting for ESC info? if (_waiting_for_esc_info) { @@ -472,10 +548,21 @@ void DShot::Run() } } else if (telem_update >= 0) { - handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData()); + need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData()); + } + + if (_bidirectional_dshot_enabled) { + // Add bdshot data to esc status + need_to_publish += handle_new_bdshot_erpm(); + } + + if (need_to_publish > 0) { + // ESC telem wrap around or bdshot update + publish_esc_status(); } } + if (_parameter_update_sub.updated()) { update_params(); } @@ -713,6 +800,11 @@ int DShot::print_status() _telemetry->handler.printStatus(); } + /* Print dshot status */ + if (_bidirectional_dshot_enabled) { + up_bdshot_status(); + } + return 0; } diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 4f314cebb75f..e60b33b86279 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -131,7 +131,11 @@ class DShot final : public ModuleBase, public OutputModuleInterface void init_telemetry(const char *device); - void handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data); + int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data); + + void publish_esc_status(void); + + int handle_new_bdshot_erpm(void); int request_esc_info(); @@ -158,6 +162,7 @@ class DShot final : public ModuleBase, public OutputModuleInterface bool _outputs_initialized{false}; bool _outputs_on{false}; bool _waiting_for_esc_info{false}; + bool _bidirectional_dshot_enabled{false}; static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS}; uint32_t _output_mask{0}; @@ -169,12 +174,14 @@ class DShot final : public ModuleBase, public OutputModuleInterface uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + uint16_t _esc_status_counter{0}; DEFINE_PARAMETERS( (ParamFloat) _param_dshot_min, (ParamBool) _param_dshot_3d_enable, (ParamInt) _param_dshot_3d_dead_h, (ParamInt) _param_dshot_3d_dead_l, - (ParamInt) _param_mot_pole_count + (ParamInt) _param_mot_pole_count, + (ParamBool) _param_bidirectional_enable ) }; diff --git a/src/drivers/dshot/DShotTelemetry.cpp b/src/drivers/dshot/DShotTelemetry.cpp index 14a21d5654e0..f0444bff1950 100644 --- a/src/drivers/dshot/DShotTelemetry.cpp +++ b/src/drivers/dshot/DShotTelemetry.cpp @@ -183,6 +183,9 @@ bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding) _latest_data.erpm); ++_num_successful_responses; successful_decoding = true; + + } else { + ++_num_checksum_errors; } return true; @@ -195,6 +198,7 @@ void DShotTelemetry::printStatus() const { PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses); PX4_INFO("Number of timeouts: %i", _num_timeouts); + PX4_INFO("Number of CRC errors: %i", _num_checksum_errors); } uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed) diff --git a/src/drivers/dshot/DShotTelemetry.h b/src/drivers/dshot/DShotTelemetry.h index a5c549c47676..1164d1e2008c 100644 --- a/src/drivers/dshot/DShotTelemetry.h +++ b/src/drivers/dshot/DShotTelemetry.h @@ -140,4 +140,5 @@ class DShotTelemetry // statistics int _num_timeouts{0}; int _num_successful_responses{0}; + int _num_checksum_errors{0}; }; diff --git a/src/drivers/dshot/module.yaml b/src/drivers/dshot/module.yaml index edd38edd23f1..e2dcf8bc97e8 100644 --- a/src/drivers/dshot/module.yaml +++ b/src/drivers/dshot/module.yaml @@ -33,6 +33,16 @@ parameters: When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent. type: boolean default: 0 + DSHOT_BIDIR_EN: + description: + short: Enable bidirectional DShot + long: | + This parameter enables bidirectional DShot which provides RPM feedback. + Note that this requires ESCs that support bidirectional DSHot, e.g. BlHeli32. + This is not the same as DShot telemetry which requires an additional serial connection. + type: boolean + default: 0 + reboot_required: true DSHOT_3D_DEAD_H: description: short: DSHOT 3D deadband high