From 8874e89d53219d82487b635f6c1489923ea78da9 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 01/27] 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 000000000000..2ee27c77b891 --- /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 5264f4016ec0..b18654c26dd7 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 401bf06ef5dd..91c857af34a3 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 000000000000..04850c96a914 --- /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 7a11dec0adbc..b309a3e5e16e 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 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/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 000000000000..14d7085e4b2d --- /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 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/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/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 From 9532a63850ed9eeed143f1d33483edc33b8560e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 31 Aug 2019 12:04:56 +0200 Subject: [PATCH 02/27] fmu: remove unused include and variable --- src/drivers/px4fmu/CMakeLists.txt | 1 - src/drivers/px4fmu/fmu.cpp | 3 --- 2 files changed, 4 deletions(-) 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}; From ca1d68f70f55137d21938e015740848ec30ed3c4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 31 Aug 2019 12:06:16 +0200 Subject: [PATCH 03/27] MOT_ORDERING, MC_AIRMODE: move param def to mixer_module and 'Mixer Output' group --- src/drivers/px4fmu/px4fmu_params.c | 52 ------------------------------ src/lib/mixer_module/params.c | 19 ++++++++++- 2 files changed, 18 insertions(+), 53 deletions(-) delete mode 100644 src/drivers/px4fmu/px4fmu_params.c diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c deleted file mode 100644 index 30f2867c5afa..000000000000 --- a/src/drivers/px4fmu/px4fmu_params.c +++ /dev/null @@ -1,52 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * 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/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); From f253ee0438b03b17ccab31590b807199e780ff7f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 31 Aug 2019 12:12:04 +0200 Subject: [PATCH 04/27] dshot: add dshot driver Supports all DShot modes, but no telemetry yet. It includes the modes and capture configuration from FMU so that it serves as drop-in replacement. --- src/drivers/dshot/CMakeLists.txt | 44 + src/drivers/dshot/dshot.cpp | 1303 ++++++++++++++++++++++++++++++ src/drivers/dshot/params.c | 69 ++ 3 files changed, 1416 insertions(+) create mode 100644 src/drivers/dshot/CMakeLists.txt create mode 100644 src/drivers/dshot/dshot.cpp create mode 100644 src/drivers/dshot/params.c diff --git a/src/drivers/dshot/CMakeLists.txt b/src/drivers/dshot/CMakeLists.txt new file mode 100644 index 000000000000..75934d22fd15 --- /dev/null +++ b/src/drivers/dshot/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# 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 + DEPENDS + arch_io_pins + mixer + mixer_module + output_limit + ) diff --git a/src/drivers/dshot/dshot.cpp b/src/drivers/dshot/dshot.cpp new file mode 100644 index 000000000000..6144682e5230 --- /dev/null +++ b/src/drivers/dshot/dshot.cpp @@ -0,0 +1,1303 @@ +/**************************************************************************** + * + * 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 + + +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; + +private: + enum class DShotConfig { + Disabled = 0, + DShot150 = 150, + DShot300 = 300, + DShot600 = 600, + DShot1200 = 1200, + }; + + MixingOutput _mixing_output{*this, MixingOutput::SchedulingPolicy::Auto, false, false}; + + Mode _mode{MODE_NONE}; + + uORB::Subscription _param_sub{ORB_ID(parameter_update)}; + + + 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 + ) +}; + +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); +} + +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; + } + + up_dshot_init(_output_mask, dshot_frequency); + _outputs_initialized = true; + } + + up_dshot_arm(on); +} + + +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 (stop_motors) { + + for (int i = 0; i < (int)num_outputs; i++) { + up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index); + } + } 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); + } + } + + 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) { + _outputs_on = outputs_on; + update_dshot_out_state(outputs_on); + } + + if (_param_sub.updated()) { + update_params(); + } + + // 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]; + + /* start the FMU if not running */ + 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. + +)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_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(); + + 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/params.c b/src/drivers/dshot/params.c new file mode 100644 index 000000000000..d5f0b9708b38 --- /dev/null +++ b/src/drivers/dshot/params.c @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Configure DShot + * + * This enabled/disabled DShot. The different modes define different + * speeds, for example DShot150 = 150kb/s. Not all ESCs support all modes. + * + * Note: this enabled DShot on the FMU outputs. For boards with an IO it is the + * AUX outputs. + * + * @value 0 Disable (use PWM/Oneshot) + * @value 150 DShot150 + * @value 300 DShot300 + * @value 600 DShot600 + * @value 1200 DShot1200 + * @reboot_required true + * @group DShot + */ +PARAM_DEFINE_INT32(DSHOT_CONFIG, 0); + +/** + * Minimum DShot Motor Output + * + * 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. + * + * @unit norm + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group DShot + */ +PARAM_DEFINE_FLOAT(DSHOT_MIN, 0.055f); + + From 929d49e0972a85345139d5e8a673bd1e80d17a1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 31 Aug 2019 12:12:56 +0200 Subject: [PATCH 05/27] ROMFS: add dshot startup support --- ROMFS/px4fmu_common/init.d/rc.interface | 162 +++++++++++++----------- ROMFS/px4fmu_common/init.d/rcS | 2 - 2 files changed, 89 insertions(+), 75 deletions(-) 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 From 474840c49a705e35e45fb9e350b08da00e3b22dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 31 Aug 2019 12:03:41 +0200 Subject: [PATCH 06/27] omnibusf4sd: add dshot timer config --- boards/omnibus/f4sd/default.cmake | 1 + .../omnibus/f4sd/nuttx-config/include/board.h | 2 +- .../omnibus/f4sd/nuttx-config/nsh/defconfig | 1 - boards/omnibus/f4sd/src/board_config.h | 2 ++ boards/omnibus/f4sd/src/timer_config.c | 19 +++++++++++++++++-- 5 files changed, 21 insertions(+), 4 deletions(-) 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 From a3c4b63744a63475f2f45e7f731af833152d91b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= Date: Sat, 31 Aug 2019 12:03:08 +0200 Subject: [PATCH 07/27] fmu-v4: add dshot timer config --- boards/px4/fmu-v4/default.cmake | 1 + boards/px4/fmu-v4/nuttx-config/include/board.h | 4 ++-- boards/px4/fmu-v4/nuttx-config/nsh/defconfig | 1 - .../px4/fmu-v4/nuttx-config/stackcheck/defconfig | 1 - boards/px4/fmu-v4/src/board_config.h | 3 +++ boards/px4/fmu-v4/src/timer_config.c | 14 ++++++++++++++ 6 files changed, 20 insertions(+), 4 deletions(-) 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 */ + } } }; From 87fec4cc8c985402273d67bb4508e6f51b18461e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 31 Aug 2019 12:29:49 +0200 Subject: [PATCH 08/27] beta75x airframe: enable DShot600 --- ROMFS/px4fmu_common/init.d/airframes/4041_beta75x | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) 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 From 10156f306c92867a60a5d3056bf4097b0e1a777f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 2 Sep 2019 15:40:19 +0200 Subject: [PATCH 09/27] px4_micro_hal: add PX4_ARCH_DCACHE_LINESIZE definition --- .../src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h | 3 ++- .../nuttx/src/px4/stm/stm32f4/include/px4_arch/micro_hal.h | 1 + .../nuttx/src/px4/stm/stm32f4/include/px4_arch/px4io_serial.h | 1 - .../nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h | 1 + .../nuttx/src/px4/stm/stm32f7/include/px4_arch/px4io_serial.h | 1 - 5 files changed, 4 insertions(+), 3 deletions(-) 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/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/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" From 78e9837570e0bef1a1bd562608618c2677d0653a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 2 Sep 2019 15:41:59 +0200 Subject: [PATCH 10/27] dshot: handle dcache, flush cache before DMA transfer --- .../src/px4/stm/stm32_common/io_pins/dshot.c | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) 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 index 04850c96a914..be302400e4f7 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/dshot.c @@ -38,6 +38,7 @@ #else #include +#include #include #include #include @@ -116,9 +117,13 @@ typedef struct dshot_handler_t { 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) + 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 +static uint8_t dshot_burst_buffer[DSHOT_TIMERS][DMA_ALIGN_UP(sizeof(uint32_t)*ALL_MOTORS_BUF_SIZE)] __attribute__(( + aligned(PX4_ARCH_DCACHE_LINESIZE))) = {}; #ifdef BOARD_DSHOT_MOTOR_ASSIGNMENT static const uint8_t motor_assignment[MOTORS_NUMBER] = BOARD_DSHOT_MOTOR_ASSIGNMENT; @@ -270,6 +275,10 @@ void up_dshot_trigger(void) dshot_dmar_data_prepare(timer, first_motor, dshot_handler[timer].motors_number); + // Flush cache so DMA sees the data + up_clean_dcache((uintptr_t)dshot_burst_buffer[timer], + (uintptr_t)dshot_burst_buffer[timer] + DMA_ALIGN_UP(sizeof(uint32_t)*ALL_MOTORS_BUF_SIZE)); + first_motor += dshot_handler[timer].motors_number; uint32_t dshot_data_size = dshot_handler[timer].motors_number * ONE_MOTOR_BUFF_SIZE; @@ -381,9 +390,14 @@ void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry) void dshot_dmar_data_prepare(uint8_t timer, uint8_t first_motor, uint8_t motors_number) { + // 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" + uint32_t *buffer = (uint32_t *)dshot_burst_buffer[timer]; +#pragma GCC diagnostic pop + 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 + + buffer[motor_data_index * motors_number + motor_index] = motor_buffer[motor_index + first_motor][motor_data_index]; } } From 5ff8193614457c50ace2bc6c32ccfe1359a96d54 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 2 Sep 2019 15:46:38 +0200 Subject: [PATCH 11/27] kakutef7: enable dshot --- boards/holybro/kakutef7/default.cmake | 1 + .../kakutef7/nuttx-config/include/board.h | 5 ---- .../kakutef7/nuttx-config/nsh/defconfig | 9 +++++- boards/holybro/kakutef7/src/timer_config.c | 30 +++++++++++++++++-- 4 files changed, 37 insertions(+), 8 deletions(-) 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/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/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 */ + } } }; From 2370c8580f2dce147d73078c911e4514c1ab28f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 2 Sep 2019 16:06:05 +0200 Subject: [PATCH 12/27] dshot: move implementation to a separate directory & library So that the static memory overhead is not added to targets w/o dshot --- .../px4/stm/stm32_common/dshot/CMakeLists.txt | 36 +++++++++++++++++++ .../stm32_common/{io_pins => dshot}/dshot.c | 0 .../stm/stm32_common/io_pins/CMakeLists.txt | 1 - .../nuttx/src/px4/stm/stm32f4/CMakeLists.txt | 1 + .../nuttx/src/px4/stm/stm32f7/CMakeLists.txt | 1 + src/drivers/dshot/CMakeLists.txt | 1 + 6 files changed, 39 insertions(+), 1 deletion(-) create mode 100644 platforms/nuttx/src/px4/stm/stm32_common/dshot/CMakeLists.txt rename platforms/nuttx/src/px4/stm/stm32_common/{io_pins => dshot}/dshot.c (100%) 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/io_pins/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c similarity index 100% rename from platforms/nuttx/src/px4/stm/stm32_common/io_pins/dshot.c rename to platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c 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 91c857af34a3..401bf06ef5dd 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,5 +36,4 @@ 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/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/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/src/drivers/dshot/CMakeLists.txt b/src/drivers/dshot/CMakeLists.txt index 75934d22fd15..6f98950d16d4 100644 --- a/src/drivers/dshot/CMakeLists.txt +++ b/src/drivers/dshot/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_module( dshot.cpp DEPENDS arch_io_pins + arch_dshot mixer mixer_module output_limit From df871ae0451f1ed1a288f822c823b990c98f7d85 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 2 Sep 2019 18:20:01 +0200 Subject: [PATCH 13/27] esc_status: trim the message and remove unused fields --- msg/esc_report.msg | 6 ++---- msg/esc_status.msg | 5 ----- src/drivers/mkblctrl/mkblctrl.cpp | 6 ++---- src/drivers/tap_esc/tap_esc.cpp | 1 - src/drivers/telemetry/hott/messages.cpp | 5 ++--- 5 files changed, 6 insertions(+), 17 deletions(-) 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..38472800f963 100644 --- a/msg/esc_status.msg +++ b/msg/esc_status.msg @@ -1,11 +1,6 @@ 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 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/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); From d3e718c37a1ef26a7b77bff48daef76ed082edb1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 2 Sep 2019 18:27:51 +0200 Subject: [PATCH 14/27] module_schema.yaml: add 'decimal' and 'increment' for floats --- validation/module_schema.yaml | 6 ++++++ 1 file changed, 6 insertions(+) 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) From fb82d9408077e0372fca67088bcf7ee23301622e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 2 Sep 2019 19:22:19 +0200 Subject: [PATCH 15/27] dshot: add telemetry and publish esc_status message --- msg/esc_status.msg | 3 +- src/drivers/dshot/CMakeLists.txt | 3 + src/drivers/dshot/dshot.cpp | 135 +++++++++- src/drivers/dshot/module.yaml | 54 ++++ src/drivers/dshot/telemetry.cpp | 265 ++++++++++++++++++++ src/drivers/dshot/{params.c => telemetry.h} | 98 +++++--- src/lib/mixer_module/mixer_module.cpp | 20 ++ src/lib/mixer_module/mixer_module.hpp | 12 +- 8 files changed, 553 insertions(+), 37 deletions(-) create mode 100644 src/drivers/dshot/module.yaml create mode 100644 src/drivers/dshot/telemetry.cpp rename src/drivers/dshot/{params.c => telemetry.h} (54%) diff --git a/msg/esc_status.msg b/msg/esc_status.msg index 38472800f963..a12b890fb8e3 100644 --- a/msg/esc_status.msg +++ b/msg/esc_status.msg @@ -3,9 +3,10 @@ uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. Current (Q2/2013) 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/src/drivers/dshot/CMakeLists.txt b/src/drivers/dshot/CMakeLists.txt index 6f98950d16d4..493419c2935d 100644 --- a/src/drivers/dshot/CMakeLists.txt +++ b/src/drivers/dshot/CMakeLists.txt @@ -36,10 +36,13 @@ px4_add_module( 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 index 6144682e5230..2004ed044d28 100644 --- a/src/drivers/dshot/dshot.cpp +++ b/src/drivers/dshot/dshot.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -60,6 +61,9 @@ #include #include #include +#include + +#include "telemetry.h" using namespace time_literals; @@ -148,6 +152,8 @@ class DShotOutput : public cdev::CDev, public ModuleBase, public Ou void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; + void mixerChanged() override; + private: enum class DShotConfig { Disabled = 0, @@ -157,8 +163,22 @@ class DShotOutput : public cdev::CDev, public ModuleBase, public Ou DShot1200 = 1200, }; + 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); + MixingOutput _mixing_output{*this, MixingOutput::SchedulingPolicy::Auto, false, false}; + Telemetry *_telemetry{nullptr}; + static char _telemetry_device[20]; + static px4::atomic_bool _request_telemetry_init; + Mode _mode{MODE_NONE}; uORB::Subscription _param_sub{ORB_ID(parameter_update)}; @@ -188,10 +208,14 @@ class DShotOutput : public cdev::CDev, public ModuleBase, public Ou DEFINE_PARAMETERS( (ParamInt) _param_dshot_config, - (ParamFloat) _param_dshot_min + (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), @@ -214,6 +238,7 @@ DShotOutput::~DShotOutput() perf_free(_cycle_perf); perf_free(_cycle_interval_perf); + delete _telemetry; } int @@ -484,6 +509,79 @@ DShotOutput::update_dshot_out_state(bool on) up_dshot_arm(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; +} + +void DShotOutput::mixerChanged() +{ + updateTelemetryNumMotors(); +} void DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) @@ -494,6 +592,10 @@ void DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS int requested_telemetry_index = -1; + if (_telemetry) { + requested_telemetry_index = _mixing_output.reorderedMotorIndex(_telemetry->handler.getRequestMotorIndex()); + } + if (stop_motors) { for (int i = 0; i < (int)num_outputs; i++) { @@ -534,10 +636,24 @@ DShotOutput::Run() update_dshot_out_state(outputs_on); } + if (_telemetry) { + int telem_update = _telemetry->handler.update(); + + 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); + } + // check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) _mixing_output.updateSubscriptions(true); @@ -1108,6 +1224,17 @@ 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; + } + /* start the FMU if not running */ if (!is_running()) { int ret = DShotOutput::task_spawn(argc, argv); @@ -1234,6 +1361,9 @@ to use DShot as ESC communication protocol instead of 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); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; @@ -1290,6 +1420,9 @@ int DShotOutput::print_status() perf_print_counter(_cycle_perf); perf_print_counter(_cycle_interval_perf); _mixing_output.printStatus(); + if (_telemetry) { + PX4_INFO("telemetry on: %s", _telemetry_device); + } return 0; } 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..50823a318fdd --- /dev/null +++ b/src/drivers/dshot/telemetry.cpp @@ -0,0 +1,265 @@ +/**************************************************************************** + * + * 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; + } + + _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::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) { + PX4_DEBUG("ESC telemetry timeout for motor %i (frame pos=%i)", _current_motor_index_request, _frame_position); + requestNextMotor(); + } + + 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; ++i) { + if (decodeByte(buf[i])) { + ret = _current_motor_index_request; + requestNextMotor(); + } + } + + return ret; +} + +bool DShotTelemetry::decodeByte(uint8_t byte) +{ + _frame_buffer[_frame_position++] = byte; + + 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); + } + + return true; + } + + return false; +} + +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; +} + +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/params.c b/src/drivers/dshot/telemetry.h similarity index 54% rename from src/drivers/dshot/params.c rename to src/drivers/dshot/telemetry.h index d5f0b9708b38..4373a8079d00 100644 --- a/src/drivers/dshot/params.c +++ b/src/drivers/dshot/telemetry.h @@ -31,39 +31,69 @@ * ****************************************************************************/ -/** - * Configure DShot - * - * This enabled/disabled DShot. The different modes define different - * speeds, for example DShot150 = 150kb/s. Not all ESCs support all modes. - * - * Note: this enabled DShot on the FMU outputs. For boards with an IO it is the - * AUX outputs. - * - * @value 0 Disable (use PWM/Oneshot) - * @value 150 DShot150 - * @value 300 DShot300 - * @value 600 DShot600 - * @value 1200 DShot1200 - * @reboot_required true - * @group DShot - */ -PARAM_DEFINE_INT32(DSHOT_CONFIG, 0); - -/** - * Minimum DShot Motor Output - * - * 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. - * - * @unit norm - * @min 0 - * @max 1 - * @decimal 2 - * @increment 0.01 - * @group DShot - */ -PARAM_DEFINE_FLOAT(DSHOT_MIN, 0.055f); +#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] + }; + + ~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, >= 0 for the motor index. Use @latestESCData() to get the data. + */ + int update(); + + /** + * 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; } + +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(); + + bool decodeByte(uint8_t byte); + + 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}; +}; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index c94848da24af..f1cdc824faac 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -374,6 +374,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 +437,8 @@ void MixingOutput::resetMixer() _mixers = nullptr; _groups_required = 0; } + + _interface.mixerChanged(); } int MixingOutput::loadMixer(const char *buf, unsigned len) @@ -447,6 +466,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..67bd21728580 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() {}; }; /** @@ -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; From 5e0b11210fc395d6d904790e0b1e724b785f8c3a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 2 Sep 2019 20:40:56 +0200 Subject: [PATCH 16/27] dshot: add support to send commands via CLI This enables the possibility to permanently reverse motor directions :) --- src/drivers/dshot/dshot.cpp | 149 +++++++++++++++++++++++++++++++++++- 1 file changed, 148 insertions(+), 1 deletion(-) diff --git a/src/drivers/dshot/dshot.cpp b/src/drivers/dshot/dshot.cpp index 2004ed044d28..52b761c1f4a8 100644 --- a/src/drivers/dshot/dshot.cpp +++ b/src/drivers/dshot/dshot.cpp @@ -154,6 +154,15 @@ class DShotOutput : public cdev::CDev, public ModuleBase, public Ou 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); + private: enum class DShotConfig { Disabled = 0, @@ -163,6 +172,15 @@ class DShotOutput : public cdev::CDev, public ModuleBase, public Ou 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)}; @@ -183,6 +201,8 @@ class DShotOutput : public cdev::CDev, public ModuleBase, public Ou uORB::Subscription _param_sub{ORB_ID(parameter_update)}; + Command _current_command; + px4::atomic _new_command{nullptr}; unsigned _num_outputs{0}; int _class_instance{-1}; @@ -578,6 +598,29 @@ void DShotOutput::handleNewTelemetryData(int motor_index, const DShotTelemetry:: _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()) { + usleep(1000); + } + + return 0; +} + void DShotOutput::mixerChanged() { updateTelemetryNumMotors(); @@ -598,13 +641,28 @@ void DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS 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++) { - up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index); + 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) { @@ -654,6 +712,16 @@ DShotOutput::Run() _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); @@ -1235,6 +1303,53 @@ int DShotOutput::custom_command(int argc, char *argv[]) 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); + } + } + + /* start the FMU if not running */ if (!is_running()) { int ret = DShotOutput::task_spawn(argc, argv); @@ -1333,6 +1448,16 @@ int DShotOutput::print_usage(const char *reason) 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"); @@ -1364,6 +1489,28 @@ to use DShot as ESC communication protocol instead of PWM. 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_DEFAULT_COMMANDS(); return 0; From 1407983978c8ebe8c1302c0df1272d5a3a89230f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 5 Sep 2019 14:58:16 +0200 Subject: [PATCH 17/27] dshot telemetry: retrieve & print ESC info with 'dshot esc_info' CLI command --- src/drivers/dshot/dshot.cpp | 95 +++++++++++++- src/drivers/dshot/telemetry.cpp | 222 +++++++++++++++++++++++++++++++- src/drivers/dshot/telemetry.h | 48 ++++++- 3 files changed, 353 insertions(+), 12 deletions(-) diff --git a/src/drivers/dshot/dshot.cpp b/src/drivers/dshot/dshot.cpp index 52b761c1f4a8..ee6105e465c3 100644 --- a/src/drivers/dshot/dshot.cpp +++ b/src/drivers/dshot/dshot.cpp @@ -163,6 +163,10 @@ class DShotOutput : public cdev::CDev, public ModuleBase, public Ou */ 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, @@ -191,12 +195,17 @@ class DShotOutput : public cdev::CDev, public ModuleBase, public Ou 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)}; @@ -615,12 +624,53 @@ int DShotOutput::sendCommandThreadSafe(dshot_command_t command, int num_repetiti // wait until main thread processed it while (_new_command.load()) { - usleep(1000); + 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(); @@ -636,7 +686,14 @@ void DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS int requested_telemetry_index = -1; if (_telemetry) { - requested_telemetry_index = _mixing_output.reorderedMotorIndex(_telemetry->handler.getRequestMotorIndex()); + // 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) { @@ -697,7 +754,14 @@ DShotOutput::Run() if (_telemetry) { int telem_update = _telemetry->handler.update(); - if (telem_update >= 0) { + // 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()); } } @@ -1349,8 +1413,27 @@ int DShotOutput::custom_command(int argc, char *argv[]) } } + 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; + } + - /* start the FMU if not running */ if (!is_running()) { int ret = DShotOutput::task_spawn(argc, argv); @@ -1511,6 +1594,9 @@ After saving, the reversed direction will be regarded as the normal one. So to r 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; @@ -1569,6 +1655,7 @@ int DShotOutput::print_status() _mixing_output.printStatus(); if (_telemetry) { PX4_INFO("telemetry on: %s", _telemetry_device); + _telemetry->handler.printStatus(); } return 0; diff --git a/src/drivers/dshot/telemetry.cpp b/src/drivers/dshot/telemetry.cpp index 50823a318fdd..c263bb330112 100644 --- a/src/drivers/dshot/telemetry.cpp +++ b/src/drivers/dshot/telemetry.cpp @@ -59,6 +59,8 @@ int DShotTelemetry::init(const char *uart_device) return -errno; } + _num_timeouts = 0; + _num_successful_responses = 0; _current_motor_index_request = -1; return setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE); } @@ -71,6 +73,20 @@ void DShotTelemetry::deinit() } } +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) { @@ -94,8 +110,18 @@ int DShotTelemetry::update() const hrt_abstime now = hrt_absolute_time(); if (_current_request_start > 0 && now - _current_request_start > 30_ms) { - PX4_DEBUG("ESC telemetry timeout for motor %i (frame pos=%i)", _current_motor_index_request, _frame_position); + 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; @@ -107,19 +133,38 @@ int DShotTelemetry::update() int num_read = read(_uart_fd, buf, buf_length); ret = -1; - for (int i = 0; i < num_read; ++i) { - if (decodeByte(buf[i])) { - ret = _current_motor_index_request; - requestNextMotor(); + 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 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); @@ -136,6 +181,8 @@ bool DShotTelemetry::decodeByte(uint8_t byte) 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; @@ -144,6 +191,12 @@ bool DShotTelemetry::decodeByte(uint8_t byte) 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; @@ -186,6 +239,163 @@ int DShotTelemetry::getRequestMotorIndex() 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; diff --git a/src/drivers/dshot/telemetry.h b/src/drivers/dshot/telemetry.h index 4373a8079d00..a5c549c47676 100644 --- a/src/drivers/dshot/telemetry.h +++ b/src/drivers/dshot/telemetry.h @@ -47,6 +47,17 @@ class DShotTelemetry 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); @@ -58,10 +69,22 @@ class DShotTelemetry /** * Read telemetry from the UART (non-blocking) and handle timeouts. - * @return -1 if no update, >= 0 for the motor index. Use @latestESCData() to get the data. + * @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 @@ -70,6 +93,15 @@ class DShotTelemetry 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; @@ -82,7 +114,13 @@ class DShotTelemetry void requestNextMotor(); - bool decodeByte(uint8_t byte); + /** + * 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); @@ -96,4 +134,10 @@ class DShotTelemetry 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}; }; From ac6de52d8c1b0c17f0370c44f4bdc8367a75db76 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sat, 7 Sep 2019 14:23:32 +0200 Subject: [PATCH 18/27] dshot: reduce static buffer size And handle failures of up_dshot_init(). On Omnibus: reduces memory usage if dshot is enabled by ~1.0KB. The buffer is roughly 1KB in size. --- boards/av/x-v1/src/board_config.h | 2 + boards/holybro/kakutef7/src/board_config.h | 2 + boards/mro/ctrl-zero-f7/src/board_config.h | 2 + boards/nxp/fmuk66-v3/src/board_config.h | 2 + boards/px4/fmu-v5/src/board_config.h | 2 + boards/px4/fmu-v5x/src/board_config.h | 2 + boards/px4/io-v2/src/board_config.h | 2 + .../nxp/kinetis/include/px4_arch/io_timer.h | 6 +- .../src/px4/stm/stm32_common/dshot/dshot.c | 83 ++++++++++++------- .../stm/stm32_common/include/px4_arch/dshot.h | 2 +- .../stm32_common/include/px4_arch/io_timer.h | 6 +- src/drivers/dshot/dshot.cpp | 14 +++- 12 files changed, 91 insertions(+), 34 deletions(-) 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/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/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/px4/fmu-v5/src/board_config.h b/boards/px4/fmu-v5/src/board_config.h index 1e6fd32f2afa..5fd9a9e31507 100644 --- a/boards/px4/fmu-v5/src/board_config.h +++ b/boards/px4/fmu-v5/src/board_config.h @@ -671,6 +671,8 @@ #define BOARD_ENABLE_CONSOLE_BUFFER +#define BOARD_NUM_IO_TIMERS 5 + __BEGIN_DECLS /**************************************************************************************************** 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/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/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index be302400e4f7..4fcb37e541f5 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -111,6 +111,7 @@ #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; @@ -119,11 +120,13 @@ typedef struct 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[MOTORS_NUMBER][ONE_MOTOR_BUFF_SIZE] = {}; -static uint8_t dshot_burst_buffer[DSHOT_TIMERS][DMA_ALIGN_UP(sizeof(uint32_t)*ALL_MOTORS_BUF_SIZE)] __attribute__(( - aligned(PX4_ARCH_DCACHE_LINESIZE))) = {}; +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; @@ -134,11 +137,37 @@ int dshot_setup_stream_registers(uint32_t timer); int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) { - int ret_val = OK; + // 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; - uint8_t timer; + 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)) { @@ -151,7 +180,7 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) if (OK == success) { channel_mask &= ~(1 << channel); - timer = timer_io_channels[channel].timer_index; + uint8_t 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); @@ -189,7 +218,7 @@ int dshot_setup_stream_registers(uint32_t timer) 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])); + rS0M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); rS0FCR(timer) &= 0x0; /* Disable FIFO */ break; @@ -199,7 +228,7 @@ int dshot_setup_stream_registers(uint32_t timer) 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])); + rS1M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); rS1FCR(timer) &= 0x0; /* Disable FIFO */ break; @@ -208,7 +237,7 @@ int dshot_setup_stream_registers(uint32_t timer) 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])); + rS2M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); rS2FCR(timer) &= 0x0; /* Disable FIFO */ break; @@ -217,7 +246,7 @@ int dshot_setup_stream_registers(uint32_t timer) 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])); + rS3M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); rS3FCR(timer) &= 0x0; /* Disable FIFO */ break; @@ -226,7 +255,7 @@ int dshot_setup_stream_registers(uint32_t timer) 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])); + rS4M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); rS4FCR(timer) &= 0x0; /* Disable FIFO */ break; @@ -235,7 +264,7 @@ int dshot_setup_stream_registers(uint32_t timer) 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])); + rS5M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); rS5FCR(timer) &= 0x0; /* Disable FIFO */ break; @@ -244,7 +273,7 @@ int dshot_setup_stream_registers(uint32_t timer) 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])); + rS6M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); rS6FCR(timer) &= 0x0; /* Disable FIFO */ break; @@ -253,7 +282,7 @@ int dshot_setup_stream_registers(uint32_t timer) 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])); + rS7M0AR(timer) = (uint32_t)(dshot_burst_buffer[timer]); rS7FCR(timer) &= 0x0; /* Disable FIFO */ break; @@ -272,16 +301,16 @@ void up_dshot_trigger(void) 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); + 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] + DMA_ALIGN_UP(sizeof(uint32_t)*ALL_MOTORS_BUF_SIZE)); + (uintptr_t)dshot_burst_buffer[timer] + DSHOT_BURST_BUFFER_SIZE(motors_number)); - first_motor += dshot_handler[timer].motors_number; + first_motor += motors_number; - uint32_t dshot_data_size = dshot_handler[timer].motors_number * ONE_MOTOR_BUFF_SIZE; + uint32_t dshot_data_size = motors_number * ONE_MOTOR_BUFF_SIZE; switch (io_timers[timer].dshot.stream) { case DShot_Stream0: @@ -370,12 +399,13 @@ static void dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool 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 + 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][16] = 0; - motor_buffer[motor_number][17] = 0; + 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) @@ -390,15 +420,12 @@ void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry) void dshot_dmar_data_prepare(uint8_t timer, uint8_t first_motor, uint8_t motors_number) { - // 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" - uint32_t *buffer = (uint32_t *)dshot_burst_buffer[timer]; -#pragma GCC diagnostic pop + 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][motor_data_index]; + buffer[motor_data_index * motors_number + motor_index] = motor_buffer[(motor_index + + first_motor) * ONE_MOTOR_BUFF_SIZE + motor_data_index]; } } } 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 index 2ee27c77b891..a944bc65e37d 100644 --- 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 @@ -83,5 +83,5 @@ typedef struct dshot_conf_t { dshot_dma_channel_t channel; dshot_dma_stream_t stream; uint32_t start_ccr_register; - uint8_t channels_number; + 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 b18654c26dd7..c2164a9812f7 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 @@ -44,7 +44,11 @@ #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 diff --git a/src/drivers/dshot/dshot.cpp b/src/drivers/dshot/dshot.cpp index ee6105e465c3..651c43859bb8 100644 --- a/src/drivers/dshot/dshot.cpp +++ b/src/drivers/dshot/dshot.cpp @@ -531,11 +531,20 @@ DShotOutput::update_dshot_out_state(bool on) break; } - up_dshot_init(_output_mask, dshot_frequency); + int ret = up_dshot_init(_output_mask, dshot_frequency); + + if (ret != 0) { + PX4_ERR("up_dshot_init failed (%i)", ret); + return; + } + _outputs_initialized = true; } - up_dshot_arm(on); + if (_outputs_initialized) { + up_dshot_arm(on); + _outputs_on = on; + } } void DShotOutput::updateTelemetryNumMotors() @@ -747,7 +756,6 @@ DShotOutput::Run() bool outputs_on = _mixing_output.armed().armed || _mixing_output.mixers(); if (_outputs_on != outputs_on) { - _outputs_on = outputs_on; update_dshot_out_state(outputs_on); } From 343b5c783adbc0be20bdb42295e9531a271bcf1d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sun, 8 Sep 2019 11:18:02 +0200 Subject: [PATCH 19/27] dshot: check if DMA still in progress No other functional change, just restructuring. --- .../src/px4/stm/stm32_common/dshot/dshot.c | 90 +++++++++++-------- 1 file changed, 55 insertions(+), 35 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 4fcb37e541f5..8fb67dfd5dbb 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -301,69 +301,89 @@ void up_dshot_trigger(void) for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) { if (true == dshot_handler[timer].init) { - 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; + 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: - rLIFCR(timer) |= DMA_INT_STREAM0_MASK; //clear flags - rS0NDTR(timer) = dshot_data_size; - rS0CR(timer) |= DMA_SCR_EN; + dma_int_streamx_mask = DMA_INT_STREAM0_MASK; + rxIFCR = &rLIFCR(timer); + rSxNDTR = &rS0NDTR(timer); + rSxCR = &rS0CR(timer); break; case DShot_Stream1: - rLIFCR(timer) |= DMA_INT_STREAM1_MASK; //clear flags - rS1NDTR(timer) = dshot_data_size; - rS1CR(timer) |= DMA_SCR_EN; + dma_int_streamx_mask = DMA_INT_STREAM1_MASK; + rxIFCR = &rLIFCR(timer); + rSxNDTR = &rS1NDTR(timer); + rSxCR = &rS1CR(timer); break; case DShot_Stream2: - rLIFCR(timer) |= DMA_INT_STREAM2_MASK; //clear flags - rS2NDTR(timer) = dshot_data_size; - rS2CR(timer) |= DMA_SCR_EN; + dma_int_streamx_mask = DMA_INT_STREAM2_MASK; + rxIFCR = &rLIFCR(timer); + rSxNDTR = &rS2NDTR(timer); + rSxCR = &rS2CR(timer); break; case DShot_Stream3: - rLIFCR(timer) |= DMA_INT_STREAM3_MASK; //clear flags - rS3NDTR(timer) = dshot_data_size; - rS3CR(timer) |= DMA_SCR_EN; + dma_int_streamx_mask = DMA_INT_STREAM3_MASK; + rxIFCR = &rLIFCR(timer); + rSxNDTR = &rS3NDTR(timer); + rSxCR = &rS3CR(timer); break; case DShot_Stream4: - rHIFCR(timer) |= DMA_INT_STREAM4_MASK; //clear flags - rS4NDTR(timer) = dshot_data_size; - rS4CR(timer) |= DMA_SCR_EN; + dma_int_streamx_mask = DMA_INT_STREAM4_MASK; + rxIFCR = &rHIFCR(timer); + rSxNDTR = &rS4NDTR(timer); + rSxCR = &rS4CR(timer); break; case DShot_Stream5: - rHIFCR(timer) |= DMA_INT_STREAM5_MASK; //clear flags - rS5NDTR(timer) = dshot_data_size; - rS5CR(timer) |= DMA_SCR_EN; + dma_int_streamx_mask = DMA_INT_STREAM5_MASK; + rxIFCR = &rHIFCR(timer); + rSxNDTR = &rS5NDTR(timer); + rSxCR = &rS5CR(timer); break; case DShot_Stream6: - rHIFCR(timer) |= DMA_INT_STREAM6_MASK; //clear flags - rS6NDTR(timer) = dshot_data_size; - rS6CR(timer) |= DMA_SCR_EN; + dma_int_streamx_mask = DMA_INT_STREAM6_MASK; + rxIFCR = &rHIFCR(timer); + rSxNDTR = &rS6NDTR(timer); + rSxCR = &rS6CR(timer); break; case DShot_Stream7: - rHIFCR(timer) |= DMA_INT_STREAM7_MASK; //clear flags - rS7NDTR(timer) = dshot_data_size; - rS7CR(timer) |= DMA_SCR_EN; - break; - 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; + *rSxCR |= DMA_SCR_EN; // Trigger DMA } } } From 5da5586144a15870e38f7f54bfc1f4b87a2c24a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sun, 8 Sep 2019 11:42:04 +0200 Subject: [PATCH 20/27] dshot: update timer generation before DMA request This reloads the timer configuration before triggering DMA. Without that, in rare cases, there were 17 bits sent instead of 16. The 1. bit (1. pulse) was always wrong (too much), the rest of the bits were the correct DShot packet that was meant to be sent. --- platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c | 1 + .../src/px4/stm/stm32_common/include/px4_arch/io_timer.h | 1 + platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c | 6 ++++++ 3 files changed, 8 insertions(+) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 8fb67dfd5dbb..b3ee564f7ef7 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -383,6 +383,7 @@ void up_dshot_trigger(void) 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 } } 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 c2164a9812f7..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 @@ -143,6 +143,7 @@ __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); 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 b309a3e5e16e..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 @@ -492,6 +492,12 @@ 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; From 479cb6df7522f24806d52ee77f74e2b8669580d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Sun, 8 Sep 2019 12:16:59 +0200 Subject: [PATCH 21/27] kakutef7, kopis airframe: enable dshot & telemetry --- .../init.d/airframes/4053_holybro_kopis2 | 17 ++++++++--------- boards/holybro/kakutef7/init/rc.board_extras | 3 ++- 2 files changed, 10 insertions(+), 10 deletions(-) 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/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 From 3aa3943da43ebc6680b62a8b71a55ebce647345c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 20 Sep 2019 17:49:20 +0200 Subject: [PATCH 22/27] mixer_module: extend printf status output --- src/lib/mixer_module/mixer_module.cpp | 10 +++++++++- src/lib/mixer_module/mixer_module.hpp | 2 +- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index f1cdc824faac..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() diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 67bd21728580..7d112eceea71 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -100,7 +100,7 @@ class MixingOutput : public ModuleParams ~MixingOutput(); - void printStatus(); + void printStatus() const; /** * Call this regularly from Run(). It will call interface.updateOutputs(). From 24bd0234080f2ab79eb20f42dccece0fd38e0b42 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 23 Sep 2019 15:16:57 +0200 Subject: [PATCH 23/27] parameters: update perf counter for flash-based param_save_default() --- src/lib/parameters/parameters.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index 3c5cad2879bc..5b613db78bf5 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -961,9 +961,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; } From 940a2240d4d41d0918b98bcfee27969f9774e658 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 24 Sep 2019 10:35:07 +0200 Subject: [PATCH 24/27] dshot: allow boards to enable dshot only on a subset of the timers --- .../src/px4/stm/stm32_common/dshot/dshot.c | 22 +++++++------------ 1 file changed, 8 insertions(+), 14 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index b3ee564f7ef7..ec15036d8256 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -170,28 +170,22 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) 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); } - int success = io_timer_channel_init(channel, IOTimerChanMode_Dshot, NULL, NULL); + ret_val = io_timer_channel_init(channel, IOTimerChanMode_Dshot, NULL, NULL); - if (OK == success) { + if (OK == ret_val) { 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 - io_timer_free_channel(channel); - ret_val = ERROR; - - } else { - dshot_handler[timer].init = true; - } - - } else { - ret_val = ERROR; + dshot_handler[timer].init = true; } } } From e9cd447352ecaae5f2ac196ca78d3bae12356f27 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 24 Sep 2019 19:58:40 +0200 Subject: [PATCH 25/27] fmu-v5: add dshot support But only on the first 4 FMU outputs, as the next ones conflict with px4io serial dma (UART8_RX) RX DMA is disabled on the GPS port as well (conflicts with TIM1). --- boards/px4/fmu-v5/default.cmake | 1 + boards/px4/fmu-v5/nuttx-config/critmonitor/defconfig | 1 - boards/px4/fmu-v5/nuttx-config/irqmonitor/defconfig | 1 - boards/px4/fmu-v5/nuttx-config/nsh/defconfig | 1 - boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig | 1 - boards/px4/fmu-v5/src/board_config.h | 2 ++ boards/px4/fmu-v5/src/timer_config.c | 8 +++++++- 7 files changed, 10 insertions(+), 5 deletions(-) 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 5fd9a9e31507..4b55f960c146 100644 --- a/boards/px4/fmu-v5/src/board_config.h +++ b/boards/px4/fmu-v5/src/board_config.h @@ -673,6 +673,8 @@ #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, From 6b191aa9594ea3444695aa25d9f5358af854be01 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 25 Sep 2019 10:58:19 +0200 Subject: [PATCH 26/27] parameters: defer auto-saving to after disarming for flash-based params This is especially important for DShot, that does not update when the CPU stalls. --- src/lib/parameters/parameters.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index 5b613db78bf5..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; From 69a556180ce28295e3da4e035e6313d86b1c1185 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 26 Sep 2019 18:52:47 +0200 Subject: [PATCH 27/27] fmu-v3: enable DShot Disables RX DMA on TEL4 and IO debug serial port --- boards/px4/fmu-v2/nuttx-config/include/board.h | 1 - boards/px4/fmu-v2/nuttx-config/nsh/defconfig | 2 -- boards/px4/fmu-v2/src/board_config.h | 2 ++ boards/px4/fmu-v2/src/timer_config.c | 17 +++++++++++++++-- boards/px4/fmu-v3/default.cmake | 1 + boards/px4/fmu-v3/nuttx-config/include/board.h | 1 - boards/px4/fmu-v3/nuttx-config/nsh/defconfig | 2 -- .../fmu-v3/nuttx-config/stackcheck/defconfig | 2 -- boards/px4/fmu-v3/src/board_config.h | 2 ++ boards/px4/fmu-v3/src/timer_config.c | 17 +++++++++++++++-- 10 files changed, 35 insertions(+), 12 deletions(-) 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 */ + } } };