Igor Mišić
6 years ago
committed by
Beat Küng
10 changed files with 790 additions and 41 deletions
@ -0,0 +1,87 @@
@@ -0,0 +1,87 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (C) 2019 PX4 Development Team. All rights reserved. |
||||
* Author: Igor Misic <igy1000mb@gmail.com> |
||||
* |
||||
* 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 <drivers/drv_pwm_output.h> |
||||
|
||||
#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; |
||||
@ -0,0 +1,398 @@
@@ -0,0 +1,398 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (C) 2019 PX4 Development Team. All rights reserved. |
||||
* Author: Igor Misic <igy1000mb@gmail.com> |
||||
* |
||||
* 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 <px4_config.h> |
||||
#include <stm32_dma.h> |
||||
#include <stm32_tim.h> |
||||
#include <px4_arch/dshot.h> |
||||
#include <px4_arch/io_timer.h> |
||||
#include <drivers/drv_pwm_output.h> |
||||
|
||||
|
||||
#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 |
||||
@ -0,0 +1,37 @@
@@ -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" |
||||
|
||||
@ -0,0 +1,37 @@
@@ -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" |
||||
|
||||
@ -0,0 +1,37 @@
@@ -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" |
||||
|
||||
Loading…
Reference in new issue