From fdba688de41c74192079fc78f7de2cbf8de055e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B5=B7=E5=BA=8A=E5=B0=B1=E7=8A=AF=E5=9B=B0?= <11730503+psx123456@user.noreply.gitee.com> Date: Thu, 16 Oct 2025 10:29:10 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E9=93=BE=E8=A1=A8=E6=8F=92?= =?UTF-8?q?=E5=85=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- APP/Device/Device_speed/speed_to_servoMotor.c | 209 ++++++++++++++++-- APP/Device/Device_speed/speed_to_servoMotor.h | 4 +- BSP/Driver/servoMotor/motorCommu.c | 79 +++++-- 3 files changed, 259 insertions(+), 33 deletions(-) diff --git a/APP/Device/Device_speed/speed_to_servoMotor.c b/APP/Device/Device_speed/speed_to_servoMotor.c index 49fe127..02d6e88 100644 --- a/APP/Device/Device_speed/speed_to_servoMotor.c +++ b/APP/Device/Device_speed/speed_to_servoMotor.c @@ -9,14 +9,11 @@ BSP_OS_SEM g_vertSpeedSem; BSP_OS_SEM g_horiSpeedMutex; BSP_OS_SEM g_vertSpeedMutex; - - uint16_t memBuffer[linkListNodeLen * sendDataBufNumber * 2] = {0}; OS_MEM *g_memPtr; ptzServoLinkList g_servoMotorLinkList = {0}; - //发送云台实际转速 void ptz_send_speed(char dev, char speed) { @@ -62,16 +59,71 @@ void ptz_send_speed(char dev, char speed) \param[out] none \retval none */ -void servoLinkListMemPut(linkList *data) +void servoHoriLinkListMemPut(uint8_t motorType) { - if (data == NULL) { - return; + //水平电机 + if (motorType == horiMotorType) { + //当前发送完成的数据为高优先级链表中的数据 + if (g_servoMotorLinkList.horiMotor.linkListNum == highPriority) { + if (g_servoMotorLinkList.horiMotor.LinkListHead_H != NULL) { + linkList *ptr; + ptr = g_servoMotorLinkList.horiMotor.LinkListHead_H; + if (ptr->next != NULL) { + g_servoMotorLinkList.horiMotor.LinkListHead_H = ptr->next; + } + else { + g_servoMotorLinkList.horiMotor.LinkListHead_H = NULL; + g_servoMotorLinkList.horiMotor.LinkListTail_H = NULL; + } + OSMemPut(g_memPtr, (uint8_t *)ptr); + } + } + //当前发送完成的数据为低优先级链表中的数据 + else if (g_servoMotorLinkList.horiMotor.linkListNum == lowPriority) { + if (g_servoMotorLinkList.horiMotor.LinkListHead_L != NULL) { + linkList *ptr; + ptr = g_servoMotorLinkList.horiMotor.LinkListHead_L; + if (ptr->next != NULL) { + g_servoMotorLinkList.horiMotor.LinkListHead_L = ptr->next; + } + else { + g_servoMotorLinkList.horiMotor.LinkListHead_L = NULL; + g_servoMotorLinkList.horiMotor.LinkListTail_L = NULL; + } + OSMemPut(g_memPtr, (uint8_t *)ptr); + } + } + } + else if (motorType == vertMotorType) { + if (g_servoMotorLinkList.vertMotor.linkListNum == highPriority) { + if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) { + linkList *ptr; + ptr = g_servoMotorLinkList.vertMotor.LinkListHead_H; + if (ptr->next != NULL) { + g_servoMotorLinkList.vertMotor.LinkListHead_H = ptr->next; + } + else { + g_servoMotorLinkList.vertMotor.LinkListHead_H = NULL; + g_servoMotorLinkList.vertMotor.LinkListTail_H = NULL; + } + OSMemPut(g_memPtr, (uint8_t *)ptr); + } + } + else if (g_servoMotorLinkList.vertMotor.linkListNum == lowPriority) { + if (g_servoMotorLinkList.vertMotor.LinkListHead_L != NULL) { + linkList *ptr; + ptr = g_servoMotorLinkList.vertMotor.LinkListHead_L; + if (ptr->next != NULL) { + g_servoMotorLinkList.vertMotor.LinkListHead_L = ptr->next; + } + else { + g_servoMotorLinkList.vertMotor.LinkListHead_L = NULL; + g_servoMotorLinkList.vertMotor.LinkListTail_L = NULL; + } + OSMemPut(g_memPtr, (uint8_t *)ptr); + } + } } - - linkList *ptr; - ptr = data; - data = data->next; - OSMemPut(g_memPtr, (uint8_t *)ptr); } /*! @@ -119,18 +171,32 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit } if (priority == highPriority) { - g_servoMotorLinkList.horiMotor.LinkListTail_H = ptr; + if (g_servoMotorLinkList.horiMotor.LinkListTail_H == NULL) { + g_servoMotorLinkList.horiMotor.LinkListTail_H = ptr; + } + else { + g_servoMotorLinkList.horiMotor.LinkListTail_H->next = ptr; + g_servoMotorLinkList.horiMotor.LinkListTail_H = ptr; + } + if (g_servoMotorLinkList.horiMotor.LinkListHead_H == NULL) { g_servoMotorLinkList.horiMotor.LinkListHead_H = g_servoMotorLinkList.horiMotor.LinkListTail_H; } } else { - g_servoMotorLinkList.horiMotor.LinkListTail_L = ptr; + if (g_servoMotorLinkList.horiMotor.LinkListTail_L == NULL) { + g_servoMotorLinkList.horiMotor.LinkListTail_L = ptr; + } + else { + g_servoMotorLinkList.horiMotor.LinkListTail_L->next = ptr; + g_servoMotorLinkList.horiMotor.LinkListTail_L = ptr; + } + if (g_servoMotorLinkList.horiMotor.LinkListHead_L == NULL) { g_servoMotorLinkList.horiMotor.LinkListHead_L = g_servoMotorLinkList.horiMotor.LinkListTail_L; - } + } } //释放信号量，通知能发送一次 @@ -143,14 +209,28 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit } if (priority == highPriority) { - g_servoMotorLinkList.vertMotor.LinkListTail_H = ptr; + if (g_servoMotorLinkList.vertMotor.LinkListTail_H == NULL) { + g_servoMotorLinkList.vertMotor.LinkListTail_H = ptr; + } + else { + g_servoMotorLinkList.vertMotor.LinkListTail_H->next = ptr; + g_servoMotorLinkList.vertMotor.LinkListTail_H = ptr; + } + if (g_servoMotorLinkList.vertMotor.LinkListHead_H == NULL) { g_servoMotorLinkList.vertMotor.LinkListHead_H = g_servoMotorLinkList.vertMotor.LinkListTail_H; } } else { - g_servoMotorLinkList.vertMotor.LinkListTail_L = ptr; + if (g_servoMotorLinkList.vertMotor.LinkListTail_L == NULL) { + g_servoMotorLinkList.vertMotor.LinkListTail_L = ptr; + } + else { + g_servoMotorLinkList.vertMotor.LinkListTail_L->next = ptr; + g_servoMotorLinkList.vertMotor.LinkListTail_L = ptr; + } + if (g_servoMotorLinkList.vertMotor.LinkListHead_L == NULL) { g_servoMotorLinkList.vertMotor.LinkListHead_L = g_servoMotorLinkList.vertMotor.LinkListTail_L; @@ -289,7 +369,101 @@ static void creat_task_vert_servo_task(void) } } +void horiServoTimeOut() +{ + OSSemPost(g_horiSpeedMutex); +} +void vertServoTimeOut() +{ + OSSemPost(g_vertSpeedMutex); +} + +BSP_OS_TMR horiServoSoftWareTim; +BSP_OS_TMR vertServoSoftWareTim; +void servoCommSoftWareTimInit() +{ + CPU_INT08U ServoSoftWareTimErr; + horiServoSoftWareTim = OSTmrCreate(10 + , 10 + , OS_TMR_OPT_PERIODIC + , (OS_TMR_CALLBACK)horiServoTimeOut + , (void *)0 + , "tmr1" + , &ServoSoftWareTimErr); + + if ((ServoSoftWareTimErr == OS_ERR_NONE)) { + pdebug(DEBUG_LEVEL_INFO,"create horiServoSoftWareTim success...\n\r"); + } else { + pdebug(DEBUG_LEVEL_FATAL,"create horiServoSoftWareTim failed...\n\r"); + } + + vertServoSoftWareTim = OSTmrCreate(10 + , 10 + , OS_TMR_OPT_PERIODIC + , (OS_TMR_CALLBACK)vertServoTimeOut + , (void *)0 + , "tmr2" + , &ServoSoftWareTimErr); + + if ((ServoSoftWareTimErr == OS_ERR_NONE)) { + pdebug(DEBUG_LEVEL_INFO,"create vertServoSoftWareTim success...\n\r"); + } else { + pdebug(DEBUG_LEVEL_FATAL,"create vertServoSoftWareTim failed...\n\r"); + } +} + +/*! + \brief 停止软件定时器 + \param[in] motor：电机 + horiMotorType 水平电机 + vertMotorType 俯仰电机 + \param[out] none + \retval none +*/ +void stopTimeOut(uint8_t motorType) +{ + if (motorType == horiMotorType) { + CPU_INT08U err; + OSTmrStop(horiServoSoftWareTim, OS_TMR_OPT_NONE, (void *)0, &err); + if (err != OS_ERR_NONE) { + pdebug(DEBUG_LEVEL_FATAL,"start horiServoSoftWareTim failed...\n\r"); + } + } + else if (motorType == vertMotorType) { + CPU_INT08U err; + OSTmrStop(vertServoSoftWareTim, OS_TMR_OPT_NONE, (void *)0, &err); + if (err != OS_ERR_NONE) { + pdebug(DEBUG_LEVEL_FATAL,"start vertServoSoftWareTim failed...\n\r"); + } + } +} + +/*! + \brief 启动软件定时器 + \param[in] motor：电机 + horiMotorType 水平电机 + vertMotorType 俯仰电机 + \param[out] none + \retval none +*/ +void startTimeOut(uint8_t motorType) +{ + if (motorType == horiMotorType) { + CPU_INT08U err; + OSTmrStart(horiServoSoftWareTim, &err); + if (err != OS_ERR_NONE) { + pdebug(DEBUG_LEVEL_FATAL,"start horiServoSoftWareTim failed...\n\r"); + } + } + else if (motorType == vertMotorType) { + CPU_INT08U err; + OSTmrStart(vertServoSoftWareTim, &err); + if (err != OS_ERR_NONE) { + pdebug(DEBUG_LEVEL_FATAL,"start vertServoSoftWareTim failed...\n\r"); + } + } +} void init_speed_module(void) { @@ -328,7 +502,8 @@ void init_speed_module(void) creat_task_hori_servo_task(); creat_task_vert_servo_task(); - + servoCommSoftWareTimInit(); + OSTimeDlyHMSM(0u, 0u, 0u, 500u);//等待硬件初始化成功 uint8_t buffer[20] = {0}; diff --git a/APP/Device/Device_speed/speed_to_servoMotor.h b/APP/Device/Device_speed/speed_to_servoMotor.h index 476fd45..01153c4 100644 --- a/APP/Device/Device_speed/speed_to_servoMotor.h +++ b/APP/Device/Device_speed/speed_to_servoMotor.h @@ -67,8 +67,10 @@ extern ptzServoLinkList g_servoMotorLinkList; void ptz_send_speed(char dev, char speed); void init_speed_module(void); -void servoLinkListMemPut(linkList *data); void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority); +void servoHoriLinkListMemPut(uint8_t motorType); +void stopTimeOut(uint8_t motorType); +void startTimeOut(uint8_t motorType); #endif diff --git a/BSP/Driver/servoMotor/motorCommu.c b/BSP/Driver/servoMotor/motorCommu.c index ccc61c4..5420551 100644 --- a/BSP/Driver/servoMotor/motorCommu.c +++ b/BSP/Driver/servoMotor/motorCommu.c @@ -334,24 +334,51 @@ void USART2_IRQHandler(void) //ͷź֪ͨյһݣԴ OSSemPost(g_horiMotorMutex); CommuInfo_t *pCommuDeal = g_commuInfoBuff[H_MOTOR].pCommuInfo; + + dma_interrupt_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, DMA_CHXCTL_FTFIE); /* лʹýջϲĽԶӵһֽ*/ if ( pCommuDeal->pDmaRsvBuffSelect == DMA_RSVBUFF_SELECT1 ) { pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT2; - dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//лûҪȽDMA - dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2); - dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct); + dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo + , g_MotorDmaBuff[H_MOTOR].dmaRxch);//лûҪȽDMA + // dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2); + // dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct); + dma_memory_address_config(g_MotorDmaBuff[H_MOTOR].dmaNo + , g_MotorDmaBuff[H_MOTOR].dmaRxch + , DMA_MEMORY_1 + , (uint32_t)(pCommuDeal->pDmaRsvBuff2)); + dma_transfer_number_config(g_MotorDmaBuff[H_MOTOR].dmaNo + , g_MotorDmaBuff[H_MOTOR].dmaRxch + , DMA_BUFF_SIZE); dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch); } else { + // pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1; + // dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//лûҪȽDMA + // dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff1); + // dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct); + // dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch); + // dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2); + // dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct); + pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1; dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//лûҪȽDMA - dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff1); - dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct); - dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch); + dma_memory_address_config(g_MotorDmaBuff[H_MOTOR].dmaNo + , g_MotorDmaBuff[H_MOTOR].dmaRxch + , DMA_MEMORY_0 + , (uint32_t)(pCommuDeal->pDmaRsvBuff1)); + dma_transfer_number_config(g_MotorDmaBuff[H_MOTOR].dmaNo + , g_MotorDmaBuff[H_MOTOR].dmaRxch + , DMA_BUFF_SIZE); + dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch); } - + dma_interrupt_flag_clear(g_MotorDmaBuff[H_MOTOR].dmaNo + , g_MotorDmaBuff[H_MOTOR].dmaRxch, DMA_INT_FLAG_FTF); + dma_interrupt_enable(g_MotorDmaBuff[H_MOTOR].dmaNo + , g_MotorDmaBuff[H_MOTOR].dmaRxch, DMA_CHXCTL_FTFIE); + /*DMAҪȡݳ*/ // DmaIdleNum = dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//ȡǻжٸû䣬Ѿ˶ @@ -379,23 +406,45 @@ void DMA0_Channel1_IRQHandler(void) /* лʹýջϲĽԶӵһֽ*/ if ( pCommuDeal->pDmaRsvBuffSelect == DMA_RSVBUFF_SELECT1 ) { + // pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT2; + // dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//лûҪȽDMA + // dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2); + // dmaStruct.number = DMA_BUFF_SIZE; + // dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct); + // dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch); + // dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2); + // dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct); + pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT2; dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//лûҪȽDMA - dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2); - dmaStruct.number = DMA_BUFF_SIZE; - dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct); + dma_memory_address_config(g_MotorDmaBuff[H_MOTOR].dmaNo + , g_MotorDmaBuff[H_MOTOR].dmaRxch + , DMA_MEMORY_1 + , (uint32_t)(pCommuDeal->pDmaRsvBuff2)); + dma_transfer_number_config(g_MotorDmaBuff[H_MOTOR].dmaNo + , g_MotorDmaBuff[H_MOTOR].dmaRxch + , DMA_BUFF_SIZE); dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch); } else { + // pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1; + // dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//лûҪȽDMA + // dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff1); + // dmaStruct.number = DMA_BUFF_SIZE; + // dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct); + // dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch); pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1; dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//лûҪȽDMA - dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff1); - dmaStruct.number = DMA_BUFF_SIZE; - dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct); - dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch); + dma_memory_address_config(g_MotorDmaBuff[H_MOTOR].dmaNo + , g_MotorDmaBuff[H_MOTOR].dmaRxch + , DMA_MEMORY_0 + , (uint32_t)(pCommuDeal->pDmaRsvBuff1)); + dma_transfer_number_config(g_MotorDmaBuff[H_MOTOR].dmaNo + , g_MotorDmaBuff[H_MOTOR].dmaRxch + , DMA_BUFF_SIZE); + dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch); } - } }