From ce897207bd36193a4f5d6c636547eeeb22b4f894 Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Tue, 13 May 2025 21:32:16 +0200 Subject: [PATCH] Fix PWM, IMU, Update config set CRSF as default --- boards/nxp/mr-tropic/default.px4board | 8 +- boards/nxp/mr-tropic/init/rc.board_defaults | 8 ++ boards/nxp/mr-tropic/init/rc.board_sensors | 2 +- boards/nxp/mr-tropic/src/board_config.h | 18 ++-- boards/nxp/mr-tropic/src/hw_rev_ver_tropic.c | 44 +--------- boards/nxp/mr-tropic/src/i2c.cpp | 2 +- boards/nxp/mr-tropic/src/timer_config.cpp | 1 + boards/nxp/mr-tropic/src/tropic_led_pwm.cpp | 90 +++++++++++++------- 8 files changed, 85 insertions(+), 88 deletions(-) diff --git a/boards/nxp/mr-tropic/default.px4board b/boards/nxp/mr-tropic/default.px4board index af8a30fd97..8beeca5238 100644 --- a/boards/nxp/mr-tropic/default.px4board +++ b/boards/nxp/mr-tropic/default.px4board @@ -5,6 +5,7 @@ CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS3" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS5" CONFIG_BOARD_PARAM_FILE="/fs/nor/mtd_params" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y @@ -16,21 +17,24 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y CONFIG_COMMON_INS=y CONFIG_COMMON_LIGHT=y CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM350=y -CONFIG_DRIVERS_OSD_MSP_OSD=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_COMMON_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y CONFIG_DRIVERS_PWM_OUT=y -CONFIG_DRIVERS_RC_INPUT=y +CONFIG_COMMON_RC=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_COMMON_TELEMETRY=y CONFIG_DRIVERS_TONE_ALARM=y diff --git a/boards/nxp/mr-tropic/init/rc.board_defaults b/boards/nxp/mr-tropic/init/rc.board_defaults index a8c34a7802..fb21d961ee 100644 --- a/boards/nxp/mr-tropic/init/rc.board_defaults +++ b/boards/nxp/mr-tropic/init/rc.board_defaults @@ -16,6 +16,10 @@ param set-default SENS_EN_INA238 0 param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 0 +param set-default RC_CRSF_PRT_CFG 300 +param set-default RC_CRSF_TEL_EN 1 +param set-default RC_SBUS_PRT_CFG 0 + if [ -f "/fs/microsd/ipcfg-eth0" ] then else @@ -29,3 +33,7 @@ if param greater -s UAVCAN_ENABLE 0 then ifup can0 fi + +# Start the ESC telemetry +#dshot telemetry -d /dev/ttyS4 -x +#TODO WAIT FOR https://github.com/PX4/PX4-Autopilot/pull/24706 diff --git a/boards/nxp/mr-tropic/init/rc.board_sensors b/boards/nxp/mr-tropic/init/rc.board_sensors index 7e298ed604..78878a66dc 100644 --- a/boards/nxp/mr-tropic/init/rc.board_sensors +++ b/boards/nxp/mr-tropic/init/rc.board_sensors @@ -41,7 +41,7 @@ fi if [ $INA_CONFIGURED = no ] then # INA226, INA228, INA238 auto-start - i2c_launcher start -b 1 + i2c_launcher start -b 3 fi diff --git a/boards/nxp/mr-tropic/src/board_config.h b/boards/nxp/mr-tropic/src/board_config.h index 0b2391f0cc..e1bc88f0c1 100644 --- a/boards/nxp/mr-tropic/src/board_config.h +++ b/boards/nxp/mr-tropic/src/board_config.h @@ -67,9 +67,9 @@ /* An RGB LED is connected through GPIO as shown below: */ #define LED_IOMUX (IOMUX_OPENDRAIN | IOMUX_PULL_NONE | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_SLOW) -#define GPIO_nLED_RED /* GPIO_AD_B0_02 GPIO1_IO02 */ (GPIO_PORT1 | GPIO_PIN2 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LED_IOMUX) -#define GPIO_nLED_GREEN /* GPIO_B1_03 GPIO2_IO19 */ (GPIO_PORT2 | GPIO_PIN19 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LED_IOMUX) -#define GPIO_nLED_BLUE /* GPIO_AD_B0_03 GPIO1_IO03 */ (GPIO_PORT1 | GPIO_PIN3 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LED_IOMUX) +#define GPIO_nLED_RED /* GPIO_EMC_33 GPIO3_IO19 */ (GPIO_PORT3 | GPIO_PIN19 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LED_IOMUX) +#define GPIO_nLED_BLUE /* GPIO_EMC_34 GPIO3_IO20 */ (GPIO_PORT3 | GPIO_PIN20 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LED_IOMUX) +#define GPIO_nLED_GREEN /* GPIO_EMC_38 GPIO3_IO24 */ (GPIO_PORT3 | GPIO_PIN24 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LED_IOMUX) #define BOARD_HAS_CONTROL_STATUS_LEDS 1 #define BOARD_OVERLOAD_LED LED_RED @@ -149,9 +149,9 @@ #define BOARD_LED_PWM_DRIVE_ACTIVE_LOW 1 #define BOARD_HAS_CUSTOM_LED_PWM 1 -#define PWM_LED_RED /* GPIO_EMC_33 */ (GPIO_FLEXPWM3_PWMA02_1 | GENERAL_OUTPUT_IOMUX) -#define PWM_LED_GREEN /* GPIO_EMC_34 */ (GPIO_FLEXPWM3_PWMB02_1 | GENERAL_OUTPUT_IOMUX) -#define PWM_LED_BLUE /* GPIO_EMC_38 */ (GPIO_FLEXPWM1_PWMA03_2 | GENERAL_OUTPUT_IOMUX) +#define PWM_LED_RED /* GPIO_EMC_33 */ (GPIO_FLEXPWM3_PWMA02_1 | GENERAL_OUTPUT_IOMUX) +#define PWM_LED_BLUE /* GPIO_EMC_34 */ (GPIO_FLEXPWM3_PWMB02_1 | GENERAL_OUTPUT_IOMUX) +#define PWM_LED_GREEN /* GPIO_EMC_38 */ (GPIO_FLEXPWM1_PWMA03_2 | GENERAL_OUTPUT_IOMUX) /* ETHERNET GPIO */ @@ -183,9 +183,9 @@ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ #define RC_SERIAL_PORT "/dev/ttyS5" -#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 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 diff --git a/boards/nxp/mr-tropic/src/hw_rev_ver_tropic.c b/boards/nxp/mr-tropic/src/hw_rev_ver_tropic.c index ca21b68b31..7414c21a32 100644 --- a/boards/nxp/mr-tropic/src/hw_rev_ver_tropic.c +++ b/boards/nxp/mr-tropic/src/hw_rev_ver_tropic.c @@ -50,15 +50,8 @@ #if defined(BOARD_HAS_HW_VERSIONING) -#define DIR_READ 0x80 -#define WHO_AMI_I 0x75 -#define REG_BANK_SEL 0x76 - #define HW_INFO_SIZE HW_INFO_VER_DIGITS + HW_INFO_REV_DIGITS -#define DEFAULT_SPI ((1 << 28) | DRV_IMU_DEVTYPE_ICM45686) - - /**************************************************************************** * Private Data ****************************************************************************/ @@ -163,42 +156,7 @@ __EXPORT int board_get_hw_revision() int board_determine_hw_info() { - struct spi_dev_s *spi = px4_spibus_initialize(3); - - SPI_SETFREQUENCY(spi, 8 * 1000 * 1000); - SPI_SETMODE(spi, SPIDEV_MODE3); - SPI_SETBITS(spi, 8); - - /* ICM Select bank 0 */ - SPI_SELECT(spi, DEFAULT_SPI, true); - uint8_t cmd_bank_sel[2] = {0}; - cmd_bank_sel[0] = REG_BANK_SEL; - cmd_bank_sel[1] = 0; - SPI_EXCHANGE(spi, cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel)); - SPI_SELECT(spi, DEFAULT_SPI, false); - - /* ICM Read WHOAM */ - SPI_SELECT(spi, DEFAULT_SPI, true); - uint8_t cmd[2] = {0}; - cmd[0] = (WHO_AMI_I) | DIR_READ; - SPI_EXCHANGE(spi, cmd, cmd, sizeof(cmd_bank_sel)); - SPI_SELECT(spi, DEFAULT_SPI, false); - - // Default is TROPIC_0 icm45686 - - if (cmd[1] == 0x47) { // TROPIC_1 icm42688p - hw_revision = 1; - - } else if (cmd[1] == 0x44) { // TROPIC_2 icm42686p - hw_revision = 2; - } - - /* Default bus 1 to 8MHz and de-assert the known chip selects. - */ - - SPI_SETFREQUENCY(spi, 8 * 1000 * 1000); - SPI_SETBITS(spi, 8); - SPI_SETMODE(spi, SPIDEV_MODE3); + hw_revision = 0; //TODO Read fuses sprintf(hw_info, "%03d", hw_revision); diff --git a/boards/nxp/mr-tropic/src/i2c.cpp b/boards/nxp/mr-tropic/src/i2c.cpp index 32378a4705..a118a69d0f 100644 --- a/boards/nxp/mr-tropic/src/i2c.cpp +++ b/boards/nxp/mr-tropic/src/i2c.cpp @@ -35,6 +35,6 @@ constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { initI2CBusExternal(1), - initI2CBusInternal(3), + initI2CBusExternal(3), initI2CBusInternal(4), }; diff --git a/boards/nxp/mr-tropic/src/timer_config.cpp b/boards/nxp/mr-tropic/src/timer_config.cpp index 6752668701..a15a7e83fb 100644 --- a/boards/nxp/mr-tropic/src/timer_config.cpp +++ b/boards/nxp/mr-tropic/src/timer_config.cpp @@ -148,4 +148,5 @@ void fmurt1062_timer_initialize(void) /* QTIMER3_TIMER0 -> Flexpwm234ExtClk */ imxrt_xbar_connect(IMXRT_XBARA1_OUT_FLEXPWM234_EXT_CLK_SEL_OFFSET, IMXRT_XBARA1_IN_QTIMER3_TMR0_OUT); + imxrt_xbar_connect(IMXRT_XBARA1_OUT_FLEXPWM1_EXT_CLK_SEL_OFFSET, IMXRT_XBARA1_IN_QTIMER3_TMR0_OUT); } diff --git a/boards/nxp/mr-tropic/src/tropic_led_pwm.cpp b/boards/nxp/mr-tropic/src/tropic_led_pwm.cpp index fc1c562d5a..45598fc787 100644 --- a/boards/nxp/mr-tropic/src/tropic_led_pwm.cpp +++ b/boards/nxp/mr-tropic/src/tropic_led_pwm.cpp @@ -73,7 +73,7 @@ /* Register accessors */ #define _REG(_addr) (*(volatile uint16_t *)(_addr)) #define _REG16(_base, _reg) (*(volatile uint16_t *)(_base + _reg)) -#define FLEXPWMREG(_tmr, _sm, _reg) _REG16(FLEXPWM_TIMER_BASE + ((_sm) * SM_SPACING), (_reg)) +#define FLEXPWMREG(_tmr, _sm, _reg) _REG16(_tmr + ((_sm) * SM_SPACING), (_reg)) /* FlexPWM Registers for LED_G */ #define rINIT(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0INIT_OFFSET) /* Initial Count Register */ @@ -93,64 +93,90 @@ #define rDTSRCSEL(_tim) FLEXPWMREG(_tim, 0, IMXRT_FLEXPWM_DTSRCSEL_OFFSET) /* PWM Source Select Register */ #define rMCTRL(_tim) FLEXPWMREG(_tim, 0, IMXRT_FLEXPWM_MCTRL_OFFSET) /* Master Control Register */ -#define FLEXPWM_TIMER 2 -#define FLEXPWM_SM 3 +#define OUTEN_A_MASK 0x1 +#define OUTEN_B_MASK 0x2 #define FREQ -static void flexpwm_led_green(uint16_t cvalue) +static void flexpwm_led(uint32_t timer, uint32_t sm, uint16_t cvalue, uint32_t gpio_mux, uint32_t pwm_mux, + uint32_t out_mask) { if (cvalue == 0) { - rMCTRL(FLEXPWM_TIMER) &= ~MCTRL_RUN(1 << FLEXPWM_SM); - px4_arch_configgpio(GPIO_nLED_GREEN); + //rMCTRL(timer) &= ~MCTRL_RUN(1 << sm); + px4_arch_configgpio(gpio_mux); } else { - rMCTRL(FLEXPWM_TIMER) |= (1 << (FLEXPWM_SM + MCTRL_CLDOK_SHIFT)); - rVAL1(FLEXPWM_TIMER, FLEXPWM_SM) = (FLEXPWM_FREQ / LED_PWM_FREQ) - 1; - rVAL5(FLEXPWM_TIMER, FLEXPWM_SM) = (FLEXPWM_FREQ / LED_PWM_FREQ) - (cvalue * 3); - rMCTRL(FLEXPWM_TIMER) |= MCTRL_LDOK(1 << FLEXPWM_SM) | MCTRL_RUN(1 << FLEXPWM_SM); - px4_arch_configgpio(PWM_LED_GREEN); + rMCTRL(timer) |= (1 << (sm + MCTRL_CLDOK_SHIFT)); + rVAL1(timer, sm) = (FLEXPWM_FREQ / LED_PWM_FREQ) - 1; + + if (out_mask & OUTEN_A_MASK) { + rVAL3(timer, sm) = (FLEXPWM_FREQ / LED_PWM_FREQ) - (cvalue); + + } else if (out_mask & OUTEN_B_MASK) { + rVAL5(timer, sm) = (FLEXPWM_FREQ / LED_PWM_FREQ) - (cvalue); + } + + rMCTRL(timer) |= MCTRL_LDOK(1 << sm) | MCTRL_RUN(1 << sm); + px4_arch_configgpio(pwm_mux); } } int led_pwm_servo_set(unsigned channel, uint8_t cvalue) { - if (channel == 2) { + if (channel == 0) { + flexpwm_led(IMXRT_FLEXPWM3_BASE, 2, cvalue, GPIO_nLED_RED, PWM_LED_RED, OUTEN_A_MASK); } else if (channel == 1) { - flexpwm_led_green(cvalue); + flexpwm_led(IMXRT_FLEXPWM1_BASE, 3, cvalue, GPIO_nLED_GREEN, PWM_LED_GREEN, OUTEN_A_MASK); - } else if (channel == 0) { + } else if (channel == 2) { + flexpwm_led(IMXRT_FLEXPWM3_BASE, 2, cvalue * 3, GPIO_nLED_BLUE, PWM_LED_BLUE, OUTEN_B_MASK); } return 0; } +void flexpwm_init(uint32_t timer, uint32_t sm, uint32_t out_mask) +{ + /* Clear all Faults */ + rFSTS0(timer) = FSTS_FFLAG_MASK; + rMCTRL(timer) |= (1 << (sm + MCTRL_CLDOK_SHIFT)); + + rCTRL2(timer, sm) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP; + rCTRL(timer, sm) = SMCTRL_PRSC_DIV16 | SMCTRL_FULL; + /* Edge aligned at 0 */ + rINIT(timer, sm) = 0; + rVAL0(timer, sm) = 0; + rVAL2(timer, sm) = 0; + rVAL4(timer, sm) = 0; + rFFILT0(timer) &= ~FFILT_FILT_PER_MASK; + rDISMAP0(timer, sm) = 0xf000; + rDISMAP1(timer, sm) = 0xf000; + + + if (out_mask & OUTEN_A_MASK) { + rOUTEN(timer) |= OUTEN_PWMA_EN(1 << sm); + } + + if (out_mask & OUTEN_B_MASK) { + rOUTEN(timer) |= OUTEN_PWMB_EN(1 << sm); + } + + rDTSRCSEL(timer) = 0; + rMCTRL(timer) |= MCTRL_LDOK(1 << sm); +} + int led_pwm_servo_init() { /* PWM_LED_GREEN - FLEXPWM2_PWMB03 */ - imxrt_clockall_pwm2(); + imxrt_clockall_pwm1(); + imxrt_clockall_pwm3(); - /* Clear all Faults */ - rFSTS0(FLEXPWM_TIMER) = FSTS_FFLAG_MASK; - rMCTRL(FLEXPWM_TIMER) |= (1 << (FLEXPWM_SM + MCTRL_CLDOK_SHIFT)); + flexpwm_init(IMXRT_FLEXPWM3_BASE, 2, OUTEN_A_MASK | OUTEN_B_MASK); // GPIO_FLEXPWM3_PWMA02_1 + flexpwm_init(IMXRT_FLEXPWM1_BASE, 3, OUTEN_A_MASK); // GPIO_FLEXPWM1_PWMA03_2 - rCTRL2(FLEXPWM_TIMER, FLEXPWM_SM) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP; - rCTRL(FLEXPWM_TIMER, FLEXPWM_SM) = SMCTRL_PRSC_DIV16 | SMCTRL_FULL; - /* Edge aligned at 0 */ - rINIT(FLEXPWM_TIMER, FLEXPWM_SM) = 0; - rVAL0(FLEXPWM_TIMER, FLEXPWM_SM) = 0; - rVAL2(FLEXPWM_TIMER, FLEXPWM_SM) = 0; - rVAL4(FLEXPWM_TIMER, FLEXPWM_SM) = 0; - rFFILT0(FLEXPWM_TIMER) &= ~FFILT_FILT_PER_MASK; - rDISMAP0(FLEXPWM_TIMER, FLEXPWM_SM) = 0xf000; - rDISMAP1(FLEXPWM_TIMER, FLEXPWM_SM) = 0xf000; - rOUTEN(FLEXPWM_TIMER) |= OUTEN_PWMB_EN(1 << FLEXPWM_SM); - - rDTSRCSEL(FLEXPWM_TIMER) = 0; - rMCTRL(FLEXPWM_TIMER) |= MCTRL_LDOK(1 << FLEXPWM_SM); return OK; }