Skip to content

Commit

Permalink
fix dshot telemetry info
Browse files Browse the repository at this point in the history
  • Loading branch information
@shanggl
shanggl committed Jun 13, 2023
1 parent 6438a4d commit 30348d9
Show file tree
Hide file tree
Showing 6 changed files with 67 additions and 102 deletions.
16 changes: 7 additions & 9 deletions src/main/drivers/dshot_bitbang.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {};
Expand All @@ -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++;
Expand All @@ -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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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);
}
}

Expand Down
93 changes: 16 additions & 77 deletions src/main/drivers/dshot_bitbang_at32bsp.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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根线通信，需要在输入和输出之间快速切换模式
* 移植思路：
Expand All @@ -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([email protected])
*/

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
Expand Down Expand Up @@ -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
Expand All @@ -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);
Expand All @@ -124,48 +113,31 @@ 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);
}

#ifdef USE_DMA_REGISTER_CACHE

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

Expand All @@ -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);
Expand All @@ -210,53 +180,34 @@ 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);
#else
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


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);
Expand All @@ -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;
Expand Down Expand Up @@ -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);
}
Expand Down
6 changes: 4 additions & 2 deletions src/main/drivers/dshot_bitbang_decode.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,14 @@
* If not, see <http://www.gnu.org/licenses/>.
*/



#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
Loading

0 comments on commit 30348d9

Please sign in to comment.