From 30348d9061e1cc509a013a2e9873ef17cadfc857 Mon Sep 17 00:00:00 2001 From: EMSR Date: Wed, 14 Jun 2023 01:44:06 +0800 Subject: [PATCH] fix dshot telemetry info --- src/main/drivers/dshot_bitbang.c | 16 ++-- src/main/drivers/dshot_bitbang_at32bsp.c | 93 ++++------------------ src/main/drivers/dshot_bitbang_decode.h | 6 +- src/main/drivers/dshot_bitbang_impl.h | 51 +++++++++--- src/main/drivers/dshot_command.c | 1 + src/main/drivers/pwm_output_dshot_shared.h | 2 +- 6 files changed, 67 insertions(+), 102 deletions(-) diff --git a/src/main/drivers/dshot_bitbang.c b/src/main/drivers/dshot_bitbang.c index eea55082ec..917c47302a 100644 --- a/src/main/drivers/dshot_bitbang.c +++ b/src/main/drivers/dshot_bitbang.c @@ -336,9 +336,9 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor) bbPort_t *bbPort = (bbPort_t *)descriptor->userParam; - bbDMA_Cmd(bbPort, DISABLE); + bbDMA_CmdEnable(bbPort, FALSE); - bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, DISABLE); + bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, FALSE); if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TEIF)) { while (1) {}; @@ -354,7 +354,7 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor) bbPort->inputIrq++; #endif // Disable DMA as telemetry reception is complete - bbDMA_Cmd(bbPort, DISABLE); + bbDMA_CmdEnable(bbPort, FALSE); } else { #ifdef DEBUG_COUNT_INTERRUPT bbPort->outputIrq++; @@ -365,7 +365,7 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor) bbSwitchToInput(bbPort); bbPort->telemetryPending = true; - bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, ENABLE); + bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, TRUE); } } #endif @@ -575,7 +575,7 @@ static bool bbDecodeTelemetry(void) } #endif for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { -#if defined(STM32F4) || defined(AT32F4) +#if defined(STM32F4) || defined(AT32F43x) uint32_t rawValue = decode_bb_bitband( bbMotors[motorIndex].bbPort->portInputBuffer, bbMotors[motorIndex].bbPort->portInputCount, @@ -682,19 +682,17 @@ static void bbUpdateComplete(void) } else #endif { -#if defined(STM32G4) ||defined(AT32F4) // Using circular mode resets the counter one short, so explicitly reload bbSwitchToOutput(bbPort); -#endif } - bbDMA_Cmd(bbPort, ENABLE); + bbDMA_CmdEnable(bbPort, TRUE); } lastSendUs = micros(); for (int i = 0; i < usedMotorPacers; i++) { bbPacer_t *bbPacer = &bbPacers[i]; - bbTIM_DMACmd(bbPacer->tim, bbPacer->dmaSources, ENABLE); + bbTIM_DMACmd(bbPacer->tim, bbPacer->dmaSources, TRUE); } } diff --git a/src/main/drivers/dshot_bitbang_at32bsp.c b/src/main/drivers/dshot_bitbang_at32bsp.c index 2e2a81e07c..89428982fe 100644 --- a/src/main/drivers/dshot_bitbang_at32bsp.c +++ b/src/main/drivers/dshot_bitbang_at32bsp.c @@ -49,7 +49,8 @@ void bbGpioSetup(bbMotor_t *bbMotor) { bbPort_t *bbPort = bbMotor->bbPort; int pinIndex = bbMotor->pinIndex; - /* 初始化对应pin的 modemask、input模式值，output模式寄存器值，供下面switchtoinput、switchtooutput函数用 + /* + *初始化对应pin的 modemask、input模式值，output模式寄存器值，供下面switchtoinput、switchtooutput函数用 * 好处： 直接寄存器操作对应的pin 在输入和输出模式之间快速翻转 * 原因：dshot 是fc 和esc 之间1根线通信，需要在输入和输出之间快速切换模式 * 移植思路： @@ -61,14 +62,12 @@ void bbGpioSetup(bbMotor_t *bbMotor) * bbGpioSetup中先设置对应pin的 mask位，然后在模式切换函数中，直接WRITE_REG操作 * 先写CRL CRH 寄存器，然后判断 是否有IPD、IPU，然后单独写BSRR\BRR * - * mapping : - * bsrr - * + * port by EMSR(shanggl@wo.cn) */ - bbPort->gpioModeMask |= (0x3 << (pinIndex * 2));// 掩模 GPIO_MODER_0 = b11 - bbPort->gpioModeInput |= (GPIO_MODE_INPUT << (pinIndex * 2)); //输入 b00 - bbPort->gpioModeOutput |= (GPIO_MODE_OUTPUT << (pinIndex * 2));//输出 b01 + bbPort->gpioModeMask |= (0x3 << (pinIndex * 2));// mask GPIO_MODER_0 = b11 + bbPort->gpioModeInput |= (GPIO_MODE_INPUT << (pinIndex * 2)); //input b00 + bbPort->gpioModeOutput |= (GPIO_MODE_OUTPUT << (pinIndex * 2));//output b01 #ifdef USE_DSHOT_TELEMETRY @@ -96,13 +95,6 @@ void bbTimerChannelInit(bbPort_t *bbPort) { const timerHardware_t *timhw = bbPort->timhw; -// TIM_OCInitTypeDef TIM_OCStruct; -// TIM_OCStructInit(&TIM_OCStruct); -// TIM_OCStruct.TIM_OCMode = TIM_OCMode_PWM1; -// TIM_OCStruct.TIM_OCIdleState = TIM_OCIdleState_Set; -// TIM_OCStruct.TIM_OutputState = TIM_OutputState_Enable; -// TIM_OCStruct.TIM_OCPolarity = TIM_OCPolarity_Low; -// TIM_OCStruct.TIM_Pulse = 10; // Duty doesn't matter, but too value small would make monitor output invalid tmr_output_config_type TIM_OCStruct; tmr_output_default_para_init(&TIM_OCStruct); TIM_OCStruct.oc_mode= TMR_OUTPUT_CONTROL_PWM_MODE_A;//when count up pwm1 eq pwma pwm2 =pwmb @@ -111,10 +103,7 @@ void bbTimerChannelInit(bbPort_t *bbPort) TIM_OCStruct.oc_polarity=TMR_OUTPUT_ACTIVE_LOW; tmr_channel_value_set(timhw->tim, (timhw->channel-1)*2, 10); -// TIM_Cmd(bbPort->timhw->tim, DISABLE); - tmr_counter_enable(bbPort->timhw->tim, DISABLE); -// timerOCInit(timhw->tim, timhw->channel, &TIM_OCStruct); -// timerOCPreloadConfig(timhw->tim, timhw->channel, TIM_OCPreload_Enable); + tmr_counter_enable(bbPort->timhw->tim, FALSE); tmr_output_channel_config(timhw->tim,(timhw->channel-1)*2, &TIM_OCStruct); tmr_channel_enable(timhw->tim, ((timhw->channel-1)*2),TRUE); tmr_output_channel_buffer_enable(timhw->tim, ((timhw->channel-1)*2),TRUE); @@ -124,13 +113,11 @@ void bbTimerChannelInit(bbPort_t *bbPort) IO_t io = IOGetByTag(timhw->tag); IOInit(io, OWNER_DSHOT_BITBANG, 0); IOConfigGPIOAF(io, IOCFG_AF_PP, timhw->alternateFunction); -// TIM_CtrlPWMOutputs(timhw->tim, ENABLE); tmr_output_enable(timhw->tim,TRUE); } #endif // Enable and keep it running -// TIM_Cmd(bbPort->timhw->tim, ENABLE); tmr_counter_enable(bbPort->timhw->tim,TRUE); } @@ -138,34 +125,19 @@ void bbTimerChannelInit(bbPort_t *bbPort) void bbLoadDMARegs(dmaResource_t *dmaResource, dmaRegCache_t *dmaRegCache) { -#if defined(AT32F4) ((DMA_ARCH_TYPE *)dmaResource)->ctrl = dmaRegCache->CCR; //ctrl info ((DMA_ARCH_TYPE *)dmaResource)->dtcnt = dmaRegCache->CNDTR; // dtcnt data count ((DMA_ARCH_TYPE *)dmaResource)->paddr = dmaRegCache->CPAR; //pheriph address ((DMA_ARCH_TYPE *)dmaResource)->maddr = dmaRegCache->CMAR; //Memory address -#else - ((DMA_Stream_TypeDef *)dmaResource)->CR = dmaRegCache->CR; - ((DMA_Stream_TypeDef *)dmaResource)->FCR = dmaRegCache->FCR; - ((DMA_Stream_TypeDef *)dmaResource)->NDTR = dmaRegCache->NDTR; - ((DMA_Stream_TypeDef *)dmaResource)->PAR = dmaRegCache->PAR; - ((DMA_Stream_TypeDef *)dmaResource)->M0AR = dmaRegCache->M0AR; -#endif } static void bbSaveDMARegs(dmaResource_t *dmaResource, dmaRegCache_t *dmaRegCache) { -#if defined(AT32F4) dmaRegCache->CCR=((DMA_ARCH_TYPE *)dmaResource)->ctrl; dmaRegCache->CNDTR=((DMA_ARCH_TYPE *)dmaResource)->dtcnt; dmaRegCache->CPAR=((DMA_ARCH_TYPE *)dmaResource)->paddr ; dmaRegCache->CMAR=((DMA_ARCH_TYPE *)dmaResource)->maddr ; -#else - dmaRegCache->CR = ((DMA_Stream_TypeDef *)dmaResource)->CR; - dmaRegCache->FCR = ((DMA_Stream_TypeDef *)dmaResource)->FCR; - dmaRegCache->NDTR = ((DMA_Stream_TypeDef *)dmaResource)->NDTR; - dmaRegCache->PAR = ((DMA_Stream_TypeDef *)dmaResource)->PAR; - dmaRegCache->M0AR = ((DMA_Stream_TypeDef *)dmaResource)->M0AR; -#endif + } #endif @@ -191,14 +163,12 @@ void bbSwitchToOutput(bbPort_t * bbPort) xDMA_DeInit(dmaResource); xDMA_Init(dmaResource, &bbPort->outputDmaInit); // Needs this, as it is DeInit'ed above... - xDMA_ITConfig(dmaResource, DMA_IT_TCIF, ENABLE); + xDMA_ITConfig(dmaResource, DMA_IT_TCIF, TRUE); #endif // Reinitialize pacer timer for output -// bbPort->timhw->tim->ARR = bbPort->outputARR; - bbPort->timhw->tim->pr = bbPort->outputARR;//maps to pr - + bbPort->timhw->tim->pr = bbPort->outputARR; bbPort->direction = DSHOT_BITBANG_DIRECTION_OUTPUT; dbgPinLo(1); @@ -210,12 +180,10 @@ void bbSwitchToInput(bbPort_t *bbPort) dbgPinHi(1); // Set GPIO to input -//f3\f4\f7\h7 else ATOMIC_BLOCK(NVIC_PRIO_TIMER) { MODIFY_REG(bbPort->gpio->cfgr, bbPort->gpioModeMask, bbPort->gpioModeInput); } // Reinitialize port group DMA for input - dmaResource_t *dmaResource = bbPort->dmaResource; #ifdef USE_DMA_REGISTER_CACHE bbLoadDMARegs(dmaResource, &bbPort->dmaRegInput); @@ -223,20 +191,15 @@ void bbSwitchToInput(bbPort_t *bbPort) xDMA_DeInit(dmaResource); xDMA_Init(dmaResource, &bbPort->inputDmaInit); // Needs this, as it is DeInit'ed above... - xDMA_ITConfig(dmaResource, DMA_IT_TCIF, ENABLE); + xDMA_ITConfig(dmaResource, DMA_IT_TCIF, TRUE); #endif // Reinitialize pacer timer for input - -// bbPort->timhw->tim->CNT = 0; bbPort->timhw->tim->cval = 0; -// bbPort->timhw->tim->ARR = bbPort->inputARR; bbPort->timhw->tim->pr = bbPort->inputARR; - bbDMA_Cmd(bbPort, ENABLE); - + bbDMA_CmdEnable(bbPort, TRUE); bbPort->direction = DSHOT_BITBANG_DIRECTION_INPUT; - dbgPinLo(1); } #endif @@ -244,19 +207,7 @@ void bbSwitchToInput(bbPort_t *bbPort) void bbDMAPreconfigure(bbPort_t *bbPort, uint8_t direction) { -// DMA_InitTypeDef *dmainit = (direction == DSHOT_BITBANG_DIRECTION_OUTPUT) ? &bbPort->outputDmaInit : &bbPort->inputDmaInit; -// -// DMA_StructInit(dmainit); -// -// dmainit->DMA_Mode = DMA_Mode_Normal; //loop mode -// dmainit->DMA_PeripheralInc = DMA_PeripheralInc_Disable; -// dmainit->DMA_MemoryInc = DMA_MemoryInc_Enable; -// -// dmainit->DMA_Channel = bbPort->dmaChannel; //not need -// dmainit->DMA_FIFOMode = DMA_FIFOMode_Enable ; //not need -// dmainit->DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull ; //not need -// dmainit->DMA_MemoryBurst = DMA_MemoryBurst_Single ; -// dmainit->DMA_PeripheralBurst = DMA_PeripheralBurst_Single; + dma_init_type * dmainit = (direction == DSHOT_BITBANG_DIRECTION_OUTPUT) ? &bbPort->outputDmaInit : &bbPort->inputDmaInit; dma_default_para_init(dmainit); @@ -265,8 +216,6 @@ void bbDMAPreconfigure(bbPort_t *bbPort, uint8_t direction) dmainit->peripheral_inc_enable =FALSE; dmainit->memory_inc_enable = TRUE; - //fixme : dma mux enable 在哪里实现？，bbPort->dmaResource 在dshot-bitbang.c bbMortorConfig？ - if (direction == DSHOT_BITBANG_DIRECTION_OUTPUT) { dmainit->priority = DMA_PRIORITY_VERY_HIGH; dmainit->direction = DMA_DIR_MEMORY_TO_PERIPHERAL; @@ -299,33 +248,23 @@ void bbDMAPreconfigure(bbPort_t *bbPort, uint8_t direction) void bbTIM_TimeBaseInit(bbPort_t *bbPort, uint16_t period) { - //fixme: 貌似之有这里用到了 timer 的baseinit 参数，header里是否定义用途也不大啊 -// tmr_base_init_type *init = &bbPort->timeBaseInit; -// -// init->TIM_Prescaler = 0; // Feed raw timerClock -// init->TIM_ClockDivision = TIM_CKD_DIV1; -// init->TIM_CounterMode = TIM_CounterMode_Up; -// init->TIM_Period = period; -// TIM_TimeBaseInit(bbPort->timhw->tim, init); -// TIM_ARRPreloadConfig(bbPort->timhw->tim, ENABLE); tmr_base_init(bbPort->timhw->tim, period,0); tmr_clock_source_div_set(bbPort->timhw->tim,TMR_CLOCK_DIV1); tmr_cnt_dir_set(bbPort->timhw->tim,TMR_COUNT_UP); tmr_period_buffer_enable(bbPort->timhw->tim,TRUE); } -void bbTIM_DMACmd(tmr_type * TIMx, uint16_t TIM_DMASource, FunctionalState NewState) +void bbTIM_DMACmd(tmr_type * TIMx, uint16_t TIM_DMASource, confirm_state NewState) { -// TIM_DMACmd(TIMx, TIM_DMASource, NewState); tmr_dma_request_enable(TIMx, TIM_DMASource, NewState); } void bbDMA_ITConfig(bbPort_t *bbPort) { - xDMA_ITConfig(bbPort->dmaResource, DMA_IT_TCIF, ENABLE); + xDMA_ITConfig(bbPort->dmaResource, DMA_IT_TCIF, TRUE); } -void bbDMA_Cmd(bbPort_t *bbPort, FunctionalState NewState) +void bbDMA_CmdEnable(bbPort_t *bbPort, confirm_state NewState) { xDMA_Cmd(bbPort->dmaResource, NewState); } diff --git a/src/main/drivers/dshot_bitbang_decode.h b/src/main/drivers/dshot_bitbang_decode.h index da546c06d1..cddb5764b8 100644 --- a/src/main/drivers/dshot_bitbang_decode.h +++ b/src/main/drivers/dshot_bitbang_decode.h @@ -18,12 +18,14 @@ * If not, see . */ + + #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY) -#define BB_NOEDGE 0xfffe -#define BB_INVALID 0xffff + uint32_t decode_bb(uint16_t buffer[], uint32_t count, uint32_t mask); uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit); + #endif diff --git a/src/main/drivers/dshot_bitbang_impl.h b/src/main/drivers/dshot_bitbang_impl.h index c101de52d4..4ef79c66d2 100644 --- a/src/main/drivers/dshot_bitbang_impl.h +++ b/src/main/drivers/dshot_bitbang_impl.h @@ -79,7 +79,7 @@ #ifdef USE_HAL_DRIVER #define BB_GPIO_PULLDOWN GPIO_PULLDOWN #define BB_GPIO_PULLUP GPIO_PULLUP -#elif defined(AT32F43x) +#elif defined(AT32F4) #define BB_GPIO_PULLDOWN GPIO_PULL_DOWN #define BB_GPIO_PULLUP GPIO_PULL_UP #else @@ -95,7 +95,7 @@ typedef struct dmaRegCache_s { uint32_t NDTR; uint32_t PAR; uint32_t M0AR; -#elif defined(STM32G4) ||defined(AT32F43x) +#elif defined(STM32G4) || defined(AT32F4) uint32_t CCR; uint32_t CNDTR; uint32_t CPAR; @@ -107,15 +107,12 @@ typedef struct dmaRegCache_s { #endif // Per pacer timer - +#if defined(AT32F4) typedef struct bbPacer_s { tmr_type *tim; uint16_t dmaSources; } bbPacer_t; - -#ifdef AT32F43x -// Feed raw timerClock typedef struct tmr_base_init_s { uint32_t TIM_Prescaler; uint32_t TIM_ClockDivision; @@ -123,13 +120,22 @@ typedef struct tmr_base_init_s { uint32_t TIM_Period; } tmr_base_init_type; -#endif +#else //for stm32 +typedef struct bbPacer_s { + TIM_TypeDef *tim; + uint16_t dmaSources; +} bbPacer_t; +#endif //bbPacer_t // Per GPIO port and timer channel typedef struct bbPort_s { int portIndex; - gpio_type *gpio; +#if defined(AT32F4) + gpio_type * gpio; +#else + GPIO_TypeDef *gpio; +#endif const timerHardware_t *timhw; #ifdef USE_HAL_DRIVER uint32_t llChannel; @@ -138,8 +144,10 @@ typedef struct bbPort_s { uint16_t dmaSource; dmaResource_t *dmaResource; // DMA resource for this port & timer channel - // uint32_t dmaChannel; // DMA channel or peripheral request + uint32_t dmaChannel; // DMA channel or peripheral request +#ifdef AT32F4 uint32_t dmaMuxId; //dma mux id for at32f43x +#endif uint8_t direction; @@ -159,23 +167,33 @@ typedef struct bbPort_s { #ifdef USE_HAL_DRIVER LL_TIM_InitTypeDef timeBaseInit; -#else +#elif defined(AT32F4) tmr_base_init_type timeBaseInit; +#else + TIM_TimeBaseInitTypeDef timeBaseInit; #endif // Output uint16_t outputARR; #ifdef USE_HAL_DRIVER LL_DMA_InitTypeDef outputDmaInit; -#else +#elif defined(AT32F4) dma_init_type outputDmaInit; +#else + DMA_InitTypeDef outputDmaInit; #endif uint32_t *portOutputBuffer; uint32_t portOutputCount; // Input uint16_t inputARR; +#ifdef USE_HAL_DRIVER + LL_DMA_InitTypeDef inputDmaInit; +#elif defined(AT32F4) dma_init_type inputDmaInit; +#else + DMA_InitTypeDef inputDmaInit; +#endif uint16_t *portInputBuffer; uint32_t portInputCount; bool inputActive; @@ -253,7 +271,14 @@ void bbSwitchToOutput(bbPort_t * bbPort); void bbSwitchToInput(bbPort_t * bbPort); void bbTIM_TimeBaseInit(bbPort_t *bbPort, uint16_t period); -void bbTIM_DMACmd(tmr_type* TIMx, uint16_t TIM_DMASource, FunctionalState NewState); void bbDMA_ITConfig(bbPort_t *bbPort); -void bbDMA_Cmd(bbPort_t *bbPort, FunctionalState NewState); int bbDMA_Count(bbPort_t *bbPort); + +#if defined(AT32F4) +void bbTIM_DMACmd(tmr_type * TIMx, uint16_t TIM_DMASource, confirm_state NewState); +void bbDMA_CmdEnable(bbPort_t *bbPort, confirm_state NewState); + +#else +void bbDMA_Cmd(bbPort_t *bbPort, FunctionalState NewState); +void bbTIM_DMACmd(TIM_TypeDef* TIMx, uint16_t TIM_DMASource, FunctionalState NewState); +#endif \ No newline at end of file diff --git a/src/main/drivers/dshot_command.c b/src/main/drivers/dshot_command.c index 7e5c4dcd8c..650ae94993 100644 --- a/src/main/drivers/dshot_command.c +++ b/src/main/drivers/dshot_command.c @@ -222,6 +222,7 @@ void dshotCommandWrite(uint8_t index, uint8_t motorCount, uint8_t command, dshot cmpTimeUs(timeoutUs, micros()) > 0); #endif motorGetVTable().updateInit();//prepare to set data + for (uint8_t i = 0; i < motorDeviceCount(); i++) { motorDmaOutput_t *const motor = getMotorDmaOutput(i); motor->protocolControl.requestTelemetry = true; diff --git a/src/main/drivers/pwm_output_dshot_shared.h b/src/main/drivers/pwm_output_dshot_shared.h index e1e1a9bfdb..8f4e8cbe70 100644 --- a/src/main/drivers/pwm_output_dshot_shared.h +++ b/src/main/drivers/pwm_output_dshot_shared.h @@ -21,7 +21,7 @@ #ifdef USE_DSHOT extern FAST_DATA_ZERO_INIT uint8_t dmaMotorTimerCount; -#if defined(STM32F7) || defined(STM32H7) || defined(AT32F43x) +#if defined(STM32F7) || defined(STM32H7) || defined(AT32F4) extern FAST_DATA_ZERO_INIT motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; extern FAST_DATA_ZERO_INIT motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS]; #else