diff --git a/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x b/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x index 6b1f8abbf8e7..4544443dd63d 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x +++ b/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x @@ -50,10 +50,7 @@ then param set MPC_MAN_TILT_MAX 60 param set MOT_ORDERING 1 - param set PWM_DISARMED 950 - param set PWM_MAX 1900 - param set PWM_MIN 1250 - param set PWM_RATE 0 + param set DSHOT_CONFIG 600 param set RC_FLT_CUTOFF 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2 b/ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2 index 27fae91ee71c..2960f1af3b53 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2 +++ b/ROMFS/px4fmu_common/init.d/airframes/4053_holybro_kopis2 @@ -25,32 +25,31 @@ then param set IMU_GYRO_CUTOFF 120 param set MC_DTERM_CUTOFF 0 - param set MC_ROLLRATE_P 0.036 + param set MC_ROLLRATE_P 0.075 param set MC_ROLLRATE_I 0.25 - param set MC_ROLLRATE_D 0.0006 + param set MC_ROLLRATE_D 0.0015 param set MC_ROLLRATE_MAX 1600 param set MC_ROLL_P 10 - param set MC_PITCHRATE_P 0.036 + param set MC_PITCHRATE_P 0.075 param set MC_PITCHRATE_I 0.32 - param set MC_PITCHRATE_D 0.0004 + param set MC_PITCHRATE_D 0.0015 param set MC_PITCHRATE_MAX 1600 param set MC_PITCH_P 10 param set MC_YAWRATE_MAX 1000 - param set MC_YAWRATE_P 0.04 + param set MC_YAWRATE_P 0.15 param set MC_YAW_P 4 param set MOT_ORDERING 1 param set MPC_MANTHR_MIN 0 param set MPC_MAN_TILT_MAX 60 - param set PWM_MAX 1950 - param set PWM_MIN 1075 - param set PWM_RATE 400 + + param set DSHOT_CONFIG 1200 param set OSD_ATXXXX_CFG 1 - param set THR_MDL_FAC 0.7 + param set THR_MDL_FAC 0.35 param set MPC_THR_CURVE 1 param set MPC_THR_HOVER 0.12 diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index e33882b17089..82bde1889f6c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -9,6 +9,7 @@ # Otherwise, the variable name goes to the end of the argument. # +set FMU_CMD fmu set MIXER_AUX_FILE none set OUTPUT_AUX_DEV /dev/pwm_output1 set OUTPUT_DEV none @@ -42,9 +43,19 @@ then if [ $IO_PRESENT = yes ] then set OUTPUT_MODE io + if param greater DSHOT_CONFIG 0 + then + set FMU_CMD dshot + fi fi else - set OUTPUT_MODE fmu + if param greater DSHOT_CONFIG 0 + then + set OUTPUT_MODE dshot + set FMU_CMD dshot + else + set OUTPUT_MODE fmu + fi fi fi @@ -80,11 +91,11 @@ then fi fi - if [ $OUTPUT_MODE = fmu ] + if [ $OUTPUT_MODE = $FMU_CMD ] then - if ! fmu mode_$FMU_MODE $FMU_ARGS + if ! $FMU_CMD mode_$FMU_MODE then - echo "FMU start failed" >> $LOG_FILE + echo "$FMU_CMD start failed" >> $LOG_FILE # Error tune. tune_control play -t 2 fi @@ -195,7 +206,7 @@ then if [ $MIXER_AUX_FILE != none ] then - if fmu mode_${AUX_MODE} $FMU_ARGS + if $FMU_CMD mode_${AUX_MODE} then # Append aux mixer to main device. if param greater SYS_HITL 0 @@ -228,82 +239,86 @@ then set FAILSAFE_AUX none fi - # Set min / max for aux out and rates. - if [ $PWM_AUX_OUT != none ] + # for DShot do not configure pwm values + if [ $FMU_CMD != dshot ] then - # Set PWM_AUX output frequency. - if [ $PWM_AUX_RATE != none ] + # Set min / max for aux out and rates. + if [ $PWM_AUX_OUT != none ] then - pwm rate -c ${PWM_AUX_OUT} -r ${PWM_AUX_RATE} -d ${OUTPUT_AUX_DEV} - fi + # Set PWM_AUX output frequency. + if [ $PWM_AUX_RATE != none ] + then + pwm rate -c ${PWM_AUX_OUT} -r ${PWM_AUX_RATE} -d ${OUTPUT_AUX_DEV} + fi - # Set disarmed, min and max PWM_AUX values. - if [ $PWM_AUX_DISARMED != none ] - then - pwm disarmed -c ${PWM_AUX_OUT} -p ${PWM_AUX_DISARMED} -d ${OUTPUT_AUX_DEV} - fi - if [ $PWM_AUX_MIN != none ] - then - pwm min -c ${PWM_AUX_OUT} -p ${PWM_AUX_MIN} -d ${OUTPUT_AUX_DEV} + # Set disarmed, min and max PWM_AUX values. + if [ $PWM_AUX_DISARMED != none ] + then + pwm disarmed -c ${PWM_AUX_OUT} -p ${PWM_AUX_DISARMED} -d ${OUTPUT_AUX_DEV} + fi + if [ $PWM_AUX_MIN != none ] + then + pwm min -c ${PWM_AUX_OUT} -p ${PWM_AUX_MIN} -d ${OUTPUT_AUX_DEV} + fi + if [ $PWM_AUX_MAX != none ] + then + pwm max -c ${PWM_AUX_OUT} -p ${PWM_AUX_MAX} -d ${OUTPUT_AUX_DEV} + fi fi - if [ $PWM_AUX_MAX != none ] + + # + # Per channel disarmed settings. + # + pwm disarmed -c 1 -p p:PWM_AUX_DIS1 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 2 -p p:PWM_AUX_DIS2 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 3 -p p:PWM_AUX_DIS3 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 4 -p p:PWM_AUX_DIS4 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 5 -p p:PWM_AUX_DIS5 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 6 -p p:PWM_AUX_DIS6 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 7 -p p:PWM_AUX_DIS7 -d ${OUTPUT_AUX_DEV} + pwm disarmed -c 8 -p p:PWM_AUX_DIS8 -d ${OUTPUT_AUX_DEV} + + # + # Per channel min settings. + # + pwm min -c 1 -p p:PWM_AUX_MIN1 -d ${OUTPUT_AUX_DEV} + pwm min -c 2 -p p:PWM_AUX_MIN2 -d ${OUTPUT_AUX_DEV} + pwm min -c 3 -p p:PWM_AUX_MIN3 -d ${OUTPUT_AUX_DEV} + pwm min -c 4 -p p:PWM_AUX_MIN4 -d ${OUTPUT_AUX_DEV} + pwm min -c 5 -p p:PWM_AUX_MIN5 -d ${OUTPUT_AUX_DEV} + pwm min -c 6 -p p:PWM_AUX_MIN6 -d ${OUTPUT_AUX_DEV} + pwm min -c 7 -p p:PWM_AUX_MIN7 -d ${OUTPUT_AUX_DEV} + pwm min -c 8 -p p:PWM_AUX_MIN8 -d ${OUTPUT_AUX_DEV} + + # + # Per channel max settings. + # + pwm max -c 1 -p p:PWM_AUX_MAX1 -d ${OUTPUT_AUX_DEV} + pwm max -c 2 -p p:PWM_AUX_MAX2 -d ${OUTPUT_AUX_DEV} + pwm max -c 3 -p p:PWM_AUX_MAX3 -d ${OUTPUT_AUX_DEV} + pwm max -c 4 -p p:PWM_AUX_MAX4 -d ${OUTPUT_AUX_DEV} + pwm max -c 5 -p p:PWM_AUX_MAX5 -d ${OUTPUT_AUX_DEV} + pwm max -c 6 -p p:PWM_AUX_MAX6 -d ${OUTPUT_AUX_DEV} + pwm max -c 7 -p p:PWM_AUX_MAX7 -d ${OUTPUT_AUX_DEV} + pwm max -c 8 -p p:PWM_AUX_MAX8 -d ${OUTPUT_AUX_DEV} + + if [ $FAILSAFE_AUX != none ] then - pwm max -c ${PWM_AUX_OUT} -p ${PWM_AUX_MAX} -d ${OUTPUT_AUX_DEV} + pwm failsafe -c ${PWM_AUX_OUT} -p ${FAILSAFE} -d ${OUTPUT_AUX_DEV} fi - fi - # - # Per channel disarmed settings. - # - pwm disarmed -c 1 -p p:PWM_AUX_DIS1 -d ${OUTPUT_AUX_DEV} - pwm disarmed -c 2 -p p:PWM_AUX_DIS2 -d ${OUTPUT_AUX_DEV} - pwm disarmed -c 3 -p p:PWM_AUX_DIS3 -d ${OUTPUT_AUX_DEV} - pwm disarmed -c 4 -p p:PWM_AUX_DIS4 -d ${OUTPUT_AUX_DEV} - pwm disarmed -c 5 -p p:PWM_AUX_DIS5 -d ${OUTPUT_AUX_DEV} - pwm disarmed -c 6 -p p:PWM_AUX_DIS6 -d ${OUTPUT_AUX_DEV} - pwm disarmed -c 7 -p p:PWM_AUX_DIS7 -d ${OUTPUT_AUX_DEV} - pwm disarmed -c 8 -p p:PWM_AUX_DIS8 -d ${OUTPUT_AUX_DEV} - - # - # Per channel min settings. - # - pwm min -c 1 -p p:PWM_AUX_MIN1 -d ${OUTPUT_AUX_DEV} - pwm min -c 2 -p p:PWM_AUX_MIN2 -d ${OUTPUT_AUX_DEV} - pwm min -c 3 -p p:PWM_AUX_MIN3 -d ${OUTPUT_AUX_DEV} - pwm min -c 4 -p p:PWM_AUX_MIN4 -d ${OUTPUT_AUX_DEV} - pwm min -c 5 -p p:PWM_AUX_MIN5 -d ${OUTPUT_AUX_DEV} - pwm min -c 6 -p p:PWM_AUX_MIN6 -d ${OUTPUT_AUX_DEV} - pwm min -c 7 -p p:PWM_AUX_MIN7 -d ${OUTPUT_AUX_DEV} - pwm min -c 8 -p p:PWM_AUX_MIN8 -d ${OUTPUT_AUX_DEV} - - # - # Per channel max settings. - # - pwm max -c 1 -p p:PWM_AUX_MAX1 -d ${OUTPUT_AUX_DEV} - pwm max -c 2 -p p:PWM_AUX_MAX2 -d ${OUTPUT_AUX_DEV} - pwm max -c 3 -p p:PWM_AUX_MAX3 -d ${OUTPUT_AUX_DEV} - pwm max -c 4 -p p:PWM_AUX_MAX4 -d ${OUTPUT_AUX_DEV} - pwm max -c 5 -p p:PWM_AUX_MAX5 -d ${OUTPUT_AUX_DEV} - pwm max -c 6 -p p:PWM_AUX_MAX6 -d ${OUTPUT_AUX_DEV} - pwm max -c 7 -p p:PWM_AUX_MAX7 -d ${OUTPUT_AUX_DEV} - pwm max -c 8 -p p:PWM_AUX_MAX8 -d ${OUTPUT_AUX_DEV} - - if [ $FAILSAFE_AUX != none ] - then - pwm failsafe -c ${PWM_AUX_OUT} -p ${FAILSAFE} -d ${OUTPUT_AUX_DEV} + # + # Per channel failsafe settings. + # + pwm failsafe -c 1 -p p:PWM_AUX_FAIL1 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 2 -p p:PWM_AUX_FAIL2 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 3 -p p:PWM_AUX_FAIL3 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 4 -p p:PWM_AUX_FAIL4 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 5 -p p:PWM_AUX_FAIL5 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 6 -p p:PWM_AUX_FAIL6 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 7 -p p:PWM_AUX_FAIL7 -d ${OUTPUT_AUX_DEV} + pwm failsafe -c 8 -p p:PWM_AUX_FAIL8 -d ${OUTPUT_AUX_DEV} fi - - # - # Per channel failsafe settings. - # - pwm failsafe -c 1 -p p:PWM_AUX_FAIL1 -d ${OUTPUT_AUX_DEV} - pwm failsafe -c 2 -p p:PWM_AUX_FAIL2 -d ${OUTPUT_AUX_DEV} - pwm failsafe -c 3 -p p:PWM_AUX_FAIL3 -d ${OUTPUT_AUX_DEV} - pwm failsafe -c 4 -p p:PWM_AUX_FAIL4 -d ${OUTPUT_AUX_DEV} - pwm failsafe -c 5 -p p:PWM_AUX_FAIL5 -d ${OUTPUT_AUX_DEV} - pwm failsafe -c 6 -p p:PWM_AUX_FAIL6 -d ${OUTPUT_AUX_DEV} - pwm failsafe -c 7 -p p:PWM_AUX_FAIL7 -d ${OUTPUT_AUX_DEV} - pwm failsafe -c 8 -p p:PWM_AUX_FAIL8 -d ${OUTPUT_AUX_DEV} fi fi @@ -386,6 +401,7 @@ then pwm failsafe -c 8 -p p:PWM_MAIN_FAIL8 fi +unset FMU_CMD unset MIXER_AUX_FILE unset OUTPUT_AUX_DEV unset OUTPUT_DEV diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 846d8abd0829..4c61b338e2c0 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -27,7 +27,6 @@ set FAILSAFE none set FAILSAFE_AUX none set FCONFIG /fs/microsd/etc/config.txt set FEXTRAS /fs/microsd/etc/extras.txt -set FMU_ARGS "" set FMU_MODE pwm set FRC /fs/microsd/etc/rc.txt set IOFW "/etc/extras/px4_io-v2_default.bin" @@ -521,7 +520,6 @@ unset FAILSAFE unset FAILSAFE_AUX unset FCONFIG unset FEXTRAS -unset FMU_ARGS unset FMU_MODE unset FRC unset IO_PRESENT diff --git a/boards/av/x-v1/src/board_config.h b/boards/av/x-v1/src/board_config.h index f3b7c63d9605..6da278ba7f02 100644 --- a/boards/av/x-v1/src/board_config.h +++ b/boards/av/x-v1/src/board_config.h @@ -328,6 +328,8 @@ #define BOARD_ENABLE_CONSOLE_BUFFER +#define BOARD_NUM_IO_TIMERS 5 + __BEGIN_DECLS /**************************************************************************************************** diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake index 6c1aeb9beb34..ef769e8bd79f 100644 --- a/boards/holybro/kakutef7/default.cmake +++ b/boards/holybro/kakutef7/default.cmake @@ -19,6 +19,7 @@ px4_add_board( DRIVERS adc barometer/bmp280 + dshot gps imu/mpu6000 magnetometer diff --git a/boards/holybro/kakutef7/init/rc.board_extras b/boards/holybro/kakutef7/init/rc.board_extras index 7cf3e02d8ddc..19cf94598d6f 100644 --- a/boards/holybro/kakutef7/init/rc.board_extras +++ b/boards/holybro/kakutef7/init/rc.board_extras @@ -8,5 +8,6 @@ then atxxxx start fi - +# DShot telemetry is always on UART7 +dshot telemetry /dev/ttyS5 diff --git a/boards/holybro/kakutef7/nuttx-config/include/board.h b/boards/holybro/kakutef7/nuttx-config/include/board.h index 29b9a2d56c70..fd5b29b6d33f 100644 --- a/boards/holybro/kakutef7/nuttx-config/include/board.h +++ b/boards/holybro/kakutef7/nuttx-config/include/board.h @@ -270,11 +270,6 @@ #define GPIO_UART7_RX GPIO_UART7_RX_1 /* PE7 */ #define GPIO_UART7_TX GPIO_UART7_TX_1 /* PE8 */ -/* UART RX DMA configurations */ - -#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 -#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 - /* SPI * SPI1 SD Card * SPI2 is OSD AT7456E diff --git a/boards/holybro/kakutef7/nuttx-config/nsh/defconfig b/boards/holybro/kakutef7/nuttx-config/nsh/defconfig index c6b82856f1bc..4a5071a58d10 100644 --- a/boards/holybro/kakutef7/nuttx-config/nsh/defconfig +++ b/boards/holybro/kakutef7/nuttx-config/nsh/defconfig @@ -1,3 +1,10 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# # CONFIG_DISABLE_OS_API is not set # CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set # CONFIG_MMCSD_HAVE_CARDDETECT is not set @@ -156,6 +163,7 @@ CONFIG_STM32F7_BBSRAM=y CONFIG_STM32F7_BBSRAM_FILES=5 CONFIG_STM32F7_BKPSRAM=y CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y CONFIG_STM32F7_I2C1=y CONFIG_STM32F7_I2C_DYNTIMEO=y CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 @@ -191,7 +199,6 @@ CONFIG_TASK_NAME_SIZE=24 CONFIG_TIME_EXTENDED=y CONFIG_UART4_BAUD=57600 CONFIG_UART4_RXBUFSIZE=600 -CONFIG_UART4_RXDMA=y CONFIG_UART4_TXBUFSIZE=1500 CONFIG_UART7_BAUD=57600 CONFIG_UART7_RXBUFSIZE=600 diff --git a/boards/holybro/kakutef7/src/board_config.h b/boards/holybro/kakutef7/src/board_config.h index 5a68aa06cace..31cfc88cc2a7 100644 --- a/boards/holybro/kakutef7/src/board_config.h +++ b/boards/holybro/kakutef7/src/board_config.h @@ -216,6 +216,8 @@ #define BOARD_ENABLE_CONSOLE_BUFFER +#define BOARD_NUM_IO_TIMERS 4 + __BEGIN_DECLS /**************************************************************************************************** diff --git a/boards/holybro/kakutef7/src/timer_config.c b/boards/holybro/kakutef7/src/timer_config.c index 325d6a160890..bbe5680bfc17 100644 --- a/boards/holybro/kakutef7/src/timer_config.c +++ b/boards/holybro/kakutef7/src/timer_config.c @@ -60,7 +60,13 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .last_channel_index = 1, .handler = io_timer_handler0, .vectorno = STM32_IRQ_TIM3, - + .dshot = { + .dma_base = DSHOT_DMA1_BASE, + .channel = DShot_Channel5, + .stream = DShot_Stream2, + .start_ccr_register = TIM_DMABASE_CCR3, + .channels_number = 2u /* CCR3 and CCR4 */ + } }, { .base = STM32_TIM1_BASE, @@ -71,7 +77,13 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .last_channel_index = 3, .handler = io_timer_handler1, .vectorno = STM32_IRQ_TIM1CC, - + .dshot = { + .dma_base = DSHOT_DMA2_BASE, + .channel = DShot_Channel6, + .stream = DShot_Stream5, + .start_ccr_register = TIM_DMABASE_CCR1, + .channels_number = 2u /* CCR1 and CCR2 */ + } }, { .base = STM32_TIM8_BASE, @@ -82,6 +94,13 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .last_channel_index = 4, .handler = io_timer_handler2, .vectorno = STM32_IRQ_TIM8CC, + .dshot = { + .dma_base = DSHOT_DMA2_BASE, + .channel = DShot_Channel7, + .stream = DShot_Stream1, + .start_ccr_register = TIM_DMABASE_CCR4, + .channels_number = 1u /* CCR4 */ + } }, { .base = STM32_TIM5_BASE, @@ -92,6 +111,13 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .last_channel_index = 5, .handler = io_timer_handler3, .vectorno = STM32_IRQ_TIM5, + .dshot = { + .dma_base = DSHOT_DMA1_BASE, + .channel = DShot_Channel6, + .stream = DShot_Stream6, // alternatively DShot_Stream0 + .start_ccr_register = TIM_DMABASE_CCR4, + .channels_number = 1u /* CCR4 */ + } } }; diff --git a/boards/mro/ctrl-zero-f7/src/board_config.h b/boards/mro/ctrl-zero-f7/src/board_config.h index 2ec6241e0934..2f4ce593c0ad 100644 --- a/boards/mro/ctrl-zero-f7/src/board_config.h +++ b/boards/mro/ctrl-zero-f7/src/board_config.h @@ -336,6 +336,8 @@ #define BOARD_ENABLE_CONSOLE_BUFFER +#define BOARD_NUM_IO_TIMERS 3 + __BEGIN_DECLS #ifndef __ASSEMBLY__ diff --git a/boards/nxp/fmuk66-v3/src/board_config.h b/boards/nxp/fmuk66-v3/src/board_config.h index 64a44ad6389d..b56fc07714e0 100644 --- a/boards/nxp/fmuk66-v3/src/board_config.h +++ b/boards/nxp/fmuk66-v3/src/board_config.h @@ -520,6 +520,8 @@ __BEGIN_DECLS #define BOARD_HAS_NOISY_FXOS8700_MAG 1 // Disable internal MAG +#define BOARD_NUM_IO_TIMERS 3 + /************************************************************************************ * Public data ************************************************************************************/ diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index 3e657ec81070..d75071d668c9 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -19,6 +19,7 @@ px4_add_board( #camera_trigger #differential_pressure # all available differential pressure drivers #distance_sensor # all available distance sensor drivers + dshot gps #heater #imu # all available imu drivers diff --git a/boards/omnibus/f4sd/nuttx-config/include/board.h b/boards/omnibus/f4sd/nuttx-config/include/board.h index 252427c48cc7..65aa60c2a43b 100644 --- a/boards/omnibus/f4sd/nuttx-config/include/board.h +++ b/boards/omnibus/f4sd/nuttx-config/include/board.h @@ -256,7 +256,7 @@ #define GPIO_USART1_TX GPIO_USART1_TX_1 /* USART1 require a RX DMA configuration */ -#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_1 /* USART3: * diff --git a/boards/omnibus/f4sd/nuttx-config/nsh/defconfig b/boards/omnibus/f4sd/nuttx-config/nsh/defconfig index 41d074536ac1..f3d03a70fc43 100644 --- a/boards/omnibus/f4sd/nuttx-config/nsh/defconfig +++ b/boards/omnibus/f4sd/nuttx-config/nsh/defconfig @@ -180,7 +180,6 @@ CONFIG_TASK_NAME_SIZE=24 CONFIG_TIME_EXTENDED=y CONFIG_UART4_BAUD=57600 CONFIG_UART4_RXBUFSIZE=300 -CONFIG_UART4_RXDMA=y CONFIG_UART4_TXBUFSIZE=300 CONFIG_USART1_RXBUFSIZE=300 CONFIG_USART1_RXDMA=y diff --git a/boards/omnibus/f4sd/src/board_config.h b/boards/omnibus/f4sd/src/board_config.h index 2e1dce831649..f3d5aa3800b3 100644 --- a/boards/omnibus/f4sd/src/board_config.h +++ b/boards/omnibus/f4sd/src/board_config.h @@ -267,6 +267,8 @@ #define BOARD_ENABLE_CONSOLE_BUFFER #define BOARD_CONSOLE_BUFFER_SIZE (1024*3) +#define BOARD_DSHOT_MOTOR_ASSIGNMENT {2, 3, 1, 0}; + __BEGIN_DECLS /**************************************************************************************************** diff --git a/boards/omnibus/f4sd/src/timer_config.c b/boards/omnibus/f4sd/src/timer_config.c index 615a728522a3..cbfae03556d2 100644 --- a/boards/omnibus/f4sd/src/timer_config.c +++ b/boards/omnibus/f4sd/src/timer_config.c @@ -59,7 +59,14 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .first_channel_index = 0, .last_channel_index = 1, .handler = io_timer_handler1, - .vectorno = STM32_IRQ_TIM2 + .vectorno = STM32_IRQ_TIM2, + .dshot = { + .dma_base = DSHOT_DMA1_BASE, + .channel = DShot_Channel3, + .stream = DShot_Stream1, + .start_ccr_register = TIM_DMABASE_CCR3, + .channels_number = 2u /* CCR3 and CCR4 */ + } }, { .base = STM32_TIM3_BASE, @@ -69,9 +76,17 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .first_channel_index = 2, .last_channel_index = 3, .handler = io_timer_handler2, - .vectorno = STM32_IRQ_TIM3 + .vectorno = STM32_IRQ_TIM3, + .dshot = { + .dma_base = DSHOT_DMA1_BASE, + .channel = DShot_Channel5, + .stream = DShot_Stream2, + .start_ccr_register = TIM_DMABASE_CCR3, + .channels_number = 2u /* CCR3 and CCR4 */ + } } }; + /* * OUTPUTS: * M3 : PA3 : TIM2_CH3 diff --git a/boards/px4/fmu-v2/nuttx-config/include/board.h b/boards/px4/fmu-v2/nuttx-config/include/board.h index 4f7d4fa1778c..f5ce3bb6e079 100644 --- a/boards/px4/fmu-v2/nuttx-config/include/board.h +++ b/boards/px4/fmu-v2/nuttx-config/include/board.h @@ -236,7 +236,6 @@ /* UART RX DMA configurations */ -#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 /* CAN diff --git a/boards/px4/fmu-v2/nuttx-config/nsh/defconfig b/boards/px4/fmu-v2/nuttx-config/nsh/defconfig index e508bfc8858a..05d469bf385a 100644 --- a/boards/px4/fmu-v2/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v2/nuttx-config/nsh/defconfig @@ -219,10 +219,8 @@ CONFIG_UART7_SERIAL_CONSOLE=y CONFIG_UART7_TXBUFSIZE=300 CONFIG_UART8_BAUD=57600 CONFIG_UART8_RXBUFSIZE=300 -CONFIG_UART8_RXDMA=y CONFIG_UART8_TXBUFSIZE=300 CONFIG_USART1_RXBUFSIZE=128 -CONFIG_USART1_RXDMA=y CONFIG_USART1_TXBUFSIZE=32 CONFIG_USART2_BAUD=57600 CONFIG_USART2_IFLOWCONTROL=y diff --git a/boards/px4/fmu-v2/src/board_config.h b/boards/px4/fmu-v2/src/board_config.h index e2a4547dff8d..c960ca86669f 100644 --- a/boards/px4/fmu-v2/src/board_config.h +++ b/boards/px4/fmu-v2/src/board_config.h @@ -468,6 +468,8 @@ #define BOARD_HAS_ON_RESET 1 +#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; + __BEGIN_DECLS /**************************************************************************************************** diff --git a/boards/px4/fmu-v2/src/timer_config.c b/boards/px4/fmu-v2/src/timer_config.c index 492400eb26ae..4d9f7e852409 100644 --- a/boards/px4/fmu-v2/src/timer_config.c +++ b/boards/px4/fmu-v2/src/timer_config.c @@ -60,7 +60,13 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .last_channel_index = 3, .handler = io_timer_handler0, .vectorno = STM32_IRQ_TIM1CC, - + .dshot = { + .dma_base = DSHOT_DMA2_BASE, + .channel = DShot_Channel6, + .stream = DShot_Stream5, + .start_ccr_register = TIM_DMABASE_CCR1, + .channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ + } }, { .base = STM32_TIM4_BASE, @@ -70,7 +76,14 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .first_channel_index = 4, .last_channel_index = 5, .handler = io_timer_handler1, - .vectorno = STM32_IRQ_TIM4 + .vectorno = STM32_IRQ_TIM4, + .dshot = { + .dma_base = DSHOT_DMA1_BASE, + .channel = DShot_Channel2, + .stream = DShot_Stream6, + .start_ccr_register = TIM_DMABASE_CCR2, + .channels_number = 2u /* CCR2 and CCR3 */ + } } }; diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 3e1b9ef3a745..92ae0f02f308 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -27,6 +27,7 @@ px4_add_board( camera_trigger differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers + dshot gps #heater imu/adis16448 diff --git a/boards/px4/fmu-v3/nuttx-config/include/board.h b/boards/px4/fmu-v3/nuttx-config/include/board.h index be9b54dc24f3..45beb0712976 100644 --- a/boards/px4/fmu-v3/nuttx-config/include/board.h +++ b/boards/px4/fmu-v3/nuttx-config/include/board.h @@ -236,7 +236,6 @@ /* UART RX DMA configurations */ -#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 /* CAN diff --git a/boards/px4/fmu-v3/nuttx-config/nsh/defconfig b/boards/px4/fmu-v3/nuttx-config/nsh/defconfig index b5076f9aa004..1403ec0b5f1b 100644 --- a/boards/px4/fmu-v3/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v3/nuttx-config/nsh/defconfig @@ -218,10 +218,8 @@ CONFIG_UART7_SERIAL_CONSOLE=y CONFIG_UART7_TXBUFSIZE=300 CONFIG_UART8_BAUD=57600 CONFIG_UART8_RXBUFSIZE=300 -CONFIG_UART8_RXDMA=y CONFIG_UART8_TXBUFSIZE=300 CONFIG_USART1_RXBUFSIZE=128 -CONFIG_USART1_RXDMA=y CONFIG_USART1_TXBUFSIZE=32 CONFIG_USART2_BAUD=57600 CONFIG_USART2_IFLOWCONTROL=y diff --git a/boards/px4/fmu-v3/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v3/nuttx-config/stackcheck/defconfig index 44feff2fc398..af8d087b8077 100644 --- a/boards/px4/fmu-v3/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v3/nuttx-config/stackcheck/defconfig @@ -212,10 +212,8 @@ CONFIG_UART7_SERIAL_CONSOLE=y CONFIG_UART7_TXBUFSIZE=300 CONFIG_UART8_BAUD=57600 CONFIG_UART8_RXBUFSIZE=300 -CONFIG_UART8_RXDMA=y CONFIG_UART8_TXBUFSIZE=300 CONFIG_USART1_RXBUFSIZE=128 -CONFIG_USART1_RXDMA=y CONFIG_USART1_TXBUFSIZE=32 CONFIG_USART2_BAUD=57600 CONFIG_USART2_IFLOWCONTROL=y diff --git a/boards/px4/fmu-v3/src/board_config.h b/boards/px4/fmu-v3/src/board_config.h index 4d305940d8e9..615abec00159 100644 --- a/boards/px4/fmu-v3/src/board_config.h +++ b/boards/px4/fmu-v3/src/board_config.h @@ -468,6 +468,8 @@ #define BOARD_HAS_ON_RESET 1 +#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; + __BEGIN_DECLS /**************************************************************************************************** diff --git a/boards/px4/fmu-v3/src/timer_config.c b/boards/px4/fmu-v3/src/timer_config.c index 492400eb26ae..4d9f7e852409 100644 --- a/boards/px4/fmu-v3/src/timer_config.c +++ b/boards/px4/fmu-v3/src/timer_config.c @@ -60,7 +60,13 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .last_channel_index = 3, .handler = io_timer_handler0, .vectorno = STM32_IRQ_TIM1CC, - + .dshot = { + .dma_base = DSHOT_DMA2_BASE, + .channel = DShot_Channel6, + .stream = DShot_Stream5, + .start_ccr_register = TIM_DMABASE_CCR1, + .channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ + } }, { .base = STM32_TIM4_BASE, @@ -70,7 +76,14 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .first_channel_index = 4, .last_channel_index = 5, .handler = io_timer_handler1, - .vectorno = STM32_IRQ_TIM4 + .vectorno = STM32_IRQ_TIM4, + .dshot = { + .dma_base = DSHOT_DMA1_BASE, + .channel = DShot_Channel2, + .stream = DShot_Stream6, + .start_ccr_register = TIM_DMABASE_CCR2, + .channels_number = 2u /* CCR2 and CCR3 */ + } } }; diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index ba4dafbb2bef..d189b479fd32 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -23,6 +23,7 @@ px4_add_board( camera_trigger differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers + dshot gps heater imu # all available imu drivers diff --git a/boards/px4/fmu-v4/nuttx-config/include/board.h b/boards/px4/fmu-v4/nuttx-config/include/board.h index 3c1a239dc411..cb68f6e3fa65 100644 --- a/boards/px4/fmu-v4/nuttx-config/include/board.h +++ b/boards/px4/fmu-v4/nuttx-config/include/board.h @@ -236,8 +236,8 @@ /* UART8 has no alternate pin config */ /* UART RX DMA configurations */ -#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 -#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_1 /*DMA2 Stream 2*/ +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_1 /*DMA2 Stream 1*/ /* * CAN diff --git a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig index 7a93c4c5946e..9f876c080322 100644 --- a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig @@ -217,7 +217,6 @@ CONFIG_UART7_SERIAL_CONSOLE=y CONFIG_UART7_TXBUFSIZE=300 CONFIG_UART8_BAUD=57600 CONFIG_UART8_RXBUFSIZE=300 -CONFIG_UART8_RXDMA=y CONFIG_UART8_TXBUFSIZE=300 CONFIG_USART1_RXBUFSIZE=600 CONFIG_USART1_RXDMA=y diff --git a/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig index 7afa549107af..8e01a66342f3 100644 --- a/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig @@ -211,7 +211,6 @@ CONFIG_UART7_SERIAL_CONSOLE=y CONFIG_UART7_TXBUFSIZE=300 CONFIG_UART8_BAUD=57600 CONFIG_UART8_RXBUFSIZE=300 -CONFIG_UART8_RXDMA=y CONFIG_UART8_TXBUFSIZE=300 CONFIG_USART1_RXBUFSIZE=600 CONFIG_USART1_RXDMA=y diff --git a/boards/px4/fmu-v4/src/board_config.h b/boards/px4/fmu-v4/src/board_config.h index a6fe9ee29c2c..26d75ee013de 100644 --- a/boards/px4/fmu-v4/src/board_config.h +++ b/boards/px4/fmu-v4/src/board_config.h @@ -355,6 +355,9 @@ #define BOARD_DMA_ALLOC_POOL_SIZE 5120 #define BOARD_HAS_ON_RESET 1 + +#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5}; + __BEGIN_DECLS /**************************************************************************************************** diff --git a/boards/px4/fmu-v4/src/timer_config.c b/boards/px4/fmu-v4/src/timer_config.c index 57cd33ec7c25..97988c34340a 100644 --- a/boards/px4/fmu-v4/src/timer_config.c +++ b/boards/px4/fmu-v4/src/timer_config.c @@ -60,6 +60,13 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .last_channel_index = 3, .handler = io_timer_handler0, .vectorno = STM32_IRQ_TIM1CC, + .dshot = { + .dma_base = DSHOT_DMA2_BASE, + .channel = DShot_Channel6, + .stream = DShot_Stream5, + .start_ccr_register = TIM_DMABASE_CCR1, + .channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ + } }, { @@ -71,6 +78,13 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .last_channel_index = 5, .handler = io_timer_handler1, .vectorno = STM32_IRQ_TIM4, + .dshot = { + .dma_base = DSHOT_DMA1_BASE, + .channel = DShot_Channel2, + .stream = DShot_Stream6, + .start_ccr_register = TIM_DMABASE_CCR2, + .channels_number = 2u /* CCR2 and CCR3 */ + } } }; diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 272a39576a5d..6ee897c4896b 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -25,6 +25,7 @@ px4_add_board( camera_trigger differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers + dshot gps #heater imu/adis16448 diff --git a/boards/px4/fmu-v5/nuttx-config/critmonitor/defconfig b/boards/px4/fmu-v5/nuttx-config/critmonitor/defconfig index 2155889c2543..84b9a985eba4 100644 --- a/boards/px4/fmu-v5/nuttx-config/critmonitor/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/critmonitor/defconfig @@ -227,7 +227,6 @@ CONFIG_UART8_RXDMA=y CONFIG_UART8_TXBUFSIZE=1500 CONFIG_USART1_BAUD=57600 CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_RXDMA=y CONFIG_USART1_TXBUFSIZE=1500 CONFIG_USART2_BAUD=57600 CONFIG_USART2_IFLOWCONTROL=y diff --git a/boards/px4/fmu-v5/nuttx-config/irqmonitor/defconfig b/boards/px4/fmu-v5/nuttx-config/irqmonitor/defconfig index dcbe35338e30..ba037bc8ec9d 100644 --- a/boards/px4/fmu-v5/nuttx-config/irqmonitor/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/irqmonitor/defconfig @@ -226,7 +226,6 @@ CONFIG_UART8_RXDMA=y CONFIG_UART8_TXBUFSIZE=1500 CONFIG_USART1_BAUD=57600 CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_RXDMA=y CONFIG_USART1_TXBUFSIZE=1500 CONFIG_USART2_BAUD=57600 CONFIG_USART2_IFLOWCONTROL=y diff --git a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig index 598b4bb5289e..9846adfaaa74 100644 --- a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig @@ -225,7 +225,6 @@ CONFIG_UART8_RXDMA=y CONFIG_UART8_TXBUFSIZE=1500 CONFIG_USART1_BAUD=57600 CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_RXDMA=y CONFIG_USART1_TXBUFSIZE=1500 CONFIG_USART2_BAUD=57600 CONFIG_USART2_IFLOWCONTROL=y diff --git a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig index 71a8b59fca9a..6f01c87d537b 100644 --- a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig @@ -217,7 +217,6 @@ CONFIG_UART8_RXDMA=y CONFIG_UART8_TXBUFSIZE=1500 CONFIG_USART1_BAUD=57600 CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_RXDMA=y CONFIG_USART1_TXBUFSIZE=1500 CONFIG_USART2_BAUD=57600 CONFIG_USART2_IFLOWCONTROL=y diff --git a/boards/px4/fmu-v5/src/board_config.h b/boards/px4/fmu-v5/src/board_config.h index 1e6fd32f2afa..4b55f960c146 100644 --- a/boards/px4/fmu-v5/src/board_config.h +++ b/boards/px4/fmu-v5/src/board_config.h @@ -671,6 +671,10 @@ #define BOARD_ENABLE_CONSOLE_BUFFER +#define BOARD_NUM_IO_TIMERS 5 + +#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; + __BEGIN_DECLS /**************************************************************************************************** diff --git a/boards/px4/fmu-v5/src/timer_config.c b/boards/px4/fmu-v5/src/timer_config.c index 42a838eef23c..cde6354cffc5 100644 --- a/boards/px4/fmu-v5/src/timer_config.c +++ b/boards/px4/fmu-v5/src/timer_config.c @@ -60,7 +60,13 @@ __EXPORT const io_timers_t io_timers[MAX_IO_TIMERS] = { .last_channel_index = 3, .handler = io_timer_handler0, .vectorno = STM32_IRQ_TIM1CC, - + .dshot = { + .dma_base = DSHOT_DMA2_BASE, + .channel = DShot_Channel6, + .stream = DShot_Stream5, + .start_ccr_register = TIM_DMABASE_CCR1, + .channels_number = 4u /* CCR1, CCR2, CCR3 and CCR4 */ + } }, { .base = STM32_TIM4_BASE, diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 030bca52c014..b1eb4e0157b8 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -672,6 +672,8 @@ #define BOARD_ENABLE_CONSOLE_BUFFER +#define BOARD_NUM_IO_TIMERS 5 + __BEGIN_DECLS /**************************************************************************************************** diff --git a/boards/px4/io-v2/src/board_config.h b/boards/px4/io-v2/src/board_config.h index 0571ae22f31d..d6d592b0c8d0 100644 --- a/boards/px4/io-v2/src/board_config.h +++ b/boards/px4/io-v2/src/board_config.h @@ -162,3 +162,5 @@ #define LED_SIGNAL 5 /* LED? + LED? */ #define LED_ASSERTION 6 /* LED? + LED? + LED? */ #define LED_PANIC 7 /* N/C + N/C + N/C + LED? */ + +#define BOARD_NUM_IO_TIMERS 3 diff --git a/msg/esc_report.msg b/msg/esc_report.msg index 106d43bb4ffa..8aaadb39f9a2 100644 --- a/msg/esc_report.msg +++ b/msg/esc_report.msg @@ -1,12 +1,10 @@ uint64 timestamp # time since system start (microseconds) -uint8 esc_vendor # Vendor of current ESC uint32 esc_errorcount # Number of reported errors by ESC - if supported int32 esc_rpm # Motor RPM, negative for reverse rotation [RPM] - if supported float32 esc_voltage # Voltage measured from current ESC [V] - if supported float32 esc_current # Current measured from current ESC [A] - if supported -float32 esc_temperature # Temperature measured from current ESC [degC] - if supported +uint8 esc_temperature # Temperature measured from current ESC [degC] - if supported float32 esc_setpoint # setpoint of current ESC uint16 esc_setpoint_raw # setpoint of current ESC (Value sent to ESC) uint16 esc_address # Address of current ESC (in most cases 1-8 / must be set by driver) -uint16 esc_version # Version of current ESC - if supported -uint16 esc_state # State of ESC - depend on Vendor +uint8 esc_state # State of ESC - depend on Vendor diff --git a/msg/esc_status.msg b/msg/esc_status.msg index cb5ea77d4121..a12b890fb8e3 100644 --- a/msg/esc_status.msg +++ b/msg/esc_status.msg @@ -1,16 +1,12 @@ uint64 timestamp # time since system start (microseconds) uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. Current (Q2/2013) we support 8 ESCs -uint8 ESC_VENDOR_GENERIC = 0 # generic ESC -uint8 ESC_VENDOR_MIKROKOPTER = 1 # Mikrokopter -uint8 ESC_VENDOR_GRAUPNER_HOTT = 2 # Graupner HoTT ESC -uint8 ESC_VENDOR_TAP = 3 # TAP ESC - uint8 ESC_CONNECTION_TYPE_PPM = 0 # Traditional PPM ESC uint8 ESC_CONNECTION_TYPE_SERIAL = 1 # Serial Bus connected ESC -uint8 ESC_CONNECTION_TYPE_ONESHOOT = 2 # One Shoot PPM +uint8 ESC_CONNECTION_TYPE_ONESHOT = 2 # One Shot PPM uint8 ESC_CONNECTION_TYPE_I2C = 3 # I2C uint8 ESC_CONNECTION_TYPE_CAN = 4 # CAN-Bus +uint8 ESC_CONNECTION_TYPE_DSHOT = 5 # DShot uint16 counter # incremented by the writing thread everytime new data is stored diff --git a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h index ca65702c8c4b..c8e635af2b56 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/io_timer.h @@ -45,7 +45,11 @@ #pragma once __BEGIN_DECLS /* configuration limits */ -#define MAX_IO_TIMERS 4 +#ifdef BOARD_NUM_IO_TIMERS +#define MAX_IO_TIMERS BOARD_NUM_IO_TIMERS +#else +#define MAX_IO_TIMERS 2 +#endif #define MAX_TIMER_IO_CHANNELS 16 #define MAX_LED_TIMERS 2 diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt new file mode 100644 index 000000000000..d04316b3b74c --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(arch_dshot + dshot.c +) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c new file mode 100644 index 000000000000..ec15036d8256 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -0,0 +1,454 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * Author: Igor Misic + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#if (CONFIG_STM32_HAVE_IP_DMA_V1) +//Do nothing. IP DMA V1 MCUs are not supported. +#else + +#include +#include +#include +#include +#include +#include +#include + + +#define REG(_tmr, _reg) (*(volatile uint32_t *)(io_timers[_tmr].dshot.dma_base + _reg)) + +/* DMA registers */ +#define rLIFCR(_tmr) REG(_tmr, STM32_DMA_LIFCR_OFFSET) +#define rHIFCR(_tmr) REG(_tmr, STM32_DMA_HIFCR_OFFSET) + +#define rS0CR(_tmr) REG(_tmr, STM32_DMA_S0CR_OFFSET) +#define rS0NDTR(_tmr) REG(_tmr, STM32_DMA_S0NDTR_OFFSET) +#define rS0PAR(_tmr) REG(_tmr, STM32_DMA_S0PAR_OFFSET) +#define rS0M0AR(_tmr) REG(_tmr, STM32_DMA_S0M0AR_OFFSET) +#define rS0FCR(_tmr) REG(_tmr, STM32_DMA_S0FCR_OFFSET) + +#define rS1CR(_tmr) REG(_tmr, STM32_DMA_S1CR_OFFSET) +#define rS1NDTR(_tmr) REG(_tmr, STM32_DMA_S1NDTR_OFFSET) +#define rS1PAR(_tmr) REG(_tmr, STM32_DMA_S1PAR_OFFSET) +#define rS1M0AR(_tmr) REG(_tmr, STM32_DMA_S1M0AR_OFFSET) +#define rS1FCR(_tmr) REG(_tmr, STM32_DMA_S1FCR_OFFSET) + +#define rS2CR(_tmr) REG(_tmr, STM32_DMA_S2CR_OFFSET) +#define rS2NDTR(_tmr) REG(_tmr, STM32_DMA_S2NDTR_OFFSET) +#define rS2PAR(_tmr) REG(_tmr, STM32_DMA_S2PAR_OFFSET) +#define rS2M0AR(_tmr) REG(_tmr, STM32_DMA_S2M0AR_OFFSET) +#define rS2FCR(_tmr) REG(_tmr, STM32_DMA_S2FCR_OFFSET) + +#define rS3CR(_tmr) REG(_tmr, STM32_DMA_S3CR_OFFSET) +#define rS3NDTR(_tmr) REG(_tmr, STM32_DMA_S3NDTR_OFFSET) +#define rS3PAR(_tmr) REG(_tmr, STM32_DMA_S3PAR_OFFSET) +#define rS3M0AR(_tmr) REG(_tmr, STM32_DMA_S3M0AR_OFFSET) +#define rS3FCR(_tmr) REG(_tmr, STM32_DMA_S3FCR_OFFSET) + +#define rS4CR(_tmr) REG(_tmr, STM32_DMA_S4CR_OFFSET) +#define rS4NDTR(_tmr) REG(_tmr, STM32_DMA_S4NDTR_OFFSET) +#define rS4PAR(_tmr) REG(_tmr, STM32_DMA_S4PAR_OFFSET) +#define rS4M0AR(_tmr) REG(_tmr, STM32_DMA_S4M0AR_OFFSET) +#define rS4FCR(_tmr) REG(_tmr, STM32_DMA_S4FCR_OFFSET) + +#define rS5CR(_tmr) REG(_tmr, STM32_DMA_S5CR_OFFSET) +#define rS5NDTR(_tmr) REG(_tmr, STM32_DMA_S5NDTR_OFFSET) +#define rS5PAR(_tmr) REG(_tmr, STM32_DMA_S5PAR_OFFSET) +#define rS5M0AR(_tmr) REG(_tmr, STM32_DMA_S5M0AR_OFFSET) +#define rS5FCR(_tmr) REG(_tmr, STM32_DMA_S5FCR_OFFSET) + +#define rS6CR(_tmr) REG(_tmr, STM32_DMA_S6CR_OFFSET) +#define rS6NDTR(_tmr) REG(_tmr, STM32_DMA_S6NDTR_OFFSET) +#define rS6PAR(_tmr) REG(_tmr, STM32_DMA_S6PAR_OFFSET) +#define rS6M0AR(_tmr) REG(_tmr, STM32_DMA_S6M0AR_OFFSET) +#define rS6FCR(_tmr) REG(_tmr, STM32_DMA_S6FCR_OFFSET) + +#define rS7CR(_tmr) REG(_tmr, STM32_DMA_S7CR_OFFSET) +#define rS7NDTR(_tmr) REG(_tmr, STM32_DMA_S7NDTR_OFFSET) +#define rS7PAR(_tmr) REG(_tmr, STM32_DMA_S7PAR_OFFSET) +#define rS7M0AR(_tmr) REG(_tmr, STM32_DMA_S7M0AR_OFFSET) +#define rS7FCR(_tmr) REG(_tmr, STM32_DMA_S7FCR_OFFSET) + +#define MOTOR_PWM_BIT_1 14u +#define MOTOR_PWM_BIT_0 7u +#define DSHOT_TIMERS MAX_IO_TIMERS +#define MOTORS_NUMBER DIRECT_PWM_OUTPUT_CHANNELS +#define ONE_MOTOR_DATA_SIZE 16u +#define ONE_MOTOR_BUFF_SIZE 18u +#define ALL_MOTORS_BUF_SIZE (MOTORS_NUMBER * ONE_MOTOR_BUFF_SIZE) +#define DSHOT_THROTTLE_POSITION 5u +#define DSHOT_TELEMETRY_POSITION 4u +#define NIBBLES_SIZE 4u +#define DSHOT_NUMBER_OF_NIBBLES 3u +#define MAX_NUM_CHANNELS_PER_TIMER 4u // CCR1-CCR4 + +typedef struct dshot_handler_t { + bool init; + uint8_t motors_number; +} dshot_handler_t; + +#define DMA_BUFFER_MASK (PX4_ARCH_DCACHE_LINESIZE - 1) +#define DMA_ALIGN_UP(n) (((n) + DMA_BUFFER_MASK) & ~DMA_BUFFER_MASK) +#define DSHOT_BURST_BUFFER_SIZE(motors_number) (DMA_ALIGN_UP(sizeof(uint32_t)*ONE_MOTOR_BUFF_SIZE*motors_number)) + +static dshot_handler_t dshot_handler[DSHOT_TIMERS] = {}; +static uint16_t *motor_buffer = NULL; +static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER)] +__attribute__((aligned(PX4_ARCH_DCACHE_LINESIZE))); // DMA buffer +static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {}; + +#ifdef BOARD_DSHOT_MOTOR_ASSIGNMENT +static const uint8_t motor_assignment[MOTORS_NUMBER] = BOARD_DSHOT_MOTOR_ASSIGNMENT; +#endif /* BOARD_DSHOT_MOTOR_ASSIGNMENT */ + +void dshot_dmar_data_prepare(uint8_t timer, uint8_t first_motor, uint8_t motors_number); +int dshot_setup_stream_registers(uint32_t timer); + +int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) +{ + // Alloc buffers if they do not exist. We don't use channel_mask so that potential future re-init calls can + // use the same buffer. + if (!motor_buffer) { + motor_buffer = (uint16_t *)malloc(sizeof(uint16_t) * ALL_MOTORS_BUF_SIZE); + + if (!motor_buffer) { + return -ENOMEM; + } + } + + unsigned buffer_offset = 0; + + for (unsigned timer = 0; timer < DSHOT_TIMERS; ++timer) { + if (io_timers[timer].base == 0) { // no more timers configured + break; + } + + // we know the uint8_t* cast to uint32_t* is fine, since we're aligned to cache line size +#pragma GCC diagnostic ignored "-Wcast-align" + dshot_burst_buffer[timer] = (uint32_t *)&dshot_burst_buffer_array[buffer_offset]; +#pragma GCC diagnostic pop + buffer_offset += DSHOT_BURST_BUFFER_SIZE(io_timers[timer].dshot.channels_number); + + if (buffer_offset > sizeof(dshot_burst_buffer_array)) { + return -EINVAL; // something is wrong with the board configuration or some other logic + } + } + + /* Init channels */ + int ret_val = OK; + + for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) { + if (channel_mask & (1 << channel)) { + uint8_t timer = timer_io_channels[channel].timer_index; + + if (io_timers[timer].dshot.dma_base == 0) { // board does not configure dshot on this timer + continue; + } + + // First free any that were not DShot mode before + if (-EBUSY == io_timer_is_channel_free(channel)) { + io_timer_free_channel(channel); + } + + ret_val = io_timer_channel_init(channel, IOTimerChanMode_Dshot, NULL, NULL); + + if (OK == ret_val) { + channel_mask &= ~(1 << channel); + dshot_handler[timer].init = true; + } + } + } + + for (uint8_t timer_index = 0; (timer_index < DSHOT_TIMERS) && (OK == ret_val); timer_index++) { + + if (true == dshot_handler[timer_index].init) { + dshot_handler[timer_index].motors_number = io_timers[timer_index].dshot.channels_number; + io_timer_set_dshot_mode(timer_index, dshot_pwm_freq, dshot_handler[timer_index].motors_number); + ret_val = dshot_setup_stream_registers(timer_index); + } + } + + return ret_val; +} + +int dshot_setup_stream_registers(uint32_t timer) +{ + int ret_val = OK; + + switch (io_timers[timer].dshot.stream) { + case DShot_Stream0: + rS0CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); + rS0CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; + rS0CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; + rS0PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; + rS0M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); + rS0FCR(timer) &= 0x0; /* Disable FIFO */ + break; + + case DShot_Stream1: + rS1CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); + rS1CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; + rS1CR(timer) |= DMA_SCR_DIR_M2P; + rS1CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; + rS1PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; + rS1M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); + rS1FCR(timer) &= 0x0; /* Disable FIFO */ + break; + + case DShot_Stream2: + rS2CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); + rS2CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; + rS2CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; + rS2PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; + rS2M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); + rS2FCR(timer) &= 0x0; /* Disable FIFO */ + break; + + case DShot_Stream3: + rS3CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); + rS3CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; + rS3CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; + rS3PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; + rS3M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); + rS3FCR(timer) &= 0x0; /* Disable FIFO */ + break; + + case DShot_Stream4: + rS4CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); + rS4CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; + rS4CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; + rS4PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; + rS4M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); + rS4FCR(timer) &= 0x0; /* Disable FIFO */ + break; + + case DShot_Stream5: + rS5CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); + rS5CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; + rS5CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; + rS5PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; + rS5M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); + rS5FCR(timer) &= 0x0; /* Disable FIFO */ + break; + + case DShot_Stream6: + rS6CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); + rS6CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; + rS6CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; + rS6PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; + rS6M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); + rS6FCR(timer) &= 0x0; /* Disable FIFO */ + break; + + case DShot_Stream7: + rS7CR(timer) |= DMA_SCR_CHSEL(io_timers[timer].dshot.channel); + rS7CR(timer) |= DMA_SCR_PRIHI | DMA_SCR_MSIZE_32BITS | DMA_SCR_PSIZE_32BITS | DMA_SCR_MINC | DMA_SCR_DIR_M2P; + rS7CR(timer) |= DMA_SCR_TCIE | DMA_SCR_HTIE | DMA_SCR_TEIE | DMA_SCR_DMEIE; + rS7PAR(timer) = io_timers[timer].base + STM32_GTIM_DMAR_OFFSET; + rS7M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); + rS7FCR(timer) &= 0x0; /* Disable FIFO */ + break; + + default: + ret_val = ERROR; + break; + } + + return ret_val; +} + +void up_dshot_trigger(void) +{ + uint8_t first_motor = 0; + + for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) { + + if (true == dshot_handler[timer].init) { + + uint32_t dma_int_streamx_mask; + volatile uint32_t *rSxNDTR; + volatile uint32_t *rSxCR; + volatile uint32_t *rxIFCR; + + switch (io_timers[timer].dshot.stream) { + case DShot_Stream0: + dma_int_streamx_mask = DMA_INT_STREAM0_MASK; + rxIFCR = &rLIFCR(timer); + rSxNDTR = &rS0NDTR(timer); + rSxCR = &rS0CR(timer); + break; + + case DShot_Stream1: + dma_int_streamx_mask = DMA_INT_STREAM1_MASK; + rxIFCR = &rLIFCR(timer); + rSxNDTR = &rS1NDTR(timer); + rSxCR = &rS1CR(timer); + break; + + case DShot_Stream2: + dma_int_streamx_mask = DMA_INT_STREAM2_MASK; + rxIFCR = &rLIFCR(timer); + rSxNDTR = &rS2NDTR(timer); + rSxCR = &rS2CR(timer); + break; + + case DShot_Stream3: + dma_int_streamx_mask = DMA_INT_STREAM3_MASK; + rxIFCR = &rLIFCR(timer); + rSxNDTR = &rS3NDTR(timer); + rSxCR = &rS3CR(timer); + break; + + case DShot_Stream4: + dma_int_streamx_mask = DMA_INT_STREAM4_MASK; + rxIFCR = &rHIFCR(timer); + rSxNDTR = &rS4NDTR(timer); + rSxCR = &rS4CR(timer); + break; + + case DShot_Stream5: + dma_int_streamx_mask = DMA_INT_STREAM5_MASK; + rxIFCR = &rHIFCR(timer); + rSxNDTR = &rS5NDTR(timer); + rSxCR = &rS5CR(timer); + break; + + case DShot_Stream6: + dma_int_streamx_mask = DMA_INT_STREAM6_MASK; + rxIFCR = &rHIFCR(timer); + rSxNDTR = &rS6NDTR(timer); + rSxCR = &rS6CR(timer); + break; + + case DShot_Stream7: + default: + dma_int_streamx_mask = DMA_INT_STREAM7_MASK; + rxIFCR = &rHIFCR(timer); + rSxNDTR = &rS7NDTR(timer); + rSxCR = &rS7CR(timer); + break; + } + + // Check if DMA is still in progress (just to be safe, not expected to happen) + if (*rSxCR & DMA_SCR_EN) { + continue; + } + + uint8_t motors_number = dshot_handler[timer].motors_number; + dshot_dmar_data_prepare(timer, first_motor, motors_number); + + // Flush cache so DMA sees the data + up_clean_dcache((uintptr_t)dshot_burst_buffer[timer], + (uintptr_t)dshot_burst_buffer[timer] + DSHOT_BURST_BUFFER_SIZE(motors_number)); + + first_motor += motors_number; + + uint32_t dshot_data_size = motors_number * ONE_MOTOR_BUFF_SIZE; + *rxIFCR |= dma_int_streamx_mask; //clear interrupt flags + *rSxNDTR = dshot_data_size; + io_timer_update_generation(timer); + *rSxCR |= DMA_SCR_EN; // Trigger DMA + } + } +} + +/** +* bits 1-11 - throttle value (0-47 are reserved, 48-2047 give 2000 steps of throttle resolution) +* bit 12 - dshot telemetry enable/disable +* bits 13-16 - XOR checksum +**/ +static void dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool telemetry) +{ + uint16_t packet = 0; + uint16_t checksum = 0; + +#ifdef BOARD_DSHOT_MOTOR_ASSIGNMENT + motor_number = motor_assignment[motor_number]; +#endif /* BOARD_DSHOT_MOTOR_ASSIGNMENT */ + + packet |= throttle << DSHOT_THROTTLE_POSITION; + packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION; + + uint32_t i; + uint16_t csum_data = packet; + + /* XOR checksum calculation */ + csum_data >>= NIBBLES_SIZE; + + for (i = 0; i < DSHOT_NUMBER_OF_NIBBLES; i++) { + checksum ^= (csum_data & 0x0F); // XOR data by nibbles + csum_data >>= NIBBLES_SIZE; + } + + packet |= (checksum & 0x0F); + + for (i = 0; i < ONE_MOTOR_DATA_SIZE; i++) { + motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + i] = (packet & 0x8000) ? MOTOR_PWM_BIT_1 : + MOTOR_PWM_BIT_0; // MSB first + packet <<= 1; + } + + motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + 16] = 0; + motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + 17] = 0; +} + +void up_dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool telemetry) +{ + dshot_motor_data_set(motor_number, throttle + DShot_cmd_MIN_throttle, telemetry); +} + +void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry) +{ + dshot_motor_data_set(channel, command, telemetry); +} + +void dshot_dmar_data_prepare(uint8_t timer, uint8_t first_motor, uint8_t motors_number) +{ + uint32_t *buffer = dshot_burst_buffer[timer]; + + for (uint32_t motor_data_index = 0; motor_data_index < ONE_MOTOR_BUFF_SIZE ; motor_data_index++) { + for (uint32_t motor_index = 0; motor_index < motors_number; motor_index++) { + buffer[motor_data_index * motors_number + motor_index] = motor_buffer[(motor_index + + first_motor) * ONE_MOTOR_BUFF_SIZE + motor_data_index]; + } + } +} + +int up_dshot_arm(bool armed) +{ + return io_timer_set_enable(armed, IOTimerChanMode_Dshot, IO_TIMER_ALL_MODES_CHANNELS); +} + + +#endif diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h new file mode 100644 index 000000000000..a944bc65e37d --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (C) 2019 PX4 Development Team. All rights reserved. + * Author: Igor Misic + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#define DSHOT_MOTOR_PWM_BIT_WIDTH 20u + +/* Configuration for each timer to setup DShot. Some timers have only one while others have two choices for the stream. + * + * TIM1UP - DMA2, Channel6, Stream5 + * TIM2UP - DMA1, Channel3, Stream1 or Stream7 + * TIM3UP - DMA1, Channel5, Stream2 + * TIM4UP - DMA1, Channel2, Stream6 + * TIM5UP - DMA1, Channel6, Stream0 or Stream6 + * TIM6UP - DMA1, Channel7, Stream1 + * TIM7UP - DMA1, Channel1, Stream2 or Stream4 + * TIM8UP - DMA2, Channel7, Stream1 + */ + +#define DSHOT_DMA1_BASE STM32_DMA1_BASE +#define DSHOT_DMA2_BASE STM32_DMA2_BASE + +typedef enum dshot_dma_channel_t { + DShot_Channel0 = 0u, + DShot_Channel1 = 1u, + DShot_Channel2 = 2u, + DShot_Channel3 = 3u, + DShot_Channel4 = 4u, + DShot_Channel5 = 5u, + DShot_Channel6 = 6u, + DShot_Channel7 = 7u +} dshot_dma_channel_t; + +typedef enum dshot_dma_stream_t { + DShot_Stream0 = 0u, + DShot_Stream1 = 1u, + DShot_Stream2 = 2u, + DShot_Stream3 = 3u, + DShot_Stream4 = 4u, + DShot_Stream5 = 5u, + DShot_Stream6 = 6u, + DShot_Stream7 = 7u +} dshot_dma_stream_t; + + +/* The structure which contains configuration for DShot + */ +typedef struct dshot_conf_t { + uint32_t dma_base; + dshot_dma_channel_t channel; + dshot_dma_stream_t stream; + uint32_t start_ccr_register; + uint8_t channels_number; ///< number of channels/outputs (<=MAX_NUM_CHANNELS_PER_TIMER) +} dshot_conf_t; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h index 5264f4016ec0..a8a319a4a72e 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/io_timer.h @@ -7,14 +7,14 @@ * are met: * * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. + * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. + * used to endorse or promote products derived from this software + * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT @@ -32,20 +32,23 @@ ****************************************************************************/ /** - * @file drv_io_timer.h - * - * stm32-specific PWM output data. + * @file io_timer.h */ #include #include #include #include +#include "dshot.h" #pragma once __BEGIN_DECLS /* configuration limits */ -#define MAX_IO_TIMERS 5 +#ifdef BOARD_NUM_IO_TIMERS +#define MAX_IO_TIMERS BOARD_NUM_IO_TIMERS +#else +#define MAX_IO_TIMERS 2 +#endif #define MAX_TIMER_IO_CHANNELS 8 #define MAX_LED_TIMERS 2 @@ -53,6 +56,12 @@ __BEGIN_DECLS #define IO_TIMER_ALL_MODES_CHANNELS 0 +/* TIM_DMA_Base_address TIM DMA Base Address */ +#define TIM_DMABASE_CCR1 0x0000000DU +#define TIM_DMABASE_CCR2 0x0000000EU +#define TIM_DMABASE_CCR3 0x0000000FU +#define TIM_DMABASE_CCR4 0x00000010U + typedef enum io_timer_channel_mode_t { IOTimerChanMode_NotUsed = 0, IOTimerChanMode_PWMOut = 1, @@ -60,6 +69,7 @@ typedef enum io_timer_channel_mode_t { IOTimerChanMode_Capture = 3, IOTimerChanMode_OneShot = 4, IOTimerChanMode_Trigger = 5, + IOTimerChanMode_Dshot = 6, IOTimerChanModeSize } io_timer_channel_mode_t; @@ -74,14 +84,15 @@ typedef uint8_t io_timer_channel_allocation_t; /* big enough to hold MAX_TIMER_I *** the resulting PSC will be one and the timer will count at it's clock frequency. */ typedef struct io_timers_t { - uint32_t base; - uint32_t clock_register; - uint32_t clock_bit; - uint32_t clock_freq; - uint32_t vectorno; - uint32_t first_channel_index; - uint32_t last_channel_index; - xcpt_t handler; + uint32_t base; + uint32_t clock_register; + uint32_t clock_bit; + uint32_t clock_freq; + uint32_t vectorno; + uint32_t first_channel_index; + uint32_t last_channel_index; + xcpt_t handler; + dshot_conf_t dshot; } io_timers_t; /* array of channels in logical order */ @@ -132,5 +143,8 @@ __EXPORT int io_timer_free_channel(unsigned channel); __EXPORT int io_timer_get_channel_mode(unsigned channel); __EXPORT int io_timer_get_mode_channels(io_timer_channel_mode_t mode); __EXPORT extern void io_timer_trigger(void); +__EXPORT void io_timer_update_generation(uint8_t timer); + +__EXPORT extern int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_rate, uint8_t dma_burst_length); __END_DECLS diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h index f8888f402419..27c2dacefc91 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h @@ -41,6 +41,7 @@ #include #include +#include #include @@ -164,6 +165,6 @@ class ArchPX4IOSerial : public PX4IO_serial /** * IO Buffer storage */ - static uint8_t _io_buffer_storage[] __attribute__((aligned(PX4IO_SERIAL_BUF_ALIGN))); + static uint8_t _io_buffer_storage[] __attribute__((aligned(PX4_ARCH_DCACHE_LINESIZE))); }; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 7a11dec0adbc..d977e4dd48f3 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -32,12 +32,7 @@ ****************************************************************************/ /* - * @file drv_pwm_servo.c - * - * Servo driver supporting PWM servos connected to STM32 timer blocks. - * - * Works with any of the 'generic' or 'advanced' STM32 timers that - * have output pins, does not require an interrupt. + * @file drv_io_timer.c */ #include @@ -59,6 +54,7 @@ #include #include +#include #include #include @@ -114,7 +110,7 @@ #define GTIM_SR_CCIF (GTIM_SR_CC4IF|GTIM_SR_CC3IF|GTIM_SR_CC2IF|GTIM_SR_CC1IF) #define GTIM_SR_CCOF (GTIM_SR_CC4OF|GTIM_SR_CC3OF|GTIM_SR_CC2OF|GTIM_SR_CC1OF) -#define CCMR_C1_RESET 0x00ff +#define CCMR_C1_RESET 0x00ff #define CCMR_C1_NUM_BITS 8 #define CCER_C1_NUM_BITS 4 @@ -131,8 +127,18 @@ #else #define CCER_C1_INIT GTIM_CCER_CC1E #endif -// NotUsed PWMOut PWMIn Capture OneShot Trigger -io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT8_MAX, 0, 0, 0, 0, 0 }; + +/* The transfer is done to 1 register starting from TIMx_CR1 + TIMx_DCR.DBA */ +#define TIM_DMABURSTLENGTH_1TRANSFER 0x00000000U +/* The transfer is done to 2 registers starting from TIMx_CR1 + TIMx_DCR.DBA */ +#define TIM_DMABURSTLENGTH_2TRANSFERS 0x00000100U +/* The transfer is done to 3 registers starting from TIMx_CR1 + TIMx_DCR.DBA */ +#define TIM_DMABURSTLENGTH_3TRANSFERS 0x00000200U +/* The transfer is done to 4 registers starting from TIMx_CR1 + TIMx_DCR.DBA */ +#define TIM_DMABURSTLENGTH_4TRANSFERS 0x00000300U + +// NotUsed PWMOut PWMIn Capture OneShot Trigger Dshot +io_timer_channel_allocation_t channel_allocations[IOTimerChanModeSize] = { UINT8_MAX, 0, 0, 0, 0, 0, 0 }; typedef uint8_t io_timer_allocation_t; /* big enough to hold MAX_IO_TIMERS */ @@ -172,7 +178,7 @@ static int io_timer_handler(uint16_t timer_index) /* What is pending and enabled? */ uint16_t statusr = rSR(timer_index); - uint16_t enabled = rDIER(timer_index) & GTIM_SR_CCIF; + uint16_t enabled = rDIER(timer_index) & GTIM_SR_CCIF; /* Iterate over the timer_io_channels table */ @@ -486,6 +492,44 @@ static inline void io_timer_set_oneshot_mode(unsigned timer) rEGR(timer) = GTIM_EGR_UG; } +void io_timer_update_generation(uint8_t timer) +{ + // Re-initialize the counter and generate an update of the registers + rEGR(timer) = ATIM_EGR_UG; +} + +int io_timer_set_dshot_mode(uint8_t timer, unsigned dshot_pwm_freq, uint8_t dma_burst_length) +{ + int ret_val = OK; + uint32_t tim_dma_burst_length; + + if (1u == dma_burst_length) { + tim_dma_burst_length = TIM_DMABURSTLENGTH_1TRANSFER; + + } else if (2u == dma_burst_length) { + tim_dma_burst_length = TIM_DMABURSTLENGTH_2TRANSFERS; + + } else if (3u == dma_burst_length) { + tim_dma_burst_length = TIM_DMABURSTLENGTH_3TRANSFERS; + + } else if (4u == dma_burst_length) { + tim_dma_burst_length = TIM_DMABURSTLENGTH_4TRANSFERS; + + } else { + ret_val = ERROR; + } + + if (OK == ret_val) { + rARR(timer) = DSHOT_MOTOR_PWM_BIT_WIDTH; + rPSC(timer) = ((int)(io_timers[timer].clock_freq / dshot_pwm_freq) / DSHOT_MOTOR_PWM_BIT_WIDTH) - 1; + rEGR(timer) = ATIM_EGR_UG; + rDCR(timer) = (io_timers[timer].dshot.start_ccr_register | tim_dma_burst_length); + rDIER(timer) = ATIM_DIER_UDE; + } + + return ret_val; +} + static inline void io_timer_set_PWM_mode(unsigned timer) { rPSC(timer) = (io_timers[timer].clock_freq / BOARD_PWM_FREQ) - 1; @@ -594,9 +638,10 @@ int io_timer_set_rate(unsigned timer, unsigned rate) uint32_t channels = get_timer_channels(timer); - /* Check that all channels are either in PWM or Oneshot */ + /* Check that all channels are either in PWM, Oneshot or Dshot*/ - if ((channels & (channel_allocations[IOTimerChanMode_PWMOut] | + if ((channels & (channel_allocations[IOTimerChanMode_Dshot] | + channel_allocations[IOTimerChanMode_PWMOut] | channel_allocations[IOTimerChanMode_OneShot] | channel_allocations[IOTimerChanMode_NotUsed])) == channels) { @@ -605,16 +650,20 @@ int io_timer_set_rate(unsigned timer, unsigned rate) /* Request to use OneShot ?*/ - if (rate == 0) { + if (PWM_RATE_ONESHOT == rate) { /* Request to use OneShot * - * We are here because ALL these channels were either PWM or Oneshot + * We are here because ALL these channels were either PWM or Dshot * Now they need to be Oneshot */ + int changePWMOut = reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot); + int changeDshot = reallocate_channel_resources(channels, IOTimerChanMode_Dshot, IOTimerChanMode_OneShot); + int changedChannels = changePWMOut | changeDshot; + /* Did the allocation change */ - if (reallocate_channel_resources(channels, IOTimerChanMode_PWMOut, IOTimerChanMode_OneShot)) { + if (changedChannels) { io_timer_set_oneshot_mode(timer); } @@ -622,11 +671,14 @@ int io_timer_set_rate(unsigned timer, unsigned rate) /* Request to use PWM * - * We are here because ALL these channels were either PWM or Oneshot + * We are here because ALL these channels were either Oneshot or Dshot * Now they need to be PWM */ - if (reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut)) { + int changeOneShot = reallocate_channel_resources(channels, IOTimerChanMode_OneShot, IOTimerChanMode_PWMOut); + int changeDshot = reallocate_channel_resources(channels, IOTimerChanMode_Dshot, IOTimerChanMode_PWMOut); + + if (changeOneShot && changeDshot) { io_timer_set_PWM_mode(timer); } @@ -656,6 +708,7 @@ int io_timer_channel_init(unsigned channel, io_timer_channel_mode_t mode, case IOTimerChanMode_OneShot: case IOTimerChanMode_PWMOut: case IOTimerChanMode_Trigger: + case IOTimerChanMode_Dshot: ccer_setbits = 0; dier_setbits = 0; setbits = CCMR_C1_PWMOUT_INIT; @@ -767,6 +820,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann uint32_t dier_bit = state ? GTIM_DIER_CC1IE : 0; uint32_t ccer_bit = state ? CCER_C1_INIT : 0; + uint32_t cr1_bit = 0; switch (mode) { case IOTimerChanMode_NotUsed: @@ -774,6 +828,12 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann case IOTimerChanMode_PWMOut: case IOTimerChanMode_Trigger: dier_bit = 0; + cr1_bit = GTIM_CR1_CEN | GTIM_CR1_ARPE; + break; + + case IOTimerChanMode_Dshot: + dier_bit = 0; + cr1_bit = state ? GTIM_CR1_CEN : 0; break; case IOTimerChanMode_PWMIn: @@ -818,6 +878,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann if ((state && (mode == IOTimerChanMode_PWMOut || mode == IOTimerChanMode_OneShot || + mode == IOTimerChanMode_Dshot || mode == IOTimerChanMode_Trigger))) { action_cache[timer].gpio[shifts] = timer_io_channels[chan_index].gpio_out; } @@ -855,7 +916,7 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann } /* arm requires the timer be enabled */ - rCR1(actions) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; + rCR1(actions) |= cr1_bit; } else { @@ -877,6 +938,7 @@ int io_timer_set_ccr(unsigned channel, uint16_t value) if (rv == 0) { if ((mode != IOTimerChanMode_PWMOut) && (mode != IOTimerChanMode_OneShot) && + (mode != IOTimerChanMode_Dshot) && (mode != IOTimerChanMode_Trigger)) { rv = -EIO; diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c index 9121ea488b14..ddd39946d0fa 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/pwm_servo.c @@ -123,15 +123,11 @@ int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) /* Allow a rate of 0 to enter oneshot mode */ - if (rate != 0) { + if (rate != PWM_RATE_ONESHOT) { /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */ - if (rate < 1) { - return -ERANGE; - } - - if (rate > 10000) { + if ((rate < PWM_RATE_LOWER_LIMIT) || (rate > PWM_RATE_UPPER_LIMIT)) { return -ERANGE; } } diff --git a/src/drivers/px4fmu/px4fmu_params.c b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/dshot.h similarity index 74% rename from src/drivers/px4fmu/px4fmu_params.c rename to platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/dshot.h index 30f2867c5afa..14d7085e4b2d 100644 --- a/src/drivers/px4fmu/px4fmu_params.c +++ b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/dshot.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -30,23 +30,8 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ +#pragma once + + +#include "../../../stm32_common/include/px4_arch/dshot.h" -/** - * Motor Ordering - * - * Determines the motor ordering. This can be used for example in combination with - * a 4-in-1 ESC that assumes a motor ordering which is different from PX4. - * - * ONLY supported for Quads. - * ONLY supported for fmu output (Pixracer or Omnibus F4). - * - * When changing this, make sure to test the motor response without props first. - * - * @value 0 PX4 - * @value 1 Betaflight / Cleanflight - * - * @min 0 - * @max 1 - * @group PWM Outputs - */ -PARAM_DEFINE_INT32(MOT_ORDERING, 0); diff --git a/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt index 0b5fe399e0b7..5acfab9f87d0 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt @@ -33,6 +33,7 @@ add_subdirectory(../stm32_common/adc adc) +add_subdirectory(../stm32_common/dshot dshot) add_subdirectory(../stm32_common/hrt hrt) add_subdirectory(../stm32_common/led_pwm led_pwm) add_subdirectory(../stm32_common/io_pins io_pins) diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/dshot.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/dshot.h new file mode 100644 index 000000000000..14d7085e4b2d --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/dshot.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once + + +#include "../../../stm32_common/include/px4_arch/dshot.h" + diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/micro_hal.h index 2c06c7641c3d..c997c4f47326 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/micro_hal.h @@ -46,6 +46,7 @@ __BEGIN_DECLS # define PX4_BBSRAM_GETDESC_IOCTL STM32_BBSRAM_GETDESC_IOCTL #endif #define PX4_NUMBER_I2C_BUSES STM32_NI2C +#define PX4_ARCH_DCACHE_LINESIZE 32 __END_DECLS diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/px4io_serial.h index 77564d2ad09f..5898838ee4b4 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/px4io_serial.h +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/px4io_serial.h @@ -33,6 +33,5 @@ #pragma once -#define PX4IO_SERIAL_BUF_ALIGN 4 #include "../../../stm32_common/include/px4_arch/px4io_serial.h" diff --git a/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt index 0b5fe399e0b7..5acfab9f87d0 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32f7/CMakeLists.txt @@ -33,6 +33,7 @@ add_subdirectory(../stm32_common/adc adc) +add_subdirectory(../stm32_common/dshot dshot) add_subdirectory(../stm32_common/hrt hrt) add_subdirectory(../stm32_common/led_pwm led_pwm) add_subdirectory(../stm32_common/io_pins io_pins) diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/dshot.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/dshot.h new file mode 100644 index 000000000000..14d7085e4b2d --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/dshot.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once + + +#include "../../../stm32_common/include/px4_arch/dshot.h" + diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h index af9a59589596..c7d0ca02e6cd 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h @@ -46,6 +46,7 @@ __BEGIN_DECLS #define PX4_BBSRAM_GETDESC_IOCTL STM32F7_BBSRAM_GETDESC_IOCTL #define PX4_FLASH_BASE 0x08000000 #define PX4_NUMBER_I2C_BUSES STM32F7_NI2C +#define PX4_ARCH_DCACHE_LINESIZE ARMV7M_DCACHE_LINESIZE void stm32_flash_lock(void); void stm32_flash_unlock(void); diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/px4io_serial.h index 0e78048670cc..5898838ee4b4 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/px4io_serial.h +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/px4io_serial.h @@ -33,6 +33,5 @@ #pragma once -#define PX4IO_SERIAL_BUF_ALIGN ARMV7M_DCACHE_LINESIZE #include "../../../stm32_common/include/px4_arch/px4io_serial.h" diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index aa4e1140c66a..714461dae3a4 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -292,6 +292,53 @@ struct pwm_output_rc_config { */ #define PWM_SERVO_GET_RATEGROUP(_n) _PX4_IOC(_PWM_SERVO_BASE, 0x70 + _n) +/** specific rates for configuring the timer for OneShot or PWM */ +#define PWM_RATE_ONESHOT 0u +#define PWM_RATE_LOWER_LIMIT 1u +#define PWM_RATE_UPPER_LIMIT 10000u + +/** Dshot PWM frequency */ +#define DSHOT1200 1200000u //Hz +#define DSHOT600 600000u //Hz +#define DSHOT300 300000u //Hz +#define DSHOT150 150000u //Hz + +#define DSHOT_MAX_THROTTLE 1999 + +typedef enum { + DShot_cmd_motor_stop = 0, + DShot_cmd_beacon1, + DShot_cmd_beacon2, + DShot_cmd_beacon3, + DShot_cmd_beacon4, + DShot_cmd_beacon5, + DShot_cmd_esc_info, // V2 includes settings + DShot_cmd_spin_direction_1, + DShot_cmd_spin_direction_2, + DShot_cmd_3d_mode_off, + DShot_cmd_3d_mode_on, + DShot_cmd_settings_request, // Currently not implemented + DShot_cmd_save_settings, + DShot_cmd_spin_direction_normal = 20, + DShot_cmd_spin_direction_reversed = 21, + DShot_cmd_led0_on, // BLHeli32 only + DShot_cmd_led1_on, // BLHeli32 only + DShot_cmd_led2_on, // BLHeli32 only + DShot_cmd_led3_on, // BLHeli32 only + DShot_cmd_led0_off, // BLHeli32 only + DShot_cmd_led1_off, // BLHeli32 only + DShot_cmd_led2_off, // BLHeli32 only + DShot_cmd_led4_off, // BLHeli32 only + DShot_cmd_audio_stream_mode_on_off = 30, // KISS audio Stream mode on/off + DShot_cmd_silent_mode_on_off = 31, // KISS silent Mode on/off + DShot_cmd_signal_line_telemeetry_disable = 32, + DShot_cmd_signal_line_continuous_erpm_telemetry = 33, + DShot_cmd_MAX = 47, + DShot_cmd_MIN_throttle = 48 + // >47 are throttle values +} dshot_command_t; + + /* * Low-level PWM output interface. * @@ -378,4 +425,48 @@ __EXPORT extern int up_pwm_servo_set(unsigned channel, servo_position_t value); */ __EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel); +/** + * Intialise the Dshot outputs using the specified configuration. + * + * @param channel_mask Bitmask of channels (LSB = channel 0) to enable. + * This allows some of the channels to remain configured + * as GPIOs or as another function. + * @param dshot_pwm_freq is frequency of DSHOT signal. Usually DSHOT1200, DSHOT600, DSHOT300 or DSHOT150 + * @return OK on success. + */ +__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq); + +/** + * Set the current dshot throttle value for a channel (motor). + * + * @param channel The channel to set. + * @param throttle The output dshot throttle value in [0, 1999 = DSHOT_MAX_THROTTLE]. + * @param telemetry If true, request telemetry from that motor + */ +__EXPORT extern void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry); + +/** + * Send DShot command to a channel (motor). + * + * @param channel The channel to set. + * @param command dshot_command_t + * @param telemetry If true, request telemetry from that motor + */ +__EXPORT extern void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry); + +/** + * Trigger dshot data transfer. + */ +__EXPORT extern void up_dshot_trigger(void); + +/** + * Arm or disarm dshot outputs (This will enable/disable complete timer for safety purpose.). + * + * When disarmed, dshot output no pulse. + * + * @param armed If true, outputs are armed; if false they + * are disarmed. + */ +__EXPORT extern int up_dshot_arm(bool armed); + __END_DECLS diff --git a/src/drivers/dshot/CMakeLists.txt b/src/drivers/dshot/CMakeLists.txt new file mode 100644 index 000000000000..493419c2935d --- /dev/null +++ b/src/drivers/dshot/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__dshot + MAIN dshot + STACK_MAIN 1200 + SRCS + dshot.cpp + telemetry.cpp + DEPENDS + arch_io_pins + arch_dshot + mixer + mixer_module + output_limit + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/dshot/dshot.cpp b/src/drivers/dshot/dshot.cpp new file mode 100644 index 000000000000..651c43859bb8 --- /dev/null +++ b/src/drivers/dshot/dshot.cpp @@ -0,0 +1,1678 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "telemetry.h" + + +using namespace time_literals; + +/** Mode given via CLI */ +enum PortMode { + PORT_MODE_UNSET = 0, + PORT_FULL_GPIO, + PORT_FULL_PWM, + PORT_PWM8, + PORT_PWM6, + PORT_PWM5, + PORT_PWM4, + PORT_PWM3, + PORT_PWM2, + PORT_PWM1, + PORT_PWM3CAP1, + PORT_PWM4CAP1, + PORT_PWM4CAP2, + PORT_PWM5CAP1, + PORT_PWM2CAP2, + PORT_CAPTURE, +}; + +#if !defined(BOARD_HAS_PWM) +# error "board_config.h needs to define BOARD_HAS_PWM" +#endif + + +class DShotOutput : public cdev::CDev, public ModuleBase, public OutputModuleInterface +{ +public: + enum Mode { + MODE_NONE, + MODE_1PWM, + MODE_2PWM, + MODE_2PWM2CAP, + MODE_3PWM, + MODE_3PWM1CAP, + MODE_4PWM, + MODE_4PWM1CAP, + MODE_4PWM2CAP, + MODE_5PWM, + MODE_5PWM1CAP, + MODE_6PWM, + MODE_8PWM, + MODE_14PWM, + MODE_4CAP, + MODE_5CAP, + MODE_6CAP, + }; + DShotOutput(); + virtual ~DShotOutput(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static DShotOutput *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void Run() override; + + /** @see ModuleBase::print_status() */ + int print_status() override; + + /** change the mode of the running module */ + static int module_new_mode(PortMode new_mode); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + + virtual int init(); + + int set_mode(Mode mode); + Mode get_mode() { return _mode; } + + static void capture_trampoline(void *context, uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, + uint32_t overflow); + + void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + void mixerChanged() override; + + /** + * Send a dshot command to one or all motors + * This is expected to be called from another thread. + * @param num_repetitions number of times to repeat, set at least to 1 + * @param motor_index index or -1 for all + * @return 0 on success, <0 error otherwise + */ + int sendCommandThreadSafe(dshot_command_t command, int num_repetitions, int motor_index); + + void retrieveAndPrintESCInfoThreadSafe(int motor_index); + + bool telemetryEnabled() const { return _telemetry != nullptr; } + +private: + enum class DShotConfig { + Disabled = 0, + DShot150 = 150, + DShot300 = 300, + DShot600 = 600, + DShot1200 = 1200, + }; + + struct Command { + dshot_command_t command; + int num_repetitions{0}; + uint8_t motor_mask{0xff}; + + bool valid() const { return num_repetitions > 0; } + void clear() { num_repetitions = 0; } + }; + + struct Telemetry { + DShotTelemetry handler; + uORB::PublicationData esc_status_pub{ORB_ID(esc_status)}; + int last_motor_index{-1}; + }; + + void updateTelemetryNumMotors(); + void initTelemetry(const char *device); + void handleNewTelemetryData(int motor_index, const DShotTelemetry::EscData &data); + + int requestESCInfo(); + + MixingOutput _mixing_output{*this, MixingOutput::SchedulingPolicy::Auto, false, false}; + + Telemetry *_telemetry{nullptr}; + static char _telemetry_device[20]; + static px4::atomic_bool _request_telemetry_init; + + px4::atomic _request_esc_info{nullptr}; + bool _waiting_for_esc_info{false}; + + Mode _mode{MODE_NONE}; + + uORB::Subscription _param_sub{ORB_ID(parameter_update)}; + + Command _current_command; + px4::atomic _new_command{nullptr}; + + unsigned _num_outputs{0}; + int _class_instance{-1}; + + bool _outputs_on{false}; + uint32_t _output_mask{0}; + bool _outputs_initialized{false}; + + perf_counter_t _cycle_perf; + perf_counter_t _cycle_interval_perf; + + void capture_callback(uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); + int pwm_ioctl(file *filp, int cmd, unsigned long arg); + void update_dshot_out_state(bool on); + + void update_params(); + + int capture_ioctl(file *filp, int cmd, unsigned long arg); + + DShotOutput(const DShotOutput &) = delete; + DShotOutput operator=(const DShotOutput &) = delete; + + DEFINE_PARAMETERS( + (ParamInt) _param_dshot_config, + (ParamFloat) _param_dshot_min, + (ParamInt) _param_mot_pole_count + ) +}; + +char DShotOutput::_telemetry_device[] {}; +px4::atomic_bool DShotOutput::_request_telemetry_init{false}; + +DShotOutput::DShotOutput() : + CDev("/dev/dshot"), + OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), + _cycle_perf(perf_alloc(PC_ELAPSED, "dshot: cycle")), + _cycle_interval_perf(perf_alloc(PC_INTERVAL, "dshot: cycle interval")) +{ + _mixing_output.setAllDisarmedValues(0); + _mixing_output.setAllMinValues(1); + _mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE); + +} + +DShotOutput::~DShotOutput() +{ + /* make sure outputs are off */ + up_dshot_arm(false); + + /* clean up the alternate device node */ + unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); + + perf_free(_cycle_perf); + perf_free(_cycle_interval_perf); + delete _telemetry; +} + +int +DShotOutput::init() +{ + /* do regular cdev init */ + int ret = CDev::init(); + + if (ret != OK) { + return ret; + } + + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + _class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* lets not be too verbose */ + } else if (_class_instance < 0) { + PX4_ERR("FAILED registering class device"); + } + + // Getting initial parameter values + update_params(); + + ScheduleNow(); + + return 0; +} + +int +DShotOutput::set_mode(Mode mode) +{ + unsigned old_mask = _output_mask; + + /* + * Configure for output. + * + * Note that regardless of the configured mode, the task is always + * listening and mixing; the mode just selects which of the channels + * are presented on the output pins. + */ + switch (mode) { + case MODE_1PWM: + /* default output rates */ + _output_mask = 0x1; + _outputs_initialized = false; + _num_outputs = 1; + break; + +#if defined(BOARD_HAS_CAPTURE) + + case MODE_2PWM2CAP: // v1 multi-port with flow control lines as PWM + up_input_capture_set(2, Rising, 0, NULL, NULL); + up_input_capture_set(3, Rising, 0, NULL, NULL); + PX4_DEBUG("MODE_2PWM2CAP"); +#endif + + /* FALLTHROUGH */ + + case MODE_2PWM: // v1 multi-port with flow control lines as PWM + PX4_DEBUG("MODE_2PWM"); + + /* default output rates */ + _output_mask = 0x3; + _outputs_initialized = false; + _num_outputs = 2; + + break; + +#if defined(BOARD_HAS_CAPTURE) + + case MODE_3PWM1CAP: // v1 multi-port with flow control lines as PWM + PX4_DEBUG("MODE_3PWM1CAP"); + up_input_capture_set(3, Rising, 0, NULL, NULL); +#endif + + /* FALLTHROUGH */ + + case MODE_3PWM: // v1 multi-port with flow control lines as PWM + PX4_DEBUG("MODE_3PWM"); + + /* default output rates */ + _output_mask = 0x7; + _outputs_initialized = false; + _num_outputs = 3; + + break; + +#if defined(BOARD_HAS_CAPTURE) + + case MODE_4PWM1CAP: + PX4_DEBUG("MODE_4PWM1CAP"); + up_input_capture_set(4, Rising, 0, NULL, NULL); +#endif + + /* FALLTHROUGH */ + + case MODE_4PWM: // v1 or v2 multi-port as 4 PWM outs + PX4_DEBUG("MODE_4PWM"); + + /* default output rates */ + _output_mask = 0xf; + _outputs_initialized = false; + _num_outputs = 4; + + break; + +#if defined(BOARD_HAS_CAPTURE) + + case MODE_4PWM2CAP: + PX4_DEBUG("MODE_4PWM2CAP"); + up_input_capture_set(5, Rising, 0, NULL, NULL); + + /* default output rates */ + _output_mask = 0x0f; + _outputs_initialized = false; + _num_outputs = 4; + + break; +#endif + +#if defined(BOARD_HAS_CAPTURE) + + case MODE_5PWM1CAP: + PX4_DEBUG("MODE_5PWM1CAP"); + up_input_capture_set(5, Rising, 0, NULL, NULL); +#endif + + /* FALLTHROUGH */ + + case MODE_5PWM: // v1 or v2 multi-port as 5 PWM outs + PX4_DEBUG("MODE_5PWM"); + + /* default output rates */ + _output_mask = 0x1f; + _outputs_initialized = false; + _num_outputs = 4; + + break; + +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 + + case MODE_6PWM: + PX4_DEBUG("MODE_6PWM"); + + /* default output rates */ + _output_mask = 0x3f; + _outputs_initialized = false; + _num_outputs = 6; + + break; +#endif + +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 + + case MODE_8PWM: // AeroCore PWMs as 8 PWM outs + PX4_DEBUG("MODE_8PWM"); + /* default output rates */ + _output_mask = 0xff; + _outputs_initialized = false; + _num_outputs = 8; + + break; +#endif + +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 + + case MODE_14PWM: + PX4_DEBUG("MODE_14PWM"); + /* default output rates */ + _output_mask = 0x3fff; + _outputs_initialized = false; + _num_outputs = 14; + + break; +#endif + + case MODE_NONE: + PX4_DEBUG("MODE_NONE"); + _output_mask = 0x0; + _outputs_initialized = false; + _num_outputs = 0; + + if (old_mask != _output_mask) { + /* disable motor outputs */ + update_dshot_out_state(false); + } + + break; + + default: + return -EINVAL; + } + + _mode = mode; + return OK; +} + +int +DShotOutput::task_spawn(int argc, char *argv[]) +{ + DShotOutput *instance = new DShotOutput(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() == PX4_OK) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +void +DShotOutput::capture_trampoline(void *context, uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) +{ + DShotOutput *dev = reinterpret_cast(context); + dev->capture_callback(chan_index, edge_time, edge_state, overflow); +} + +void +DShotOutput::capture_callback(uint32_t chan_index, + hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) +{ + fprintf(stdout, "DShot: Capture chan:%d time:%lld state:%d overflow:%d\n", chan_index, edge_time, edge_state, overflow); +} + +void +DShotOutput::update_dshot_out_state(bool on) +{ + if (on && !_outputs_initialized && _output_mask != 0) { + DShotConfig config = (DShotConfig)_param_dshot_config.get(); + unsigned dshot_frequency; + + switch (config) { + case DShotConfig::DShot150: + dshot_frequency = DSHOT150; + break; + + case DShotConfig::DShot300: + dshot_frequency = DSHOT300; + break; + + case DShotConfig::DShot1200: + dshot_frequency = DSHOT1200; + break; + + case DShotConfig::DShot600: + default: + dshot_frequency = DSHOT600; + break; + } + + int ret = up_dshot_init(_output_mask, dshot_frequency); + + if (ret != 0) { + PX4_ERR("up_dshot_init failed (%i)", ret); + return; + } + + _outputs_initialized = true; + } + + if (_outputs_initialized) { + up_dshot_arm(on); + _outputs_on = on; + } +} + +void DShotOutput::updateTelemetryNumMotors() +{ + if (!_telemetry) { + return; + } + + int motor_count = 0; + + if (_mixing_output.mixers()) { + motor_count = _mixing_output.mixers()->get_multirotor_count(); + } + + _telemetry->handler.setNumMotors(motor_count); +} + +void DShotOutput::initTelemetry(const char *device) +{ + if (!_telemetry) { + _telemetry = new Telemetry{}; + + if (!_telemetry) { + PX4_ERR("alloc failed"); + return; + } + } + + int ret = _telemetry->handler.init(device); + + if (ret != 0) { + PX4_ERR("telemetry init failed (%i)", ret); + } + + updateTelemetryNumMotors(); +} + +void DShotOutput::handleNewTelemetryData(int motor_index, const DShotTelemetry::EscData &data) +{ + // fill in new motor data + esc_status_s &esc_status = _telemetry->esc_status_pub.get(); + + if (motor_index < esc_status_s::CONNECTED_ESC_MAX) { + esc_status.esc_online_flags |= 1 << motor_index; + esc_status.esc[motor_index].timestamp = data.time; + esc_status.esc[motor_index].esc_rpm = ((int)data.erpm * 100) / (_param_mot_pole_count.get() / 2); + esc_status.esc[motor_index].esc_voltage = (float)data.voltage * 0.01f; + esc_status.esc[motor_index].esc_current = (float)data.current * 0.01f; + esc_status.esc[motor_index].esc_temperature = data.temperature; + // TODO: accumulate consumption and use for battery estimation + } + + // publish when motor index wraps (which is robust against motor timeouts) + if (motor_index < _telemetry->last_motor_index) { + esc_status.timestamp = hrt_absolute_time(); + esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; + esc_status.esc_count = _telemetry->handler.numMotors(); + ++esc_status.counter; + // FIXME: mark all ESC's as online, otherwise commander complains even for a single dropout + esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1; + + _telemetry->esc_status_pub.update(); + + // reset esc data (in case a motor times out, so we won't send stale data) + memset(&esc_status.esc, 0, sizeof(_telemetry->esc_status_pub.get().esc)); + esc_status.esc_online_flags = 0; + } + + _telemetry->last_motor_index = motor_index; +} + +int DShotOutput::sendCommandThreadSafe(dshot_command_t command, int num_repetitions, int motor_index) +{ + Command cmd; + cmd.command = command; + + if (motor_index == -1) { + cmd.motor_mask = 0xff; + + } else { + cmd.motor_mask = 1 << _mixing_output.reorderedMotorIndex(motor_index); + } + + cmd.num_repetitions = num_repetitions; + _new_command.store(&cmd); + + // wait until main thread processed it + while (_new_command.load()) { + px4_usleep(1000); + } + + return 0; +} + +void DShotOutput::retrieveAndPrintESCInfoThreadSafe(int motor_index) +{ + if (_request_esc_info.load() != nullptr) { + // already in progress (not expected to ever happen) + return; + } + + DShotTelemetry::OutputBuffer output_buffer; + output_buffer.motor_index = motor_index; + // start the request + _request_esc_info.store(&output_buffer); + + // wait until processed + int max_time = 1000; + + while (_request_esc_info.load() != nullptr && max_time-- > 0) { + px4_usleep(1000); + } + + _request_esc_info.store(nullptr); // just in case we time out... + + if (output_buffer.buf_pos == 0) { + PX4_ERR("No data received. If telemetry is setup correctly, try again"); + return; + } + + DShotTelemetry::decodeAndPrintEscInfoPacket(output_buffer); +} + +int DShotOutput::requestESCInfo() +{ + _telemetry->handler.redirectOutput(*_request_esc_info.load()); + _waiting_for_esc_info = true; + int motor_index = _mixing_output.reorderedMotorIndex(_request_esc_info.load()->motor_index); + _current_command.motor_mask = 1 << motor_index; + _current_command.num_repetitions = 1; + _current_command.command = DShot_cmd_esc_info; + PX4_DEBUG("Requesting ESC info for motor %i", motor_index); + return motor_index; +} + +void DShotOutput::mixerChanged() +{ + updateTelemetryNumMotors(); +} + +void DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) +{ + if (!_outputs_on) { + return; + } + + int requested_telemetry_index = -1; + + if (_telemetry) { + // check for an ESC info request. We only process it when we're not expecting other telemetry data + if (_request_esc_info.load() != nullptr && !_waiting_for_esc_info && stop_motors + && !_telemetry->handler.expectingData() && !_current_command.valid()) { + requested_telemetry_index = requestESCInfo(); + + } else { + requested_telemetry_index = _mixing_output.reorderedMotorIndex(_telemetry->handler.getRequestMotorIndex()); + } + } + + if (stop_motors) { + + // when motors are stopped we check if we have other commands to send + for (int i = 0; i < (int)num_outputs; i++) { + if (_current_command.valid() && (_current_command.motor_mask & (1 << i))) { + // for some reason we need to always request telemetry when sending a command + up_dshot_motor_command(i, _current_command.command, true); + + } else { + up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index); + } + } + + if (_current_command.valid()) { + --_current_command.num_repetitions; + } + + } else { + for (int i = 0; i < (int)num_outputs; i++) { + up_dshot_motor_data_set(i, math::min(outputs[i], (uint16_t)DSHOT_MAX_THROTTLE), i == requested_telemetry_index); + } + + // clear commands when motors are running + _current_command.clear(); + } + + if (stop_motors || num_control_groups_updated > 0) { + up_dshot_trigger(); + } +} + +void +DShotOutput::Run() +{ + if (should_exit()) { + ScheduleClear(); + _mixing_output.unregister(); + + exit_and_cleanup(); + return; + } + + perf_begin(_cycle_perf); + perf_count(_cycle_interval_perf); + + _mixing_output.update(); + + /* update output status if armed or if mixer is loaded */ + bool outputs_on = _mixing_output.armed().armed || _mixing_output.mixers(); + + if (_outputs_on != outputs_on) { + update_dshot_out_state(outputs_on); + } + + if (_telemetry) { + int telem_update = _telemetry->handler.update(); + + // Are we waiting for ESC info? + if (_waiting_for_esc_info) { + if (telem_update != -1) { + _request_esc_info.store(nullptr); + _waiting_for_esc_info = false; + } + + } else if (telem_update >= 0) { + handleNewTelemetryData(telem_update, _telemetry->handler.latestESCData()); + } + } + + if (_param_sub.updated()) { + update_params(); + } + + // telemetry device update request? + if (_request_telemetry_init.load()) { + initTelemetry(_telemetry_device); + _request_telemetry_init.store(false); + } + + // new command? + if (!_current_command.valid()) { + Command *new_command = _new_command.load(); + + if (new_command) { + _current_command = *new_command; + _new_command.store(nullptr); + } + } + + // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) + _mixing_output.updateSubscriptions(true); + + perf_end(_cycle_perf); +} + +void DShotOutput::update_params() +{ + parameter_update_s pupdate; + _param_sub.update(&pupdate); + + updateParams(); + + // we use a minimum value of 1, since 0 is for disarmed + _mixing_output.setAllMinValues(math::constrain((int)(_param_dshot_min.get() * (float)DSHOT_MAX_THROTTLE), 1, + DSHOT_MAX_THROTTLE)); +} + + +int +DShotOutput::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret; + + /* try it as a Capture ioctl next */ + ret = capture_ioctl(filp, cmd, arg); + + if (ret != -ENOTTY) { + return ret; + } + + /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + switch (_mode) { + case MODE_1PWM: + case MODE_2PWM: + case MODE_3PWM: + case MODE_4PWM: + case MODE_5PWM: + case MODE_2PWM2CAP: + case MODE_3PWM1CAP: + case MODE_4PWM1CAP: + case MODE_4PWM2CAP: + case MODE_5PWM1CAP: +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 + case MODE_6PWM: +#endif +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 + case MODE_8PWM: +#endif +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 + case MODE_14PWM: +#endif + ret = pwm_ioctl(filp, cmd, arg); + break; + + default: + PX4_DEBUG("not in a PWM mode"); + break; + } + + /* if nobody wants it, let CDev have it */ + if (ret == -ENOTTY) { + ret = CDev::ioctl(filp, cmd, arg); + } + + return ret; +} + +int +DShotOutput::pwm_ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + PX4_DEBUG("dshot ioctl cmd: %d, arg: %ld", cmd, arg); + + lock(); + + switch (cmd) { + case PWM_SERVO_GET_COUNT: + case MIXERIOCGETOUTPUTCOUNT: + switch (_mode) { + +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14 + + case MODE_14PWM: + *(unsigned *)arg = 14; + break; +#endif + +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 + + case MODE_8PWM: + *(unsigned *)arg = 8; + break; +#endif + +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 + + case MODE_6PWM: + *(unsigned *)arg = 6; + break; +#endif + + case MODE_5PWM: + case MODE_5PWM1CAP: + *(unsigned *)arg = 5; + break; + + case MODE_4PWM: + case MODE_4PWM1CAP: + case MODE_4PWM2CAP: + *(unsigned *)arg = 4; + break; + + case MODE_3PWM: + case MODE_3PWM1CAP: + *(unsigned *)arg = 3; + break; + + case MODE_2PWM: + case MODE_2PWM2CAP: + *(unsigned *)arg = 2; + break; + + case MODE_1PWM: + *(unsigned *)arg = 1; + break; + + default: + ret = -EINVAL; + break; + } + + break; + + case PWM_SERVO_SET_COUNT: { + /* change the number of outputs that are enabled for + * PWM. This is used to change the split between GPIO + * and PWM under control of the flight config + * parameters. + */ + switch (arg) { + case 0: + set_mode(MODE_NONE); + break; + + case 1: + set_mode(MODE_1PWM); + break; + + case 2: + set_mode(MODE_2PWM); + break; + + case 3: + set_mode(MODE_3PWM); + break; + + case 4: + set_mode(MODE_4PWM); + break; + + case 5: + set_mode(MODE_5PWM); + break; + +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >=6 + + case 6: + set_mode(MODE_6PWM); + break; +#endif + +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >=8 + + case 8: + set_mode(MODE_8PWM); + break; +#endif + + default: + ret = -EINVAL; + break; + } + + break; + } + + case PWM_SERVO_SET_MODE: { + switch (arg) { + case PWM_SERVO_MODE_NONE: + ret = set_mode(MODE_NONE); + break; + + case PWM_SERVO_MODE_1PWM: + ret = set_mode(MODE_1PWM); + break; + + case PWM_SERVO_MODE_2PWM: + ret = set_mode(MODE_2PWM); + break; + + case PWM_SERVO_MODE_2PWM2CAP: + ret = set_mode(MODE_2PWM2CAP); + break; + + case PWM_SERVO_MODE_3PWM: + ret = set_mode(MODE_3PWM); + break; + + case PWM_SERVO_MODE_3PWM1CAP: + ret = set_mode(MODE_3PWM1CAP); + break; + + case PWM_SERVO_MODE_4PWM: + ret = set_mode(MODE_4PWM); + break; + + case PWM_SERVO_MODE_4PWM1CAP: + ret = set_mode(MODE_4PWM1CAP); + break; + + case PWM_SERVO_MODE_4PWM2CAP: + ret = set_mode(MODE_4PWM2CAP); + break; + + case PWM_SERVO_MODE_5PWM: + ret = set_mode(MODE_5PWM); + break; + + case PWM_SERVO_MODE_5PWM1CAP: + ret = set_mode(MODE_5PWM1CAP); + break; + + case PWM_SERVO_MODE_6PWM: + ret = set_mode(MODE_6PWM); + break; + + case PWM_SERVO_MODE_8PWM: + ret = set_mode(MODE_8PWM); + break; + + case PWM_SERVO_MODE_4CAP: + ret = set_mode(MODE_4CAP); + break; + + case PWM_SERVO_MODE_5CAP: + ret = set_mode(MODE_5CAP); + break; + + case PWM_SERVO_MODE_6CAP: + ret = set_mode(MODE_6CAP); + break; + + default: + ret = -EINVAL; + } + + break; + } + + case MIXERIOCRESET: + _mixing_output.resetMixerThreadSafe(); + + break; + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + unsigned buflen = strnlen(buf, 1024); + ret = _mixing_output.loadMixerThreadSafe(buf, buflen); + + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; +} + +int +DShotOutput::capture_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -EINVAL; + +#if defined(BOARD_HAS_CAPTURE) + + lock(); + + input_capture_config_t *pconfig = 0; + + input_capture_stats_t *stats = (input_capture_stats_t *)arg; + + if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP || + _mode == MODE_4PWM1CAP || _mode == MODE_5PWM1CAP || + _mode == MODE_4PWM2CAP) { + + pconfig = (input_capture_config_t *)arg; + } + + switch (cmd) { + + case INPUT_CAP_SET: + if (pconfig) { + ret = up_input_capture_set(pconfig->channel, pconfig->edge, pconfig->filter, + pconfig->callback, pconfig->context); + } + + break; + + case INPUT_CAP_SET_CALLBACK: + if (pconfig) { + ret = up_input_capture_set_callback(pconfig->channel, pconfig->callback, pconfig->context); + } + + break; + + case INPUT_CAP_GET_CALLBACK: + if (pconfig) { + ret = up_input_capture_get_callback(pconfig->channel, &pconfig->callback, &pconfig->context); + } + + break; + + case INPUT_CAP_GET_STATS: + if (arg) { + ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, false); + } + + break; + + case INPUT_CAP_GET_CLR_STATS: + if (arg) { + ret = up_input_capture_get_stats(stats->chan_in_edges_out, stats, true); + } + + break; + + case INPUT_CAP_SET_EDGE: + if (pconfig) { + ret = up_input_capture_set_trigger(pconfig->channel, pconfig->edge); + } + + break; + + case INPUT_CAP_GET_EDGE: + if (pconfig) { + ret = up_input_capture_get_trigger(pconfig->channel, &pconfig->edge); + } + + break; + + case INPUT_CAP_SET_FILTER: + if (pconfig) { + ret = up_input_capture_set_filter(pconfig->channel, pconfig->filter); + } + + break; + + case INPUT_CAP_GET_FILTER: + if (pconfig) { + ret = up_input_capture_get_filter(pconfig->channel, &pconfig->filter); + } + + break; + + case INPUT_CAP_GET_COUNT: + ret = OK; + + switch (_mode) { + case MODE_5PWM1CAP: + case MODE_4PWM1CAP: + case MODE_3PWM1CAP: + *(unsigned *)arg = 1; + break; + + case MODE_2PWM2CAP: + case MODE_4PWM2CAP: + *(unsigned *)arg = 2; + break; + + default: + ret = -EINVAL; + break; + } + + break; + + case INPUT_CAP_SET_COUNT: + ret = OK; + + switch (_mode) { + case MODE_3PWM1CAP: + set_mode(MODE_3PWM1CAP); + break; + + case MODE_2PWM2CAP: + set_mode(MODE_2PWM2CAP); + break; + + case MODE_4PWM1CAP: + set_mode(MODE_4PWM1CAP); + break; + + case MODE_4PWM2CAP: + set_mode(MODE_4PWM2CAP); + break; + + case MODE_5PWM1CAP: + set_mode(MODE_5PWM1CAP); + break; + + default: + ret = -EINVAL; + break; + } + + break; + + default: + ret = -ENOTTY; + break; + } + + unlock(); + +#else + ret = -ENOTTY; +#endif + return ret; +} + +int +DShotOutput::module_new_mode(PortMode new_mode) +{ + if (!is_running()) { + return -1; + } + + DShotOutput::Mode mode; + + mode = DShotOutput::MODE_NONE; + + switch (new_mode) { + case PORT_FULL_GPIO: + case PORT_MODE_UNSET: + break; + + case PORT_FULL_PWM: + +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 4 + /* select 4-pin PWM mode */ + mode = DShotOutput::MODE_4PWM; +#endif +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 6 + mode = DShotOutput::MODE_6PWM; +#endif +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 8 + mode = DShotOutput::MODE_8PWM; +#endif +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 14 + mode = DShotOutput::MODE_14PWM; +#endif + break; + + case PORT_PWM1: + /* select 2-pin PWM mode */ + mode = DShotOutput::MODE_1PWM; + break; + +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 + + case PORT_PWM8: + /* select 8-pin PWM mode */ + mode = DShotOutput::MODE_8PWM; + break; +#endif +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 + + case PORT_PWM6: + /* select 6-pin PWM mode */ + mode = DShotOutput::MODE_6PWM; + break; + + case PORT_PWM5: + /* select 5-pin PWM mode */ + mode = DShotOutput::MODE_5PWM; + break; + + +# if defined(BOARD_HAS_CAPTURE) + + case PORT_PWM5CAP1: + /* select 5-pin PWM mode 1 capture */ + mode = DShotOutput::MODE_5PWM1CAP; + break; + +# endif + + case PORT_PWM4: + /* select 4-pin PWM mode */ + mode = DShotOutput::MODE_4PWM; + break; + + +# if defined(BOARD_HAS_CAPTURE) + + case PORT_PWM4CAP1: + /* select 4-pin PWM mode 1 capture */ + mode = DShotOutput::MODE_4PWM1CAP; + break; + + case PORT_PWM4CAP2: + /* select 4-pin PWM mode 2 capture */ + mode = DShotOutput::MODE_4PWM2CAP; + break; + +# endif + + case PORT_PWM3: + /* select 3-pin PWM mode */ + mode = DShotOutput::MODE_3PWM; + break; + +# if defined(BOARD_HAS_CAPTURE) + + case PORT_PWM3CAP1: + /* select 3-pin PWM mode 1 capture */ + mode = DShotOutput::MODE_3PWM1CAP; + break; +# endif + + case PORT_PWM2: + /* select 2-pin PWM mode */ + mode = DShotOutput::MODE_2PWM; + break; + +# if defined(BOARD_HAS_CAPTURE) + + case PORT_PWM2CAP2: + /* select 2-pin PWM mode 2 capture */ + mode = DShotOutput::MODE_2PWM2CAP; + break; + +# endif +#endif + + default: + return -1; + } + + DShotOutput *object = get_instance(); + + if (mode != object->get_mode()) { + /* (re)set the output mode */ + object->set_mode(mode); + } + + return OK; +} + +int DShotOutput::custom_command(int argc, char *argv[]) +{ + PortMode new_mode = PORT_MODE_UNSET; + const char *verb = argv[0]; + + if (!strcmp(verb, "telemetry")) { + if (argc > 1) { + // telemetry can be requested before the module is started + strncpy(_telemetry_device, argv[1], sizeof(_telemetry_device) - 1); + _telemetry_device[sizeof(_telemetry_device) - 1] = '\0'; + _request_telemetry_init.store(true); + } + + return 0; + } + + int motor_index = -1; // select motor index, default: -1=all + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "m:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'm': + motor_index = strtol(myoptarg, nullptr, 10) - 1; + break; + + default: + return print_usage("unrecognized flag"); + } + } + + struct Command { + const char *name; + dshot_command_t command; + int num_repetitions; + }; + + constexpr Command commands[] = { + {"reverse", DShot_cmd_spin_direction_reversed, 10}, + {"normal", DShot_cmd_spin_direction_normal, 10}, + {"save", DShot_cmd_save_settings, 10}, + {"3d_on", DShot_cmd_3d_mode_on, 10}, + {"3d_off", DShot_cmd_3d_mode_off, 10}, + {"beep1", DShot_cmd_beacon1, 1}, + {"beep2", DShot_cmd_beacon2, 1}, + {"beep3", DShot_cmd_beacon3, 1}, + {"beep4", DShot_cmd_beacon4, 1}, + {"beep5", DShot_cmd_beacon5, 1}, + }; + + for (unsigned i = 0; i < sizeof(commands) / sizeof(commands[0]); ++i) { + if (!strcmp(verb, commands[i].name)) { + if (!is_running()) { + PX4_ERR("module not running"); + return -1; + } + + return get_instance()->sendCommandThreadSafe(commands[i].command, commands[i].num_repetitions, motor_index); + } + } + + if (!strcmp(verb, "esc_info")) { + if (!is_running()) { + PX4_ERR("module not running"); + return -1; + } + + if (motor_index == -1) { + PX4_ERR("No motor index specified"); + return -1; + } + + if (!get_instance()->telemetryEnabled()) { + PX4_ERR("Telemetry is not enabled, but required to get ESC info"); + return -1; + } + + get_instance()->retrieveAndPrintESCInfoThreadSafe(motor_index); + return 0; + } + + + if (!is_running()) { + int ret = DShotOutput::task_spawn(argc, argv); + + if (ret) { + return ret; + } + } + + /* + * Mode switches. + */ + if (!strcmp(verb, "mode_gpio")) { + new_mode = PORT_FULL_GPIO; + + } else if (!strcmp(verb, "mode_pwm")) { + new_mode = PORT_FULL_PWM; + + // mode: defines which outputs to drive (others may be used by other tasks such as camera capture) +#if defined(BOARD_HAS_PWM) + + } else if (!strcmp(verb, "mode_pwm1")) { + new_mode = PORT_PWM1; +#endif + +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 + + } else if (!strcmp(verb, "mode_pwm6")) { + new_mode = PORT_PWM6; + + } else if (!strcmp(verb, "mode_pwm5")) { + new_mode = PORT_PWM5; + +# if defined(BOARD_HAS_CAPTURE) + + } else if (!strcmp(verb, "mode_pwm5cap1")) { + new_mode = PORT_PWM5CAP1; +# endif + + } else if (!strcmp(verb, "mode_pwm4")) { + new_mode = PORT_PWM4; + +# if defined(BOARD_HAS_CAPTURE) + + } else if (!strcmp(verb, "mode_pwm4cap1")) { + new_mode = PORT_PWM4CAP1; + + } else if (!strcmp(verb, "mode_pwm4cap2")) { + new_mode = PORT_PWM4CAP2; +# endif + + } else if (!strcmp(verb, "mode_pwm3")) { + new_mode = PORT_PWM3; + +# if defined(BOARD_HAS_CAPTURE) + + } else if (!strcmp(verb, "mode_pwm3cap1")) { + new_mode = PORT_PWM3CAP1; +# endif + + } else if (!strcmp(verb, "mode_pwm2")) { + new_mode = PORT_PWM2; + +# if defined(BOARD_HAS_CAPTURE) + + } else if (!strcmp(verb, "mode_pwm2cap2")) { + new_mode = PORT_PWM2CAP2; +# endif +#endif +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 + + } else if (!strcmp(verb, "mode_pwm8")) { + new_mode = PORT_PWM8; +#endif + } + + /* was a new mode set? */ + if (new_mode != PORT_MODE_UNSET) { + + /* switch modes */ + return DShotOutput::module_new_mode(new_mode); + } + + return print_usage("unknown command"); +} + +int DShotOutput::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This is the DShot output driver. It is similar to the fmu driver, and can be used as drop-in replacement +to use DShot as ESC communication protocol instead of PWM. + +It supports: +- DShot150, DShot300, DShot600, DShot1200 +- telemetry via separate UART and publishing as esc_status message +- sending DShot commands via CLI + +### Examples +Permanently reverse motor 1: +$ dshot reverse -m 1 +$ dshot save -m 1 +After saving, the reversed direction will be regarded as the normal one. So to reverse again repeat the same commands. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("dshot", "driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task (without any mode set, use any of the mode_* cmds)"); + + PRINT_MODULE_USAGE_PARAM_COMMENT("All of the mode_* commands will start the module if not running already"); + + PRINT_MODULE_USAGE_COMMAND("mode_gpio"); + PRINT_MODULE_USAGE_COMMAND_DESCR("mode_pwm", "Select all available pins as PWM"); +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 + PRINT_MODULE_USAGE_COMMAND("mode_pwm8"); +#endif +#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6 + PRINT_MODULE_USAGE_COMMAND("mode_pwm6"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm5"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm5cap1"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm4"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap1"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap2"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm3"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm3cap1"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm2"); + PRINT_MODULE_USAGE_COMMAND("mode_pwm2cap2"); +#endif +#if defined(BOARD_HAS_PWM) + PRINT_MODULE_USAGE_COMMAND("mode_pwm1"); +#endif + + PRINT_MODULE_USAGE_COMMAND_DESCR("telemetry", "Enable Telemetry on a UART"); + PRINT_MODULE_USAGE_ARG("", "UART device", false); + + // DShot commands + PRINT_MODULE_USAGE_COMMAND_DESCR("reverse", "Reverse motor direction"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("normal", "Normal motor direction"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("save", "Save current settings"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("3d_on", "Enable 3D mode"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("3d_off", "Disable 3D mode"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("beep1", "Send Beep pattern 1"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("beep2", "Send Beep pattern 2"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("beep3", "Send Beep pattern 3"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("beep4", "Send Beep pattern 4"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("beep5", "Send Beep pattern 5"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based, default=all)", true); + + PRINT_MODULE_USAGE_COMMAND_DESCR("esc_info", "Request ESC information"); + PRINT_MODULE_USAGE_PARAM_INT('m', -1, 0, 16, "Motor index (1-based)", false); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int DShotOutput::print_status() +{ + const char *mode_str = nullptr; + + switch (_mode) { + + case MODE_NONE: mode_str = "no outputs"; break; + + case MODE_1PWM: mode_str = "outputs1"; break; + + case MODE_2PWM: mode_str = "outputs2"; break; + + case MODE_2PWM2CAP: mode_str = "outputs2cap2"; break; + + case MODE_3PWM: mode_str = "outputs3"; break; + + case MODE_3PWM1CAP: mode_str = "outputs3cap1"; break; + + case MODE_4PWM: mode_str = "outputs4"; break; + + case MODE_4PWM1CAP: mode_str = "outputs4cap1"; break; + + case MODE_4PWM2CAP: mode_str = "outputs4cap2"; break; + + case MODE_5PWM: mode_str = "outputs5"; break; + + case MODE_5PWM1CAP: mode_str = "outputs5cap1"; break; + + case MODE_6PWM: mode_str = "outputs6"; break; + + case MODE_8PWM: mode_str = "outputs8"; break; + + case MODE_4CAP: mode_str = "cap4"; break; + + case MODE_5CAP: mode_str = "cap5"; break; + + case MODE_6CAP: mode_str = "cap6"; break; + + default: + break; + } + + if (mode_str) { + PX4_INFO("Mode: %s", mode_str); + } + + PX4_INFO("Outputs initialized: %s", _outputs_initialized ? "yes" : "no"); + PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no"); + perf_print_counter(_cycle_perf); + perf_print_counter(_cycle_interval_perf); + _mixing_output.printStatus(); + if (_telemetry) { + PX4_INFO("telemetry on: %s", _telemetry_device); + _telemetry->handler.printStatus(); + } + + return 0; +} + +extern "C" __EXPORT int dshot_main(int argc, char *argv[]); + +int +dshot_main(int argc, char *argv[]) +{ + return DShotOutput::main(argc, argv); +} diff --git a/src/drivers/dshot/module.yaml b/src/drivers/dshot/module.yaml new file mode 100644 index 000000000000..a276ceda75bb --- /dev/null +++ b/src/drivers/dshot/module.yaml @@ -0,0 +1,54 @@ +module_name: DShot Driver +serial_config: + - command: dshot telemetry ${SERIAL_DEV} + port_config_param: + name: DSHOT_TEL_CFG + group: DShot + +parameters: + - group: DShot + definitions: + DSHOT_CONFIG: + description: + short: Configure DShot + long: | + This enables/disables DShot. The different modes define different + speeds, for example DShot150 = 150kb/s. Not all ESCs support all modes. + + Note: this enables DShot on the FMU outputs. For boards with an IO it is the + AUX outputs. + type: enum + values: + 0: Disable (use PWM/Oneshot) + 150: DShot150 + 300: DShot300 + 600: DShot600 + 1200: DShot1200 + reboot_required: true + default: 0 + DSHOT_MIN: + description: + short: Minimum DShot Motor Output + long: | + Minimum Output Value for DShot in percent. The value depends on the ESC. Make + sure to set this high enough so that the motors are always spinning while + armed. + type: float + unit: '%' + min: 0 + max: 1 + decimal: 2 + increment: 0.01 + default: 0.055 + MOT_POLE_COUNT: # only used by dshot so far, so keep it under the dshot group + description: + short: Number of magnetic poles of the motors + long: | + Specify the number of magnetic poles of the motors. + It is required to compute the RPM value from the eRPM returned with the ESC telemetry. + + Either get the number from the motor spec sheet or count the magnets on the bell of the motor (not the stator magnets). + Typical motors for 5 inch props have 14 poles. + type: int32 + default: 14 + diff --git a/src/drivers/dshot/telemetry.cpp b/src/drivers/dshot/telemetry.cpp new file mode 100644 index 000000000000..c263bb330112 --- /dev/null +++ b/src/drivers/dshot/telemetry.cpp @@ -0,0 +1,475 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "telemetry.h" + +#include + +#include +#include +#include +#include + +using namespace time_literals; + +#define DSHOT_TELEMETRY_UART_BAUDRATE 115200 + +DShotTelemetry::~DShotTelemetry() +{ + deinit(); +} + +int DShotTelemetry::init(const char *uart_device) +{ + deinit(); + _uart_fd = ::open(uart_device, O_RDONLY | O_NOCTTY); + + if (_uart_fd < 0) { + PX4_ERR("failed to open serial port: %s err: %d", uart_device, errno); + return -errno; + } + + _num_timeouts = 0; + _num_successful_responses = 0; + _current_motor_index_request = -1; + return setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE); +} + +void DShotTelemetry::deinit() +{ + if (_uart_fd >= 0) { + close(_uart_fd); + _uart_fd = -1; + } +} + +int DShotTelemetry::redirectOutput(OutputBuffer &buffer) +{ + if (expectingData()) { + // Error: cannot override while we already expect data + return -EBUSY; + } + + _current_motor_index_request = buffer.motor_index; + _current_request_start = hrt_absolute_time(); + _redirect_output = &buffer; + _redirect_output->buf_pos = 0; + return 0; +} + +int DShotTelemetry::update() +{ + if (_uart_fd < 0) { + return -1; + } + + if (_current_motor_index_request == -1) { + // nothing in progress, start a request + _current_motor_index_request = 0; + _current_request_start = 0; + _frame_position = 0; + return -1; + } + + // read from the uart. This must be non-blocking, so check first if there is data available + int bytes_available = 0; + int ret = ioctl(_uart_fd, FIONREAD, (unsigned long)&bytes_available); + + if (ret != 0 || bytes_available <= 0) { + // no data available. Check for a timeout + const hrt_abstime now = hrt_absolute_time(); + + if (_current_request_start > 0 && now - _current_request_start > 30_ms) { + if (_redirect_output) { + // clear and go back to internal buffer + _redirect_output = nullptr; + _current_motor_index_request = -1; + + } else { + PX4_DEBUG("ESC telemetry timeout for motor %i (frame pos=%i)", _current_motor_index_request, _frame_position); + ++_num_timeouts; + } + + requestNextMotor(); + return -2; + } + + return -1; + } + + const int buf_length = ESC_FRAME_SIZE; + uint8_t buf[buf_length]; + + int num_read = read(_uart_fd, buf, buf_length); + ret = -1; + + for (int i = 0; i < num_read && ret == -1; ++i) { + if (_redirect_output) { + _redirect_output->buffer[_redirect_output->buf_pos++] = buf[i]; + + if (_redirect_output->buf_pos == sizeof(_redirect_output->buffer)) { + // buffer full: return & go back to internal buffer + _redirect_output = nullptr; + ret = _current_motor_index_request; + _current_motor_index_request = -1; + requestNextMotor(); + } + + } else { + bool successful_decoding; + + if (decodeByte(buf[i], successful_decoding)) { + if (successful_decoding) { + ret = _current_motor_index_request; + } + + requestNextMotor(); + } + } + } + + return ret; +} + +bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding) +{ + _frame_buffer[_frame_position++] = byte; + successful_decoding = false; + + if (_frame_position == ESC_FRAME_SIZE) { + PX4_DEBUG("got ESC frame for motor %i", _current_motor_index_request); + uint8_t checksum = crc8(_frame_buffer, ESC_FRAME_SIZE - 1); + uint8_t checksum_data = _frame_buffer[ESC_FRAME_SIZE - 1]; + + if (checksum == checksum_data) { + _latest_data.time = hrt_absolute_time(); + _latest_data.temperature = _frame_buffer[0]; + _latest_data.voltage = (_frame_buffer[1] << 8) | _frame_buffer[2]; + _latest_data.current = (_frame_buffer[3] << 8) | _frame_buffer[4]; + _latest_data.consumption = (_frame_buffer[5]) << 8 | _frame_buffer[6]; + _latest_data.erpm = (_frame_buffer[7] << 8) | _frame_buffer[8]; + PX4_DEBUG("Motor %i: temp=%i, V=%i, cur=%i, consumpt=%i, rpm=%i", _current_motor_index_request, + _latest_data.temperature, _latest_data.voltage, _latest_data.current, _latest_data.consumption, + _latest_data.erpm); + ++_num_successful_responses; + successful_decoding = true; + } + + return true; + } + + return false; +} + +void DShotTelemetry::printStatus() const +{ + PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses); + PX4_INFO("Number of timeouts: %i", _num_timeouts); +} + +uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed) +{ + uint8_t crc_u = crc ^ crc_seed; + + for (int i = 0; i < 8; ++i) { + crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); + } + + return crc_u; +} + + +uint8_t DShotTelemetry::crc8(const uint8_t *buf, uint8_t len) +{ + uint8_t crc = 0; + + for (int i = 0; i < len; ++i) { + crc = updateCrc8(buf[i], crc); + } + + return crc; +} + + +void DShotTelemetry::requestNextMotor() +{ + _current_motor_index_request = (_current_motor_index_request + 1) % _num_motors; + _current_request_start = 0; + _frame_position = 0; +} + +int DShotTelemetry::getRequestMotorIndex() +{ + if (_current_request_start != 0) { + // already in progress, do not send another request + return -1; + } + + _current_request_start = hrt_absolute_time(); + return _current_motor_index_request; +} + +void DShotTelemetry::decodeAndPrintEscInfoPacket(const OutputBuffer &buffer) +{ + static constexpr int version_position = 12; + const uint8_t *data = buffer.buffer; + + if (buffer.buf_pos < version_position) { + PX4_ERR("Not enough data received"); + return; + } + + enum class ESCVersionInfo { + BLHELI32, + KissV1, + KissV2, + }; + ESCVersionInfo version; + int packet_length; + + if (data[version_position] == 254) { + version = ESCVersionInfo::BLHELI32; + packet_length = esc_info_size_blheli32; + + } else if (data[version_position] == 255) { + version = ESCVersionInfo::KissV2; + packet_length = esc_info_size_kiss_v2; + + } else { + version = ESCVersionInfo::KissV1; + packet_length = esc_info_size_kiss_v1; + } + + if (buffer.buf_pos != packet_length) { + PX4_ERR("Packet length mismatch (%i != %i)", buffer.buf_pos, packet_length); + return; + } + + if (DShotTelemetry::crc8(data, packet_length - 1) != data[packet_length - 1]) { + PX4_ERR("Checksum mismatch"); + return; + } + + uint8_t esc_firmware_version = 0; + uint8_t esc_firmware_subversion = 0; + uint8_t esc_type = 0; + + switch (version) { + case ESCVersionInfo::KissV1: + esc_firmware_version = data[12]; + esc_firmware_subversion = (data[13] & 0x1f) + 97; + esc_type = (data[13] & 0xe0) >> 5; + break; + + case ESCVersionInfo::KissV2: + case ESCVersionInfo::BLHELI32: + esc_firmware_version = data[13]; + esc_firmware_subversion = data[14]; + esc_type = data[15]; + break; + } + + const char *esc_type_str = ""; + + switch (version) { + case ESCVersionInfo::KissV1: + case ESCVersionInfo::KissV2: + switch (esc_type) { + case 1: esc_type_str = "KISS8A"; + break; + + case 2: esc_type_str = "KISS16A"; + break; + + case 3: esc_type_str = "KISS24A"; + break; + + case 5: esc_type_str = "KISS Ultralite"; + break; + + default: esc_type_str = "KISS (unknown)"; + break; + } + + break; + + case ESCVersionInfo::BLHELI32: { + char *esc_type_mutable = (char *)(data + 31); + esc_type_mutable[32] = 0; + esc_type_str = esc_type_mutable; + } + break; + } + + PX4_INFO("ESC Type: %s", esc_type_str); + + PX4_INFO("MCU Serial Number: %02x%02x%02x-%02x%02x%02x-%02x%02x%02x-%02x%02x%02x", + data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8], + data[9], data[10], data[11]); + + switch (version) { + case ESCVersionInfo::KissV1: + case ESCVersionInfo::KissV2: + PX4_INFO("Firmware version: %d.%d%c", esc_firmware_version / 100, esc_firmware_version % 100, + (char)esc_firmware_subversion); + break; + + case ESCVersionInfo::BLHELI32: + PX4_INFO("Firmware version: %d.%d", esc_firmware_version, esc_firmware_subversion); + break; + } + + if (version == ESCVersionInfo::KissV2 || version == ESCVersionInfo::BLHELI32) { + PX4_INFO("Rotation Direction: %s", data[16] ? "reversed" : "normal"); + PX4_INFO("3D Mode: %s", data[17] ? "on" : "off"); + } + + if (version == ESCVersionInfo::BLHELI32) { + uint8_t setting = data[18]; + + switch (setting) { + case 0: + PX4_INFO("Low voltage Limit: off"); + break; + + case 255: + PX4_INFO("Low voltage Limit: unsupported"); + break; + + default: + PX4_INFO("Low voltage Limit: %d.%01d V", setting / 10, setting % 10); + break; + } + + setting = data[19]; + + switch (setting) { + case 0: + PX4_INFO("Current Limit: off"); + break; + + case 255: + PX4_INFO("Current Limit: unsupported"); + break; + + default: + PX4_INFO("Current Limit: %d A", setting); + break; + } + + for (int i = 0; i < 4; ++i) { + setting = data[i + 20]; + PX4_INFO("LED %d: %s", i, setting ? (setting == 255 ? "unsupported" : "on") : "off"); + } + } + + +} + +int DShotTelemetry::setBaudrate(unsigned baud) +{ + int speed; + + switch (baud) { + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + + default: + return -EINVAL; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_uart_fd, &uart_config); + + // + // Input flags - Turn off input processing + // + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | + INLCR | PARMRK | INPCK | ISTRIP | IXON); + // + // Output flags - Turn off output processing + // + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | + // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); + uart_config.c_oflag = 0; + + // + // No line processing + // + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + /* no parity, one stop bit, disable flow control */ + uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + return -errno; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + return -errno; + } + + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { + return -errno; + } + + return 0; +} + diff --git a/src/drivers/dshot/telemetry.h b/src/drivers/dshot/telemetry.h new file mode 100644 index 000000000000..a5c549c47676 --- /dev/null +++ b/src/drivers/dshot/telemetry.h @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +class DShotTelemetry +{ +public: + struct EscData { + hrt_abstime time; + int8_t temperature; ///< [deg C] + int16_t voltage; ///< [0.01V] + int16_t current; ///< [0.01A] + int16_t consumption; ///< [mAh] + int16_t erpm; ///< [100ERPM] + }; + + static constexpr int esc_info_size_blheli32 = 64; + static constexpr int esc_info_size_kiss_v1 = 15; + static constexpr int esc_info_size_kiss_v2 = 21; + static constexpr int max_esc_info_size = esc_info_size_blheli32; + + struct OutputBuffer { + uint8_t buffer[max_esc_info_size]; + int buf_pos{0}; + int motor_index; + }; + + ~DShotTelemetry(); + + int init(const char *uart_device); + + void deinit(); + + void setNumMotors(int num_motors) { _num_motors = num_motors; } + int numMotors() const { return _num_motors; } + + /** + * Read telemetry from the UART (non-blocking) and handle timeouts. + * @return -1 if no update, -2 timeout, >= 0 for the motor index. Use @latestESCData() to get the data. + */ + int update(); + + /** + * Redirect everything that is read into a different buffer. + * Future calls to @update will write to that instead of an internal buffer, until @update returns + * a value different from -1. No decoding is done. + * The caller must ensure the buffer exists until that point. + * @param buffer + * @return 0 on success <0 on error + */ + int redirectOutput(OutputBuffer &buffer); + + bool redirectActive() const { return _redirect_output != nullptr; } + + /** + * Get the motor index for which telemetry should be requested. + * @return -1 if no request should be made, motor index otherwise + */ + int getRequestMotorIndex(); + + const EscData &latestESCData() const { return _latest_data; } + + /** + * Check whether we are currently expecting to read new data from an ESC + */ + bool expectingData() const { return _current_request_start != 0; } + + void printStatus() const; + + static void decodeAndPrintEscInfoPacket(const OutputBuffer &buffer); + +private: + static constexpr int ESC_FRAME_SIZE = 10; + + /** + * set the Baudrate + * @param baud + * @return 0 on success, <0 on error + */ + int setBaudrate(unsigned baud); + + void requestNextMotor(); + + /** + * Decode a single byte from an ESC feedback frame + * @param byte + * @param successful_decoding set to true if checksum matches + * @return true if received the expected amount of bytes and the next motor can be requested + */ + bool decodeByte(uint8_t byte, bool &successful_decoding); + + static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed); + static uint8_t crc8(const uint8_t *buf, uint8_t len); + + int _uart_fd{-1}; + int _num_motors{0}; + uint8_t _frame_buffer[ESC_FRAME_SIZE]; + int _frame_position{0}; + + EscData _latest_data; + + int _current_motor_index_request{-1}; + hrt_abstime _current_request_start{0}; + + OutputBuffer *_redirect_output{nullptr}; ///< if set, all read bytes are stored here instead of the internal buffer + + // statistics + int _num_timeouts{0}; + int _num_successful_responses{0}; +}; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index cc311dbba169..9c40ac742568 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -614,8 +614,6 @@ MK::task_main() for (unsigned int i = 0; i < _num_outputs; i++) { esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i; - esc.esc[i].esc_vendor = esc_status_s::ESC_VENDOR_MIKROKOPTER; - esc.esc[i].esc_version = (uint16_t) Motor[i].Version; esc.esc[i].esc_voltage = 0.0F; esc.esc[i].esc_current = static_cast(Motor[i].Current) * 0.1F; esc.esc[i].esc_rpm = (uint16_t) 0; @@ -630,8 +628,8 @@ MK::task_main() esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint; } - esc.esc[i].esc_temperature = static_cast(Motor[i].Temperature); - esc.esc[i].esc_state = (uint16_t) Motor[i].State; + esc.esc[i].esc_temperature = static_cast(Motor[i].Temperature); + esc.esc[i].esc_state = (uint8_t) Motor[i].State; esc.esc[i].esc_errorcount = (uint16_t) 0; // if motortest is requested - do it... (deprecated in future) diff --git a/src/drivers/px4fmu/CMakeLists.txt b/src/drivers/px4fmu/CMakeLists.txt index d57c0148cf87..f3854322cc09 100644 --- a/src/drivers/px4fmu/CMakeLists.txt +++ b/src/drivers/px4fmu/CMakeLists.txt @@ -38,7 +38,6 @@ px4_add_module( fmu.cpp DEPENDS arch_io_pins - circuit_breaker mixer mixer_module output_limit diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9ffef8e70f13..31dfa6bc2999 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -48,7 +48,6 @@ #include #include #include -#include #include #include #include @@ -166,8 +165,6 @@ class PX4FMU : public cdev::CDev, public ModuleBase, public OutputModule private: MixingOutput _mixing_output{*this, MixingOutput::SchedulingPolicy::Auto, true}; - static constexpr uint8_t MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS; - Mode _mode{MODE_NONE}; unsigned _pwm_default_rate{50}; diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp index 27970cc02b19..c5c1068baf61 100644 --- a/src/drivers/tap_esc/tap_esc.cpp +++ b/src/drivers/tap_esc/tap_esc.cpp @@ -563,7 +563,6 @@ void TAP_ESC::cycle() if (feed_back_data.channelID < esc_status_s::CONNECTED_ESC_MAX) { _esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed; _esc_feedback.esc[feed_back_data.channelID].esc_state = feed_back_data.ESCStatus; - _esc_feedback.esc[feed_back_data.channelID].esc_vendor = esc_status_s::ESC_VENDOR_TAP; _esc_feedback.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_SERIAL; _esc_feedback.counter++; _esc_feedback.esc_count = num_outputs; diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index 3f14dee2abaf..edee553d82c1 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -113,9 +113,8 @@ publish_gam_message(const uint8_t *buffer) esc.esc_count = 1; esc.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_PPM; - esc.esc[0].esc_vendor = esc_status_s::ESC_VENDOR_GRAUPNER_HOTT; esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; - esc.esc[0].esc_temperature = static_cast(msg.temperature1) - 20.0F; + esc.esc[0].esc_temperature = msg.temperature1 - 20; esc.esc[0].esc_voltage = static_cast((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F; esc.esc[0].esc_current = static_cast((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F; @@ -186,7 +185,7 @@ build_gam_response(uint8_t *buffer, size_t *size) msg.gam_sensor_id = GAM_SENSOR_ID; msg.sensor_text_id = GAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F); + msg.temperature1 = esc.esc[0].esc_temperature + 20; msg.temperature2 = 20; // 0 deg. C. const uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage * 10.0F); diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index c94848da24af..5e631820cec5 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -79,10 +79,18 @@ MixingOutput::~MixingOutput() px4_sem_destroy(&_lock); } -void MixingOutput::printStatus() +void MixingOutput::printStatus() const { perf_print_counter(_control_latency_perf); PX4_INFO("Switched to rate_ctrl work queue: %i", (int)_wq_switched); + PX4_INFO("Mixer loaded: %s", _mixers ? "yes" : "no"); + + PX4_INFO("Channel Configuration:"); + + for (unsigned i = 0; i < MAX_ACTUATORS; i++) { + PX4_INFO("Channel %i: failsafe: %d, disarmed: %d, min: %d, max: %d", i, _failsafe_value[i], _disarmed_value[i], + _min_value[i], _max_value[i]); + } } void MixingOutput::updateParams() @@ -374,6 +382,23 @@ MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS]) */ } +int MixingOutput::reorderedMotorIndex(int index) +{ + if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) { + switch (index) { + case 0: return 1; + + case 1: return 2; + + case 2: return 3; + + case 3: return 0; + } + } + + return index; +} + int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) { const MixingOutput *output = (const MixingOutput *)handle; @@ -420,6 +445,8 @@ void MixingOutput::resetMixer() _mixers = nullptr; _groups_required = 0; } + + _interface.mixerChanged(); } int MixingOutput::loadMixer(const char *buf, unsigned len) @@ -447,6 +474,7 @@ int MixingOutput::loadMixer(const char *buf, unsigned len) PX4_DEBUG("loaded mixers \n%s\n", buf); updateParams(); + _interface.mixerChanged(); return ret; } diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index e60a2847ad55..7d112eceea71 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -66,6 +66,9 @@ class OutputModuleInterface : public px4::ScheduledWorkItem, public ModuleParams virtual void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) = 0; + + /** called whenever the mixer gets updated/reset */ + virtual void mixerChanged() {}; }; /** @@ -97,7 +100,7 @@ class MixingOutput : public ModuleParams ~MixingOutput(); - void printStatus(); + void printStatus() const; /** * Call this regularly from Run(). It will call interface.updateOutputs(). @@ -148,11 +151,18 @@ class MixingOutput : public ModuleParams uint16_t &reverseOutputMask() { return _reverse_output_mask; } uint16_t &failsafeValue(int index) { return _failsafe_value[index]; } - /** Disarmed values: if ramp_up is true, then disarmedValue < minValue needs to hold */ + /** Disarmed values: disarmedValue < minValue needs to hold */ uint16_t &disarmedValue(int index) { return _disarmed_value[index]; } uint16_t &minValue(int index) { return _min_value[index]; } uint16_t &maxValue(int index) { return _max_value[index]; } + /** + * Get the motor index that maps from PX4 convention to the configured one + * @param index motor index in [0, num_motors-1] + * @return reordered motor index. When out of range, the input index is returned + */ + int reorderedMotorIndex(int index); + protected: void updateParams() override; diff --git a/src/lib/mixer_module/params.c b/src/lib/mixer_module/params.c index 26acd17694cf..040c77b74b1d 100644 --- a/src/lib/mixer_module/params.c +++ b/src/lib/mixer_module/params.c @@ -13,6 +13,23 @@ * @value 0 Disabled * @value 1 Roll/Pitch * @value 2 Roll/Pitch/Yaw - * @group Multicopter Attitude Control + * @group Mixer Output */ PARAM_DEFINE_INT32(MC_AIRMODE, 0); + +/** + * Motor Ordering + * + * Determines the motor ordering. This can be used for example in combination with + * a 4-in-1 ESC that assumes a motor ordering which is different from PX4. + * + * ONLY supported for Quads. + * + * When changing this, make sure to test the motor response without props first. + * + * @value 0 PX4 + * @value 1 Betaflight / Cleanflight + * + * @group Mixer Output + */ +PARAM_DEFINE_INT32(MOT_ORDERING, 0); diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index 3c5cad2879bc..1be858bf2c4e 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -66,6 +66,8 @@ using namespace time_literals; #if !defined(PARAM_NO_ORB) # include "uORB/uORB.h" # include "uORB/topics/parameter_update.h" +# include +# include #endif #if defined(FLASH_BASED_PARAMS) @@ -614,6 +616,21 @@ autosave_worker(void *arg) { bool disabled = false; +#if !defined(PARAM_NO_ORB) + + if (!param_get_default_file()) { + // In case we save to FLASH, defer param writes until disarmed, + // as writing to FLASH can stall the entire CPU (in rare cases around 300ms on STM32F7) + uORB::SubscriptionData armed_sub{ORB_ID(actuator_armed)}; + + if (armed_sub.get().armed) { + work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(1_s)); + return; + } + } + +#endif + param_lock_writer(); last_autosave_timestamp = hrt_absolute_time(); autosave_scheduled = false; @@ -961,9 +978,11 @@ param_save_default() const char *filename = param_get_default_file(); if (!filename) { + perf_begin(param_export_perf); param_lock_writer(); res = flash_param_save(false); param_unlock_writer(); + perf_end(param_export_perf); return res; } diff --git a/validation/module_schema.yaml b/validation/module_schema.yaml index 8c31fa2c83af..31d11576a740 100644 --- a/validation/module_schema.yaml +++ b/validation/module_schema.yaml @@ -123,6 +123,12 @@ parameters: max: # Optional maximum value (only for int32 or float) type: number + decimal: + # Optional number of decimal places to display (only for float) + type: number + increment: + # Optional increment step size (for a GCS) (only for float) + type: number unit: # Optional parameter unit (only for int32 or float) # (Extend this list as needed)