diff --git a/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig index 76198dd15481..60a0e3e1648d 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig @@ -83,7 +83,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/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c index ab3408c12ca8..e0fad4bb9e79 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c @@ -33,78 +33,295 @@ ****************************************************************************/ #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 +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_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; } dshot_handler_t; 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 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); +} + +static inline uint32_t get_timer_status_flags(void) +{ + return flexio_getreg32(IMXRT_FLEXIO_TIMSTAT_OFFSET); +} + +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); - } else if (dshot_pwm_freq == 1200000) { - timer_compare = 0x2F11; +} + +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].irq_data != 0) { + flexio_putreg32(dshot_inst[channel].irq_data, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4); + dshot_inst[channel].irq_data = 0; + + } else if (dshot_inst[channel].irq_data == 0 && 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? + } + } + } + } + + flags = get_timer_status_flags(); + + for (channel = 0; flags && channel < DSHOT_TIMERS; channel++) { + clear_timer_status_flags(1 << channel); + + if (flags & (1 << channel)) { + if (dshot_inst[channel].state == DSHOT_START) { + 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; + uint8_t timer = timer_io_channels[channel].timer_index; + + /* 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(io_timers[timer].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 * io_timers[timer].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); + } + } - } else { - // Not supported Dshot frequency - return 0; } - /* Init FlexIO peripheral */ + return OK; +} - flexio_s = imxrt_flexio_initialize(1); + +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(0) | + 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)) { @@ -114,64 +331,107 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) 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(io_timers[timer].dshot.pinmux | IOMUX_PULL_UP); + + dshot_inst[channel].bdshot = enable_bidirectional_dshot; + + flexio_dshot_output(channel, io_timers[timer].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 erpm; + uint32_t csum_data; + + // 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 */ + erpm = gcr_decode[value & 0x1fU]; + erpm |= gcr_decode[(value >> 5U) & 0x1fU] << 4U; + erpm |= gcr_decode[(value >> 10U) & 0x1fU] << 8U; + erpm |= gcr_decode[(value >> 15U) & 0x1fU] << 12U; + + /* Calculate checksum */ + csum_data = erpm; + 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 { + dshot_inst[channel].erpm = ~(erpm >> 4) & 0xFFF; + //TODO store this or foward this + } + + } else { + dshot_inst[channel].frame_error_cnt++; + } + } + } +} + +void up_bdshot_status(void) +{ + /* Call this function to calculate last ERPM ideally a workqueue does this. + For now this to debug using the dshot status cli command */ + up_bdshot_erpm(); + + for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) { + + if (dshot_inst[channel].init) { + PX4_INFO("Channel %i Last erpm %i value", channel, 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; + clear_timer_status_flags(0xFF); + + for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) { + if ((bdshot_recv_mask & (1 << channel)) == 0) { + dshot_inst[channel].no_response_cnt++; + } - 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); + 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 +457,24 @@ 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) { + uint8_t timer = timer_io_channels[channel].timer_index; + + 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 +487,19 @@ 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, io_timers[timer].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot); + + clear_timer_status_flags(0xFF); + } } }