fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 16:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

implement dshot on pwm 4

Browse Source 
Signed-off-by: dirksavage88 <dirksavage88@gmail.com>
This commit is contained in:
dirksavage88
2025-07-17 08:17:08 -04:00
parent 78a2a3d0dd
commit b23a7fc314
5 changed files with 327 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files
platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c
+277 -80
View File
@@ -45,7 +45,9 @@
#include "arm_internal.h"
#define FLEXIO_BASE IMXRT_FLEXIO1_BASE
#define FLEXIO1_BASE IMXRT_FLEXIO1_BASE
#define FLEXIO2_BASE IMXRT_FLEXIO2_BASE
#define DSHOT_TIMERS FLEXIO_SHIFTBUFNIS_COUNT
#define DSHOT_THROTTLE_POSITION 5u
#define DSHOT_TELEMETRY_POSITION 4u
@@ -98,77 +100,80 @@ static uint32_t dshot_mask;
static uint32_t bdshot_recv_mask;
static uint32_t bdshot_parsed_recv_mask;
static inline uint32_t flexio_getreg32(uint32_t offset)
bool flexio1_channels = false;
bool flexio2_channels = false;
static inline uint32_t flexio_getreg32(uint32_t flexio_base, uint32_t offset)
{
return getreg32(FLEXIO_BASE + offset);
return getreg32(flexio_base + offset);
}
static inline void flexio_modifyreg32(unsigned int offset,
static inline void flexio_modifyreg32(uint32_t flexio_base, unsigned int offset,
uint32_t clearbits,
uint32_t setbits)
{
modifyreg32(FLEXIO_BASE + offset, clearbits, setbits);
modifyreg32(flexio_base + offset, clearbits, setbits);
}
static inline void flexio_putreg32(uint32_t value, uint32_t offset)
static inline void flexio_putreg32(uint32_t flexio_base, uint32_t value, uint32_t offset)
{
putreg32(value, FLEXIO_BASE + offset);
putreg32(value, flexio_base + offset);
}
static inline void enable_shifter_status_interrupts(uint32_t mask)
static inline void enable_shifter_status_interrupts(uint32_t flexio_base, uint32_t mask)
{
flexio_modifyreg32(IMXRT_FLEXIO_SHIFTSIEN_OFFSET, 0, mask);
flexio_modifyreg32(flexio_base, IMXRT_FLEXIO_SHIFTSIEN_OFFSET, 0, mask);
}
static inline void disable_shifter_status_interrupts(uint32_t mask)
static inline void disable_shifter_status_interrupts(uint32_t flexio_base, uint32_t mask)
{
flexio_modifyreg32(IMXRT_FLEXIO_SHIFTSIEN_OFFSET, mask, 0);
flexio_modifyreg32(flexio_base, IMXRT_FLEXIO_SHIFTSIEN_OFFSET, mask, 0);
}
static inline uint32_t get_shifter_status_flags(void)
static inline uint32_t get_shifter_status_flags(uint32_t flexio_base)
{
return flexio_getreg32(IMXRT_FLEXIO_SHIFTSTAT_OFFSET);
return flexio_getreg32(flexio_base, IMXRT_FLEXIO_SHIFTSTAT_OFFSET);
}
static inline void clear_shifter_status_flags(uint32_t mask)
static inline void clear_shifter_status_flags(uint32_t flexio_base, uint32_t mask)
{
flexio_putreg32(mask, IMXRT_FLEXIO_SHIFTSTAT_OFFSET);
flexio_putreg32(flexio_base, mask, IMXRT_FLEXIO_SHIFTSTAT_OFFSET);
}
static inline void enable_timer_status_interrupts(uint32_t mask)
static inline void enable_timer_status_interrupts(uint32_t flexio_base, uint32_t mask)
{
flexio_modifyreg32(IMXRT_FLEXIO_TIMIEN_OFFSET, 0, mask);
flexio_modifyreg32(flexio_base, IMXRT_FLEXIO_TIMIEN_OFFSET, 0, mask);
}
static inline void disable_timer_status_interrupts(uint32_t mask)
static inline void disable_timer_status_interrupts(uint32_t flexio_base, uint32_t mask)
{
flexio_modifyreg32(IMXRT_FLEXIO_TIMIEN_OFFSET, mask, 0);
flexio_modifyreg32(flexio_base, IMXRT_FLEXIO_TIMIEN_OFFSET, mask, 0);
}
static inline uint32_t get_timer_status_flags(void)
static inline uint32_t get_timer_status_flags(uint32_t flexio_base)
{
return flexio_getreg32(IMXRT_FLEXIO_TIMSTAT_OFFSET);
return flexio_getreg32(flexio_base, IMXRT_FLEXIO_TIMSTAT_OFFSET);
}
static inline void clear_timer_status_flags(uint32_t mask)
static inline void clear_timer_status_flags(uint32_t flexio_base, uint32_t mask)
{
flexio_putreg32(mask, IMXRT_FLEXIO_TIMSTAT_OFFSET);
flexio_putreg32(flexio_base, mask, IMXRT_FLEXIO_TIMSTAT_OFFSET);
}
static void flexio_dshot_output(uint32_t channel, uint32_t pin, uint32_t timcmp, bool inverted)
static void flexio_dshot_output(uint32_t flexio_base, uint32_t channel, uint32_t pin, uint32_t timcmp, bool inverted)
{
/* Disable Shifter */
flexio_putreg32(0, IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4);
flexio_putreg32(flexio_base, 0, IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4);
/* No start bit, stop bit low */
flexio_putreg32(FLEXIO_SHIFTCFG_INSRC(FLEXIO_SHIFTER_INPUT_FROM_PIN) |
flexio_putreg32(flexio_base, 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_putreg32(flexio_base, FLEXIO_SHIFTCTL_TIMSEL(channel) |
FLEXIO_SHIFTCTL_TIMPOL(FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE) |
FLEXIO_SHIFTCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT) |
FLEXIO_SHIFTCTL_PINSEL(pin) |
@@ -177,7 +182,7 @@ static void flexio_dshot_output(uint32_t channel, uint32_t pin, uint32_t timcmp,
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_putreg32(flexio_base, 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) |
@@ -186,10 +191,10 @@ static void flexio_dshot_output(uint32_t channel, uint32_t pin, uint32_t timcmp,
FLEXIO_TIMCFG_TSTART(FLEXIO_TIMER_START_BIT_DISABLED),
IMXRT_FLEXIO_TIMCFG0_OFFSET + channel * 0x4);
flexio_putreg32(timcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4);
flexio_putreg32(flexio_base, timcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4);
/* Baud mode, Trigger on shifter write */
flexio_putreg32(FLEXIO_TIMCTL_TRGSEL((4 * channel) + 1) |
flexio_putreg32(flexio_base, 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) |
@@ -200,22 +205,23 @@ static void flexio_dshot_output(uint32_t channel, uint32_t pin, uint32_t timcmp,
}
static int flexio_irq_handler(int irq, void *context, void *arg)
static int flexio1_irq_handler(int irq, void *context, void *arg)
{
uint32_t flags = get_shifter_status_flags();
uint32_t flags = get_shifter_status_flags(FLEXIO1_BASE);
uint32_t channel;
//TODO: only do this for flexio 1 base
for (channel = 0; flags && channel < DSHOT_TIMERS; channel++) {
if (flags & (1 << channel)) {
disable_shifter_status_interrupts(1 << channel);
if ((flags & (1 << channel)) && timer_io_channels[channel].flex_io_base == 1) {
disable_shifter_status_interrupts(FLEXIO1_BASE, 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);
flexio_putreg32(FLEXIO1_BASE, 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);
dshot_inst[channel].raw_response = flexio_getreg32(FLEXIO1_BASE, IMXRT_FLEXIO_SHIFTBUFBIS0_OFFSET + channel * 0x4);
bdshot_recv_mask |= (1 << channel);
@@ -227,13 +233,13 @@ static int flexio_irq_handler(int irq, void *context, void *arg)
}
}
flags = get_timer_status_flags();
flags = get_timer_status_flags(FLEXIO1_BASE);
for (channel = 0; flags; (channel = (channel + 1) % DSHOT_TIMERS)) {
flags = get_timer_status_flags();
flags = get_timer_status_flags(FLEXIO1_BASE);
if (flags & (1 << channel)) {
clear_timer_status_flags(1 << channel);
if ((flags & (1 << channel)) && timer_io_channels[channel].flex_io_base == 1) {
clear_timer_status_flags(FLEXIO1_BASE, 1 << channel);
if (dshot_inst[channel].state == DSHOT_12BIT_FIFO) {
dshot_inst[channel].state = DSHOT_12BIT_TRANSFERRED;
@@ -242,21 +248,21 @@ static int flexio_irq_handler(int irq, void *context, void *arg)
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);
disable_shifter_status_interrupts(FLEXIO1_BASE, 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);
flexio_putreg32(FLEXIO1_BASE, 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_putreg32(FLEXIO1_BASE, 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_putreg32(FLEXIO1_BASE, 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) |
@@ -265,10 +271,10 @@ static int flexio_irq_handler(int irq, void *context, void *arg)
IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4);
/* Make sure there no shifter flags high from transmission */
clear_shifter_status_flags(1 << channel);
clear_shifter_status_flags(FLEXIO1_BASE, 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_putreg32(FLEXIO1_BASE, 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) |
@@ -278,10 +284,10 @@ static int flexio_irq_handler(int irq, void *context, void *arg)
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);
flexio_putreg32(FLEXIO1_BASE, 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_putreg32(FLEXIO1_BASE, 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) |
@@ -291,7 +297,7 @@ static int flexio_irq_handler(int irq, void *context, void *arg)
IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
/* Enable shifter interrupt for receiving data */
enable_shifter_status_interrupts(1 << channel);
enable_shifter_status_interrupts(FLEXIO1_BASE, 1 << channel);
}
}
@@ -300,6 +306,105 @@ static int flexio_irq_handler(int irq, void *context, void *arg)
return OK;
}
static int flexio2_irq_handler(int irq, void *context, void *arg)
{
uint32_t flags = get_shifter_status_flags(FLEXIO2_BASE);
uint32_t channel;
for (channel = 0; flags && channel < DSHOT_TIMERS; channel++) {
if ((flags & (1 << channel)) && timer_io_channels[channel].flex_io_base == 2) {
disable_shifter_status_interrupts(FLEXIO2_BASE, 1 << channel);
if (dshot_inst[channel].state == DSHOT_START) {
dshot_inst[channel].state = DSHOT_12BIT_FIFO;
flexio_putreg32(FLEXIO2_BASE, 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(FLEXIO2_BASE, 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(FLEXIO2_BASE);
for (channel = 0; flags; (channel = (channel + 1) % DSHOT_TIMERS)) {
flags = get_timer_status_flags(FLEXIO2_BASE);
if ((flags & (1 << channel)) && timer_io_channels[channel].flex_io_base == 2) {
clear_timer_status_flags(FLEXIO2_BASE, 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(FLEXIO2_BASE, 1 << channel);
dshot_inst[channel].state = BDSHOT_RECEIVE;
/* Transmit done, disable timer and reconfigure to receive*/
flexio_putreg32(FLEXIO2_BASE, 0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
/* Input data from pin, no start/stop bit*/
flexio_putreg32(FLEXIO2_BASE, 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(FLEXIO2_BASE, 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(FLEXIO2_BASE, 1 << channel);
/* Enable on pin transition, resychronize through reset on rising edge */
flexio_putreg32(FLEXIO2_BASE, 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(FLEXIO2_BASE, bdshot_tcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4);
/* Trigger on FXIO pin transition, Baud mode */
flexio_putreg32(FLEXIO2_BASE, 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(FLEXIO2_BASE, 1 << channel);
}
}
}
return OK;
}
int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
{
@@ -307,27 +412,65 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi
dshot_tcmp = 0x2F00 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2) - 1) & 0xFF);
bdshot_tcmp = 0x2900 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 3) & 0xFF);
/* Clock FlexIO peripheral */
imxrt_clockall_flexio1();
for (unsigned i = 0; (channel_mask != 0) && (i < DSHOT_TIMERS); i++) {
if (channel_mask & (1 << i)) {
if (timer_io_channels[i].flex_io_base == 1) {
PX4_INFO("Flexio1 channel found");
flexio1_channels = true;
/* Reset FlexIO peripheral */
flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0,
FLEXIO_CTRL_SWRST_MASK);
flexio_putreg32(0, IMXRT_FLEXIO_CTRL_OFFSET);
} else if (timer_io_channels[i].flex_io_base == 2) {
PX4_INFO("Flexio2 channel found");
flexio2_channels = true;
/* 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)));
} else {PX4_ERR("Invalid flexio base defined");}
}
}
/* FlexIO IRQ handling */
up_enable_irq(IMXRT_IRQ_FLEXIO1);
irq_attach(IMXRT_IRQ_FLEXIO1, flexio_irq_handler, 0);
if (flexio1_channels) { // FlexIO peripheral 1
/* Clock FlexIO peripheral */
imxrt_clockall_flexio1();
/* Reset FlexIO 1 peripheral */
flexio_modifyreg32(FLEXIO1_BASE, IMXRT_FLEXIO_CTRL_OFFSET, 0,
FLEXIO_CTRL_SWRST_MASK);
flexio_putreg32(FLEXIO1_BASE, 0, IMXRT_FLEXIO_CTRL_OFFSET);
/* Initialize FlexIO 1 peripheral */
flexio_modifyreg32(FLEXIO1_BASE, 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, flexio1_irq_handler, 0);
}
if (flexio2_channels) { // FlexIO peripheral 2
/* Clock FlexIO peripheral */
imxrt_clockall_flexio2();
/* Reset FlexIO 2 peripheral */
flexio_modifyreg32(FLEXIO2_BASE, IMXRT_FLEXIO_CTRL_OFFSET, 0,
FLEXIO_CTRL_SWRST_MASK);
flexio_putreg32(FLEXIO2_BASE, 0, IMXRT_FLEXIO_CTRL_OFFSET);
/* Initialize FlexIO 2 peripheral */
flexio_modifyreg32(FLEXIO2_BASE, 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)));
up_enable_irq(IMXRT_IRQ_FLEXIO2);
irq_attach(IMXRT_IRQ_FLEXIO2, flexio2_irq_handler, 0);
}
dshot_mask = 0x0;
@@ -342,7 +485,16 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi
dshot_inst[channel].bdshot = enable_bidirectional_dshot;
flexio_dshot_output(channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot);
if (timer_io_channels[channel].flex_io_base == 1) {
flexio_dshot_output(FLEXIO1_BASE, channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp,
dshot_inst[channel].bdshot);
} else if (timer_io_channels[channel].flex_io_base == 2) {
flexio_dshot_output(FLEXIO2_BASE, channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp,
dshot_inst[channel].bdshot);
} else {PX4_ERR("Invalid or undefined flexio bases in timer config");}
dshot_inst[channel].init = true;
@@ -351,8 +503,16 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi
}
}
flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0,
FLEXIO_CTRL_FLEXEN_MASK);
if (flexio1_channels) {
flexio_modifyreg32(FLEXIO1_BASE, IMXRT_FLEXIO_CTRL_OFFSET, 0,
FLEXIO_CTRL_FLEXEN_MASK);
}
if (flexio2_channels) {
flexio_modifyreg32(FLEXIO2_BASE, IMXRT_FLEXIO_CTRL_OFFSET, 0,
FLEXIO_CTRL_FLEXEN_MASK);
}
return channel_mask;
}
@@ -417,7 +577,6 @@ void up_bdshot_erpm(void)
}
}
int up_bdshot_num_erpm_ready(void)
{
int num_ready = 0;
@@ -475,7 +634,14 @@ void up_dshot_trigger(void)
up_bdshot_erpm();
}
clear_timer_status_flags(0xFF);
if (flexio1_channels) {
clear_timer_status_flags(FLEXIO1_BASE, 0xFF);
}
if (flexio2_channels) {
clear_timer_status_flags(FLEXIO2_BASE, 0xFF);
}
for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) {
if (dshot_inst[channel].bdshot && (bdshot_recv_mask & (1 << channel)) == 0) {
@@ -483,15 +649,31 @@ void up_dshot_trigger(void)
}
if (dshot_inst[channel].init && dshot_inst[channel].data_seg1 != 0) {
flexio_putreg32(dshot_inst[channel].data_seg1, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4);
if (timer_io_channels[channel].flex_io_base == 1) {
flexio_putreg32(FLEXIO1_BASE, dshot_inst[channel].data_seg1, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4);
} else if (timer_io_channels[channel].flex_io_base == 2) {
flexio_putreg32(FLEXIO2_BASE, dshot_inst[channel].data_seg1, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4);
}
}
}
bdshot_recv_mask = 0x0;
clear_timer_status_flags(0xFF);
enable_shifter_status_interrupts(0xFF);
enable_timer_status_interrupts(0xFF);
if (flexio1_channels) {
clear_timer_status_flags(FLEXIO1_BASE, 0xFF);
enable_shifter_status_interrupts(FLEXIO1_BASE, 0xFF);
enable_timer_status_interrupts(FLEXIO1_BASE, 0xFF);
}
if (flexio2_channels) {
clear_timer_status_flags(FLEXIO2_BASE, 0xFF);
enable_shifter_status_interrupts(FLEXIO2_BASE, 0xFF);
enable_timer_status_interrupts(FLEXIO2_BASE, 0xFF);
}
}
/* Expand packet from 16 bits 48 to get T0H and T1H timing */
@@ -522,6 +704,7 @@ uint64_t dshot_expand_data(uint16_t packet)
**/
void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
{
if (channel < DSHOT_TIMERS && dshot_inst[channel].init) {
uint16_t csum_data;
uint16_t packet = 0;
@@ -555,12 +738,26 @@ void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
if (dshot_inst[channel].bdshot) {
flexio_putreg32(0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
disable_shifter_status_interrupts(1 << channel);
if (timer_io_channels[channel].flex_io_base == 1) {
flexio_putreg32(FLEXIO1_BASE, 0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
disable_shifter_status_interrupts(FLEXIO1_BASE, 1 << channel);
flexio_dshot_output(channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot);
flexio_dshot_output(FLEXIO1_BASE, channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp,
dshot_inst[channel].bdshot);
clear_timer_status_flags(FLEXIO1_BASE, 0xFF);
} else if (timer_io_channels[channel].flex_io_base == 2) {
flexio_putreg32(FLEXIO2_BASE, 0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4);
disable_shifter_status_interrupts(FLEXIO2_BASE, 1 << channel);
flexio_dshot_output(FLEXIO2_BASE, channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp,
dshot_inst[channel].bdshot);
clear_timer_status_flags(FLEXIO2_BASE, 0xFF);
}
clear_timer_status_flags(0xFF);
}
}
}
platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h
+2 -1
View File
@@ -111,7 +111,8 @@ typedef struct timer_io_channels_t {
uint8_t val_offset; /* IMXRT_FLEXPWM_SM0VAL3_OFFSET or IMXRT_FLEXPWM_SM0VAL5_OFFSET */
uint8_t sub_module; /* 0 based sub module offset */
uint8_t sub_module_bits; /* LDOK and CLDOK bits */
uint8_t timer_channel; /* Unused */
uint8_t timer_channel;/* Unused */
uint8_t flex_io_base;
dshot_conf_t dshot;
} timer_io_channels_t;
platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer_hw_description.h
+2 -3
View File
@@ -600,14 +600,13 @@ 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)
PWM::FlexPWMConfig pwm_config, IOMUX::Pad pad, uint32_t dshot_pinmux, uint8_t flexio, 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;
ret.flex_io_base = flexio;
return ret;
}