diff --git a/boards/nxp/rddrone-uavcan146/default.cmake b/boards/nxp/rddrone-uavcan146/default.cmake index 96ef52a747..7f4e27c17c 100644 --- a/boards/nxp/rddrone-uavcan146/default.cmake +++ b/boards/nxp/rddrone-uavcan146/default.cmake @@ -37,6 +37,7 @@ px4_add_board( gps #imu # all available imu drivers #lights + lights/rgbled_pwm #magnetometer # all available magnetometer drivers #optical_flow # all available optical flow drivers pwm_out @@ -55,7 +56,7 @@ px4_add_board( #esc_calib #hardfault_log i2cdetect - #led_control + led_control mixer #motor_ramp #motor_test diff --git a/boards/nxp/rddrone-uavcan146/src/board_config.h b/boards/nxp/rddrone-uavcan146/src/board_config.h index 8cca199283..c05306011f 100644 --- a/boards/nxp/rddrone-uavcan146/src/board_config.h +++ b/boards/nxp/rddrone-uavcan146/src/board_config.h @@ -105,17 +105,18 @@ __BEGIN_DECLS * Pins: * Defined in board.h */ -// todo:Design this! #define DIRECT_PWM_OUTPUT_CHANNELS 1 #define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS #define BOARD_HAS_LED_PWM 1 +#define BOARD_LED_PWM_DRIVE_ACTIVE_LOW 1 +#define BOARD_HAS_SHARED_PWM_TIMERS 1 -#define LED_TIM3_CH1OUT /* PTD1 RGB_R */ PIN_FTM3_CH1_1 -#define LED_TIM3_CH5OUT /* PTC9 RGB_G */ PIN_FTM3_CH5_1 -#define LED_TIM3_CH4OUT /* PTC8 RGB_B */ PIN_FTM3_CH4_1 +#define LED_TIM0_CH0OUT /* PTD15 RGB_R */ PIN_FTM0_CH0_3 +#define LED_TIM0_CH1OUT /* PTD16 RGB_G */ PIN_FTM0_CH1_3 +#define LED_TIM0_CH2OUT /* PTD0 RGB_B */ PIN_FTM0_CH2_3 /**************************************************************************** * Public Types diff --git a/boards/nxp/rddrone-uavcan146/src/timer_config.cpp b/boards/nxp/rddrone-uavcan146/src/timer_config.cpp index d6c5a8af15..436b958787 100644 --- a/boards/nxp/rddrone-uavcan146/src/timer_config.cpp +++ b/boards/nxp/rddrone-uavcan146/src/timer_config.cpp @@ -118,39 +118,33 @@ constexpr io_timers_channel_mapping_t io_timers_channel_mapping = initIOTimerChannelMapping(io_timers, timer_io_channels); const struct io_timers_t led_pwm_timers[MAX_LED_TIMERS] = { - // To Do: Remove or add the right definitions. - /* { .base = S32K1XX_FTM0_BASE, .clock_register = S32K1XX_PCC_FTM0, .clock_bit = PCC_CGC, //.vectorno = 0, }, - */ }; const struct timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS] = { - // To Do: Remove or add the right definitions. - /* { - .gpio_out = LED_TIM3_CH1OUT, // RGB_R + .gpio_out = LED_TIM0_CH0OUT, // RGB_R + .gpio_in = 0, + .timer_index = 0, + .timer_channel = 1, + }, + { + .gpio_out = LED_TIM0_CH1OUT, // RGB_G .gpio_in = 0, .timer_index = 0, .timer_channel = 2, }, { - .gpio_out = LED_TIM3_CH5OUT, // RGB_G + .gpio_out = LED_TIM0_CH2OUT, // RGB_B .gpio_in = 0, .timer_index = 0, - .timer_channel = 6, + .timer_channel = 3, }, - { - .gpio_out = LED_TIM3_CH4OUT, // RGB_B - .gpio_in = 0, - .timer_index = 0, - .timer_channel = 5, - }, - */ }; void ucans32k_timer_initialize(void) diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c index aa00400657..1dffff74cb 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/io_pins/io_timer.c @@ -846,13 +846,13 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann action_cache[timer].base = io_timers[timer].base; action_cache[timer].cnsc[action_cache[timer].index].cnsc_offset = io_timers[timer].base + S32K1XX_FTM_CNSC_OFFSET(chan); action_cache[timer].cnsc[action_cache[timer].index].cnsc_value = bits; - action_cache[timer].mask |= 1 << chan; if ((state && (mode == IOTimerChanMode_PWMOut || mode == IOTimerChanMode_OneShot || mode == IOTimerChanMode_Trigger))) { action_cache[timer].cnsc[action_cache[timer].index].gpio = timer_io_channels[chan_index].gpio_out; + action_cache[timer].mask |= 1 << chan; } action_cache[timer].index++; @@ -891,7 +891,6 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann /* arm requires the timer be enabled */ regval |= (FTM_SC_CLKS_EXTCLK); - regval &= ~FTM_SC_PWMEN_MASK; regval |= action_cache[actions].mask << FTM_SC_PWMEN_SHIFT; } diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp index e59021d556..8a7e69a526 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/led_pwm/led_pwm.cpp @@ -58,13 +58,13 @@ #include -#include -#include "hardware/kinetis_sim.h" -#include "hardware/kinetis_ftm.h" +#include "s32k1xx_pin.h" +#include "hardware/s32k1xx_pcc.h" +#include "hardware/s32k1xx_ftm.h" #if defined(BOARD_HAS_LED_PWM) || defined(BOARD_HAS_UI_LED_PWM) -#define FTM_SRC_CLOCK_FREQ 16000000 +#define FTM_SRC_CLOCK_FREQ 8000000 #define LED_PWM_FREQ 1000000 #if (BOARD_LED_PWM_RATE) @@ -80,54 +80,54 @@ /* Timer register accessors */ -#define rSC(_tmr) REG(_tmr,KINETIS_FTM_SC_OFFSET) -#define rCNT(_tmr) REG(_tmr,KINETIS_FTM_CNT_OFFSET) -#define rMOD(_tmr) REG(_tmr,KINETIS_FTM_MOD_OFFSET) -#define rC0SC(_tmr) REG(_tmr,KINETIS_FTM_C0SC_OFFSET) -#define rC0V(_tmr) REG(_tmr,KINETIS_FTM_C0V_OFFSET) -#define rC1SC(_tmr) REG(_tmr,KINETIS_FTM_C1SC_OFFSET) -#define rC1V(_tmr) REG(_tmr,KINETIS_FTM_C1V_OFFSET) -#define rC2SC(_tmr) REG(_tmr,KINETIS_FTM_C2SC_OFFSET) -#define rC2V(_tmr) REG(_tmr,KINETIS_FTM_C2V_OFFSET) -#define rC3SC(_tmr) REG(_tmr,KINETIS_FTM_C3SC_OFFSET) -#define rC3V(_tmr) REG(_tmr,KINETIS_FTM_C3V_OFFSET) -#define rC4SC(_tmr) REG(_tmr,KINETIS_FTM_C4SC_OFFSET) -#define rC4V(_tmr) REG(_tmr,KINETIS_FTM_C4V_OFFSET) -#define rC5SC(_tmr) REG(_tmr,KINETIS_FTM_C5SC_OFFSET) -#define rC5V(_tmr) REG(_tmr,KINETIS_FTM_C5V_OFFSET) -#define rC6SC(_tmr) REG(_tmr,KINETIS_FTM_C6SC_OFFSET) -#define rC6V(_tmr) REG(_tmr,KINETIS_FTM_C6V_OFFSET) -#define rC7SC(_tmr) REG(_tmr,KINETIS_FTM_C7SC_OFFSET) -#define rC7V(_tmr) REG(_tmr,KINETIS_FTM_C7V_OFFSET) +#define rSC(_tmr) REG(_tmr, S32K1XX_FTM_SC_OFFSET) +#define rCNT(_tmr) REG(_tmr, S32K1XX_FTM_CNT_OFFSET) +#define rMOD(_tmr) REG(_tmr, S32K1XX_FTM_MOD_OFFSET) +#define rC0SC(_tmr) REG(_tmr, S32K1XX_FTM_C0SC_OFFSET) +#define rC0V(_tmr) REG(_tmr, S32K1XX_FTM_C0V_OFFSET) +#define rC1SC(_tmr) REG(_tmr, S32K1XX_FTM_C1SC_OFFSET) +#define rC1V(_tmr) REG(_tmr, S32K1XX_FTM_C1V_OFFSET) +#define rC2SC(_tmr) REG(_tmr, S32K1XX_FTM_C2SC_OFFSET) +#define rC2V(_tmr) REG(_tmr, S32K1XX_FTM_C2V_OFFSET) +#define rC3SC(_tmr) REG(_tmr, S32K1XX_FTM_C3SC_OFFSET) +#define rC3V(_tmr) REG(_tmr, S32K1XX_FTM_C3V_OFFSET) +#define rC4SC(_tmr) REG(_tmr, S32K1XX_FTM_C4SC_OFFSET) +#define rC4V(_tmr) REG(_tmr, S32K1XX_FTM_C4V_OFFSET) +#define rC5SC(_tmr) REG(_tmr, S32K1XX_FTM_C5SC_OFFSET) +#define rC5V(_tmr) REG(_tmr, S32K1XX_FTM_C5V_OFFSET) +#define rC6SC(_tmr) REG(_tmr, S32K1XX_FTM_C6SC_OFFSET) +#define rC6V(_tmr) REG(_tmr, S32K1XX_FTM_C6V_OFFSET) +#define rC7SC(_tmr) REG(_tmr, S32K1XX_FTM_C7SC_OFFSET) +#define rC7V(_tmr) REG(_tmr, S32K1XX_FTM_C7V_OFFSET) -#define rCNTIN(_tmr) REG(_tmr,KINETIS_FTM_CNTIN_OFFSET) -#define rSTATUS(_tmr) REG(_tmr,KINETIS_FTM_STATUS_OFFSET) -#define rMODE(_tmr) REG(_tmr,KINETIS_FTM_MODE_OFFSET) -#define rSYNC(_tmr) REG(_tmr,KINETIS_FTM_SYNC_OFFSET) -#define rOUTINIT(_tmr) REG(_tmr,KINETIS_FTM_OUTINIT_OFFSET) -#define rOUTMASK(_tmr) REG(_tmr,KINETIS_FTM_OUTMASK_OFFSET) -#define rCOMBINE(_tmr) REG(_tmr,KINETIS_FTM_COMBINE_OFFSET) -#define rDEADTIME(_tmr) REG(_tmr,KINETIS_FTM_DEADTIME_OFFSET) -#define rEXTTRIG(_tmr) REG(_tmr,KINETIS_FTM_EXTTRIG_OFFSET) -#define rPOL(_tmr) REG(_tmr,KINETIS_FTM_POL_OFFSET) -#define rFMS(_tmr) REG(_tmr,KINETIS_FTM_FMS_OFFSET) -#define rFILTER(_tmr) REG(_tmr,KINETIS_FTM_FILTER_OFFSET) -#define rFLTCTRL(_tmr) REG(_tmr,KINETIS_FTM_FLTCTRL_OFFSET) -#define rQDCTRL(_tmr) REG(_tmr,KINETIS_FTM_QDCTRL_OFFSET) -#define rCONF(_tmr) REG(_tmr,KINETIS_FTM_CONF_OFFSET) -#define rFLTPOL(_tmr) REG(_tmr,KINETIS_FTM_FLTPOL_OFFSET) -#define rSYNCONF(_tmr) REG(_tmr,KINETIS_FTM_SYNCONF_OFFSET) -#define rINVCTRL(_tmr) REG(_tmr,KINETIS_FTM_INVCTRL_OFFSET) -#define rSWOCTRL(_tmr) REG(_tmr,KINETIS_FTM_SWOCTRL_OFFSET) -#define rPWMLOAD(_tmr) REG(_tmr,KINETIS_FTM_PWMLOAD_OFFSET) +#define rCNTIN(_tmr) REG(_tmr, S32K1XX_FTM_CNTIN_OFFSET) +#define rSTATUS(_tmr) REG(_tmr, S32K1XX_FTM_STATUS_OFFSET) +#define rMODE(_tmr) REG(_tmr, S32K1XX_FTM_MODE_OFFSET) +#define rSYNC(_tmr) REG(_tmr, S32K1XX_FTM_SYNC_OFFSET) +#define rOUTINIT(_tmr) REG(_tmr, S32K1XX_FTM_OUTINIT_OFFSET) +#define rOUTMASK(_tmr) REG(_tmr, S32K1XX_FTM_OUTMASK_OFFSET) +#define rCOMBINE(_tmr) REG(_tmr, S32K1XX_FTM_COMBINE_OFFSET) +#define rDEADTIME(_tmr) REG(_tmr, S32K1XX_FTM_DEADTIME_OFFSET) +#define rEXTTRIG(_tmr) REG(_tmr, S32K1XX_FTM_EXTTRIG_OFFSET) +#define rPOL(_tmr) REG(_tmr, S32K1XX_FTM_POL_OFFSET) +#define rFMS(_tmr) REG(_tmr, S32K1XX_FTM_FMS_OFFSET) +#define rFILTER(_tmr) REG(_tmr, S32K1XX_FTM_FILTER_OFFSET) +#define rFLTCTRL(_tmr) REG(_tmr, S32K1XX_FTM_FLTCTRL_OFFSET) +#define rQDCTRL(_tmr) REG(_tmr, S32K1XX_FTM_QDCTRL_OFFSET) +#define rCONF(_tmr) REG(_tmr, S32K1XX_FTM_CONF_OFFSET) +#define rFLTPOL(_tmr) REG(_tmr, S32K1XX_FTM_FLTPOL_OFFSET) +#define rSYNCONF(_tmr) REG(_tmr, S32K1XX_FTM_SYNCONF_OFFSET) +#define rINVCTRL(_tmr) REG(_tmr, S32K1XX_FTM_INVCTRL_OFFSET) +#define rSWOCTRL(_tmr) REG(_tmr, S32K1XX_FTM_SWOCTRL_OFFSET) +#define rPWMLOAD(_tmr) REG(_tmr, S32K1XX_FTM_PWMLOAD_OFFSET) -#define CnSC_RESET (FTM_CSC_CHF|FTM_CSC_CHIE|FTM_CSC_MSB|FTM_CSC_MSA|FTM_CSC_ELSB|FTM_CSC_ELSA|FTM_CSC_DMA) -#define CnSC_CAPTURE_INIT (FTM_CSC_CHIE|FTM_CSC_ELSB|FTM_CSC_ELSA) // Both +#define CnSC_RESET (FTM_CNSC_CHF | FTM_CNSC_CHIE | FTM_CNSC_MSB | FTM_CNSC_MSA | FTM_CNSC_ELSB | FTM_CNSC_ELSA | FTM_CNSC_DMA) +#define CnSC_CAPTURE_INIT (FTM_CNSC_CHIE | FTM_CNSC_ELSB | FTM_CNSC_ELSA) // Both #if defined(BOARD_LED_PWM_DRIVE_ACTIVE_LOW) -#define CnSC_PWMOUT_INIT (FTM_CSC_MSB|FTM_CSC_ELSA) +#define CnSC_PWMOUT_INIT (FTM_CNSC_MSB | FTM_CNSC_ELSA) #else -#define CnSC_PWMOUT_INIT (FTM_CSC_MSB|FTM_CSC_ELSB) +#define CnSC_PWMOUT_INIT (FTM_CNSC_MSB | FTM_CNSC_ELSB) #endif #define FTM_SYNC (FTM_SYNC_SWSYNC) @@ -187,7 +187,7 @@ led_pwm_timer_init(unsigned timer) /* disable and configure the timer */ - rSC(timer) = FTM_SC_CLKS_NONE; + rSC(timer) = FTM_SC_CLKS_DIS; rCNT(timer) = 0; rMODE(timer) = 0; @@ -241,11 +241,12 @@ led_pwm_channel_init(unsigned channel) uint32_t chan = led_pwm_channels[channel].timer_channel - 1; - uint16_t rvalue = REG(timer, KINETIS_FTM_CSC_OFFSET(chan)); + uint16_t rvalue = REG(timer, S32K1XX_FTM_CNSC_OFFSET(chan)); rvalue &= ~CnSC_RESET; rvalue |= CnSC_PWMOUT_INIT; - REG(timer, KINETIS_FTM_CSC_OFFSET(chan)) = rvalue; - REG(timer, KINETIS_FTM_CV_OFFSET(0)) = 0; + REG(timer, S32K1XX_FTM_CNSC_OFFSET(chan)) = rvalue; + REG(timer, S32K1XX_FTM_CNV_OFFSET(0)) = 0; + rSC(timer) |= 1 << (FTM_SC_PWMEN_SHIFT + chan); px4_leave_critical_section(flags); } } @@ -274,7 +275,7 @@ led_pwm_servo_set(unsigned channel, uint8_t cvalue) value--; } - REG(timer, KINETIS_FTM_CV_OFFSET(led_pwm_channels[channel].timer_channel - 1)) = value; + REG(timer, S32K1XX_FTM_CNV_OFFSET(led_pwm_channels[channel].timer_channel - 1)) = value; return 0; } @@ -294,7 +295,7 @@ led_pwm_servo_get(unsigned channel) return value; } - value = REG(timer, KINETIS_FTM_CV_OFFSET(led_pwm_channels[channel].timer_channel - 1)); + value = REG(timer, S32K1XX_FTM_CNV_OFFSET(led_pwm_channels[channel].timer_channel - 1)); unsigned period = led_pwm_timer_get_period(timer); return ((value + 1) * 255 / period); } @@ -303,6 +304,10 @@ led_pwm_servo_init(void) { /* do basic timer initialisation first */ for (unsigned i = 0; i < arraySize(led_pwm_timers); i++) { +#if defined(BOARD_HAS_SHARED_PWM_TIMERS) + // Use the io_timer init to mark it initialized + io_timer_init_timer(i); +#endif led_pwm_timer_init(i); } @@ -335,7 +340,7 @@ led_pwm_servo_arm(bool armed) } else { /* disable and configure the timer */ - rSC(i) = FTM_SC_CLKS_NONE; + rSC(i) = FTM_SC_CLKS_DIS; rCNT(i) = 0; } }