fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 04:43:33 +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
boards/nxp/tropic-community/src/board_config.h
+36 -24
View File
@@ -98,9 +98,9 @@
#define GPIO_SPI1_MISO_OFF _PIN_OFF(_GPIO_LPSPI1_MISO)
#define GPIO_SPI1_MOSI_OFF _PIN_OFF(_GPIO_LPSPI1_MOSI)
#define _GPIO_LPSPI3_SCK /* GPIO_AD_B1_15 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN31 | CS_IOMUX)
#define _GPIO_LPSPI3_MISO /* GPIO_AD_B1_13 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN29 | CS_IOMUX)
#define _GPIO_LPSPI3_MOSI /* GPIO_AD_B1_14 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN30 | CS_IOMUX)
#define _GPIO_LPSPI3_SCK /* GPIO_AD_B0_00 GPIO1_IO0 */ (GPIO_PORT1 | GPIO_PIN0 | CS_IOMUX)
#define _GPIO_LPSPI3_MISO /* GPIO_AD_B1_13 GPIO1_IO29 */ (GPIO_PORT1 | GPIO_PIN29 | CS_IOMUX)
#define _GPIO_LPSPI3_MOSI /* GPIO_AD_B1_14 GPIO1_IO30 */ (GPIO_PORT1 | GPIO_PIN30 | CS_IOMUX)
#define GPIO_SPI3_SCK_OFF _PIN_OFF(_GPIO_LPSPI3_SCK)
#define GPIO_SPI3_MISO_OFF _PIN_OFF(_GPIO_LPSPI3_MISO)
@@ -112,7 +112,6 @@
#define GPIO_DRDY_OFF_SPI4_DRDY7_EXTERNAL1 _PIN_OFF(GPIO_SPI4_DRDY7_EXTERNAL1)
#define ADC_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_HIZ)
#define ADC1_CH(n) (n)
@@ -121,13 +120,13 @@
/* Define GPIO pins used as ADC N.B. Channel numbers are for reference, */
#define PX4_ADC_GPIO \
/* BATTERY1_VOLTAGE GPIO_AD_B1_08 GPIO1 Pin 27 */ ADC1_GPIO(0, 27), \
/* BATTERY1_CURRENT GPIO_AD_B1_11 GPIO1 Pin 24 */ ADC1_GPIO(0, 24)
/* BATTERY1_VOLTAGE GPIO_AD_B1_14 GPIO1 Pin 30 */ ADC1_GPIO(0, 30), \
/* BATTERY1_CURRENT GPIO_AD_B1_08 GPIO1 Pin 27 */ ADC1_GPIO(0, 27)
/* Define Channel numbers must match above GPIO pin IN(n)*/
#define ADC_BATTERY_VOLTAGE_CHANNEL /* GPIO_AD_B1_08 GPIO1 Pin 27 */ ADC1_CH(13)
#define ADC_BATTERY_CURRENT_CHANNEL /* GPIO_AD_B1_11 GPIO1 Pin 12 */ ADC1_CH(0)
#define ADC_BATTERY_VOLTAGE_CHANNEL /* GPIO_AD_B1_14 GPIO1 Pin 30 */ ADC1_CH(3)
#define ADC_BATTERY_CURRENT_CHANNEL /* GPIO_AD_B1_08 GPIO1 Pin 27 */ ADC1_CH(13)
#define ADC_CHANNELS \
((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \
@@ -142,7 +141,6 @@
#define GENERAL_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_UP_47K | IOMUX_DRIVE_HIZ)
#define GENERAL_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST)
/* PWM
*/
@@ -162,8 +160,6 @@
#define PWM_LED_GREEN /* GPIO_B1_03 */ (GPIO_FLEXPWM2_PWMB03_4 | GENERAL_OUTPUT_IOMUX)
#define PWM_LED_BLUE /* GPIO_AD_B0_03 */ (GPIO_XBAR1_INOUT17_1 | GENERAL_OUTPUT_IOMUX)
/* Tone alarm output */
//FIXME FlexPWM TONE DRIVER
@@ -185,12 +181,6 @@
#define HRT_TIMER 1 /* use GPT1 for the HRT */
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */
#define RC_SERIAL_PORT "/dev/ttyS4"
#define RC_SERIAL_SINGLEWIRE 1 // Suport Single wire wiring
#define RC_SERIAL_SWAP_RXTX 1 // Set Swap (but not supported in HW) to use Single wire
#define RC_SERIAL_SWAP_USING_SINGLEWIRE 1 // Set to use Single wire swap as HW does not support swap
#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT
/* Safety Switch is HW version dependent on having an PX4IO
* So we init to a benign state with the _INIT definition
* and provide the the non _INIT one for the driver to make a run time
@@ -234,9 +224,6 @@ static inline int board_read_usb2_vbus_state(void)
#define BOARD_ADC_BRICK_VALID (board_read_usb2_vbus_state() == 0)
#define BOARD_ADC_USB_CONNECTED (board_read_VBUS_state() == 0)
/* FMUv5 never powers odd the Servo rail */
@@ -255,13 +242,34 @@ static inline int board_read_usb2_vbus_state(void)
#define PX4_GPIO_INIT_LIST { \
PX4_ADC_GPIO, \
GPIO_FLEXCAN3_TX, \
GPIO_FLEXCAN3_RX, \
GPIO_TONE_ALARM_IDLE, \
GPIO_SAFETY_SWITCH_IN \
GPIO_MUX_SEL_PAY1, \
GPIO_MUX_SEL_PAY2, \
GPIO_MUX_SEL_PAY3, \
GPIO_MUX_SEL_PAY4, \
GPIO_ACC1_PWR, \
GPIO_ACC2_PWR, \
GPIO_MUX_SEL_FORE, \
GPIO_MUX_SEL_AFT, \
GPIO_MUX_SEL_M1, \
GPIO_MUX_SEL_M2, \
GPIO_MUX_SEL_IMU, \
GPIO_INT_I2CEXP, \
GPIO_EXT_I2CEXP, \
}
#define BOARD_ENABLE_CONSOLE_BUFFER
/* To detect IMU */
#define BOARD_HAS_HW_VERSIONING 1
#define BOARD_HAS_HW_SPLIT_VERSIONING 1
#define HW_INFO_INIT_PREFIX "TROPIC"
#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3
// Base/FMUM
#define TROPIC_0 HW_FMUM_ID(0x0) // ICM42688
#define TROPIC_1 HW_FMUM_ID(0x1) // ICM42688
#define TROPIC_2 HW_FMUM_ID(0x2) // ICM45686
__BEGIN_DECLS
/****************************************************************************************************
@@ -316,6 +324,8 @@ extern int imxrt1062_spi_bus_initialize(void);
*
************************************************************************************/
extern void imxrt_spiinitialize(void);
extern int imxrt_usb_initialize(void);
extern void imxrt_usbinitialize(void);
@@ -326,6 +336,8 @@ extern void fmurt1062_timer_initialize(void);
#include <px4_platform_common/board_common.h>
void imxrt_flash_romapi_initialize(void);
int imxrt_flexspi_storage_initialize(void);
#endif /* __ASSEMBLY__ */
boards/nxp/tropic-community/src/timer_config.cpp
+10 -7
View File
@@ -77,19 +77,22 @@
constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule0), // PWM_1, PMW_5
initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule1), // PWM_0
initIOPWM(PWM::FlexPWM2, PWM::Submodule2), // PWM_4
initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule2), // PWM_4
initIOPWMDshot(PWM::FlexPWM4, PWM::Submodule2), // PWM_2, PWM_3
};
#define FXIO_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER)
//TODO: distinguish between the different FlexIO instances
//
constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_06, GPIO_FLEXIO1_FLEXIO06_1 | FXIO_IOMUX, 6), /* RevA. PWM_1 RevB. PWM1 */
initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_B, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_07, GPIO_FLEXIO1_FLEXIO07_1 | FXIO_IOMUX, 7), /* RevA. PWM_5 RevB. PWM2 */
initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_08, GPIO_FLEXIO1_FLEXIO08_1 | FXIO_IOMUX, 8), /* RevA. PWM_0 RevB. PWM3 */
initIOTimerChannel(io_timers, {PWM::PWM2_PWM_B, PWM::Submodule2}, IOMUX::Pad::GPIO_B0_11), /* RevA. PWM_4 RevB. PWM4 */
initIOTimerChannelDshot(io_timers, {PWM::PWM4_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_04, GPIO_FLEXIO1_FLEXIO04_1 | FXIO_IOMUX, 4), /* RevA. PWM_3 RevB. PWM5 */
initIOTimerChannelDshot(io_timers, {PWM::PWM4_PWM_B, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_05, GPIO_FLEXIO1_FLEXIO05_1 | FXIO_IOMUX, 5), /* RevA. PWM_2 RevB. PWM6 */
initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_06, GPIO_FLEXIO1_FLEXIO06_1 | FXIO_IOMUX, 1, 6), /* RevA. PWM_1 RevB. PWM1 */
initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_B, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_07, GPIO_FLEXIO1_FLEXIO07_1 | FXIO_IOMUX, 1, 7), /* RevA. PWM_5 RevB. PWM2 */
initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_08, GPIO_FLEXIO1_FLEXIO08_1 | FXIO_IOMUX, 1, 8), /* RevA. PWM_0 RevB. PWM3 */
initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_B, PWM::Submodule2}, IOMUX::Pad::GPIO_B0_11, GPIO_FLEXIO2_FLEXIO11_1 | FXIO_IOMUX, 2, 11), /* RevA. PWM_4 RevB. PWM4 */
initIOTimerChannelDshot(io_timers, {PWM::PWM4_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_04, GPIO_FLEXIO1_FLEXIO04_1 | FXIO_IOMUX, 1, 4), /* RevA. PWM_3 RevB. PWM5 */
initIOTimerChannelDshot(io_timers, {PWM::PWM4_PWM_B, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_05, GPIO_FLEXIO1_FLEXIO05_1 | FXIO_IOMUX, 1, 5), /* RevA. PWM_2 RevB. PWM6 */
};
constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
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;
}