From 5bd965930175e990753ba031c4e227cb951a5152 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Sat, 31 Aug 2019 11:55:14 +0200 Subject: [PATCH] stm32 timers: add dshot implementation --- .../stm/stm32_common/include/px4_arch/dshot.h | 87 ++++ .../stm32_common/include/px4_arch/io_timer.h | 43 +- .../stm/stm32_common/io_pins/CMakeLists.txt | 1 + .../src/px4/stm/stm32_common/io_pins/dshot.c | 398 ++++++++++++++++++ .../px4/stm/stm32_common/io_pins/io_timer.c | 92 +++- .../px4/stm/stm32_common/io_pins/pwm_servo.c | 8 +- .../px4/stm/stm32f1/include/px4_arch/dshot.h | 37 ++ .../px4/stm/stm32f4/include/px4_arch/dshot.h | 37 ++ .../px4/stm/stm32f7/include/px4_arch/dshot.h | 37 ++ src/drivers/drv_pwm_output.h | 91 ++++ 10 files changed, 790 insertions(+), 41 deletions(-) create mode 100644 platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/dshot.h create mode 100644 platforms/nuttx/src/px4/stm/stm32_common/io_pins/dshot.c create mode 100644 platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/dshot.h create mode 100644 platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/dshot.h create mode 100644 platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/dshot.h 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 0000000000..2ee27c77b8 --- /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; +} 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 5264f4016e..b18654c26d 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,15 +32,14 @@ ****************************************************************************/ /** - * @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 @@ -53,6 +52,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 +65,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 +80,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 */ @@ -133,4 +140,6 @@ __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 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/io_pins/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/CMakeLists.txt index 401bf06ef5..91c857af34 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/CMakeLists.txt @@ -36,4 +36,5 @@ px4_add_library(arch_io_pins pwm_servo.c pwm_trigger.c input_capture.c + dshot.c ) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/dshot.c new file mode 100644 index 0000000000..04850c96a9 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/dshot.c @@ -0,0 +1,398 @@ +/**************************************************************************** + * + * 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 + + +#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 + +typedef struct dshot_handler_t { + bool init; + uint8_t motors_number; +} dshot_handler_t; + +static dshot_handler_t dshot_handler[DSHOT_TIMERS] = {}; +static uint16_t motor_buffer[MOTORS_NUMBER][ONE_MOTOR_BUFF_SIZE] = {}; +static uint32_t dshot_burst_buffer[DSHOT_TIMERS][ALL_MOTORS_BUF_SIZE] = {}; // TODO: DMA-able memory + +#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) +{ + int ret_val = OK; + + uint8_t timer; + + /* Init channels */ + for (unsigned channel = 0; (channel_mask != 0) && (channel < MAX_TIMER_IO_CHANNELS) && (OK == ret_val); channel++) { + if (channel_mask & (1 << channel)) { + + // First free any that were not DShot mode before + if (-EBUSY == io_timer_is_channel_free(channel)) { + io_timer_free_channel(channel); + } + + int success = io_timer_channel_init(channel, IOTimerChanMode_Dshot, NULL, NULL); + + if (OK == success) { + channel_mask &= ~(1 << channel); + timer = timer_io_channels[channel].timer_index; + + if (io_timers[timer].dshot.dma_base == 0) { // board does not configure dshot + io_timer_free_channel(channel); + ret_val = ERROR; + + } else { + dshot_handler[timer].init = true; + } + + } else { + ret_val = ERROR; + } + } + } + + 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) { + + dshot_dmar_data_prepare(timer, first_motor, dshot_handler[timer].motors_number); + + first_motor += dshot_handler[timer].motors_number; + + uint32_t dshot_data_size = dshot_handler[timer].motors_number * ONE_MOTOR_BUFF_SIZE; + + switch (io_timers[timer].dshot.stream) { + case DShot_Stream0: + rLIFCR(timer) |= DMA_INT_STREAM0_MASK; //clear flags + rS0NDTR(timer) = dshot_data_size; + rS0CR(timer) |= DMA_SCR_EN; + break; + + case DShot_Stream1: + rLIFCR(timer) |= DMA_INT_STREAM1_MASK; //clear flags + rS1NDTR(timer) = dshot_data_size; + rS1CR(timer) |= DMA_SCR_EN; + break; + + case DShot_Stream2: + rLIFCR(timer) |= DMA_INT_STREAM2_MASK; //clear flags + rS2NDTR(timer) = dshot_data_size; + rS2CR(timer) |= DMA_SCR_EN; + break; + + case DShot_Stream3: + rLIFCR(timer) |= DMA_INT_STREAM3_MASK; //clear flags + rS3NDTR(timer) = dshot_data_size; + rS3CR(timer) |= DMA_SCR_EN; + break; + + case DShot_Stream4: + rHIFCR(timer) |= DMA_INT_STREAM4_MASK; //clear flags + rS4NDTR(timer) = dshot_data_size; + rS4CR(timer) |= DMA_SCR_EN; + break; + + case DShot_Stream5: + rHIFCR(timer) |= DMA_INT_STREAM5_MASK; //clear flags + rS5NDTR(timer) = dshot_data_size; + rS5CR(timer) |= DMA_SCR_EN; + break; + + case DShot_Stream6: + rHIFCR(timer) |= DMA_INT_STREAM6_MASK; //clear flags + rS6NDTR(timer) = dshot_data_size; + rS6CR(timer) |= DMA_SCR_EN; + break; + + case DShot_Stream7: + rHIFCR(timer) |= DMA_INT_STREAM7_MASK; //clear flags + rS7NDTR(timer) = dshot_data_size; + rS7CR(timer) |= DMA_SCR_EN; + break; + + default: + break; + } + } + } +} + +/** +* 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][i] = (packet & 0x8000) ? MOTOR_PWM_BIT_1 : MOTOR_PWM_BIT_0; // MSB first + packet <<= 1; + } + + motor_buffer[motor_number][16] = 0; + motor_buffer[motor_number][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) +{ + 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++) { + dshot_burst_buffer[timer][motor_data_index * motors_number + motor_index] = motor_buffer[motor_index + + first_motor][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/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 7a11dec0ad..b309a3e5e1 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,38 @@ static inline void io_timer_set_oneshot_mode(unsigned timer) rEGR(timer) = GTIM_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 +632,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 +644,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 +665,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 +702,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 +814,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 +822,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 +872,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 +910,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 +932,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 9121ea488b..ddd39946d0 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/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/dshot.h b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/dshot.h new file mode 100644 index 0000000000..14d7085e4b --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f1/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/dshot.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/dshot.h new file mode 100644 index 0000000000..14d7085e4b --- /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/stm32f7/include/px4_arch/dshot.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/dshot.h new file mode 100644 index 0000000000..14d7085e4b --- /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/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index aa4e1140c6..714461dae3 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