pengsx
/
servoMotor
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修改链表插入

This commit is contained in:
起床就犯困 2025-10-16 10:29:10 +08:00
parent d9df2548c5
commit fdba688de4
3 changed files with 259 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

209
APP/Device/Device_speed/speed_to_servoMotor.c
View File

@ -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};

4
APP/Device/Device_speed/speed_to_servoMotor.h
View File

@ -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

79
BSP/Driver/servoMotor/motorCommu.c
View File

@ -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);
}
}
}