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

解决链表释放BUG

This commit is contained in:
起床就犯困 2025-10-20 21:27:33 +08:00
parent 42f0144389
commit fdd19f00a9
3 changed files with 66 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
APP/Device/Device_rotate/rotate_servo.c
View File

@ -136,10 +136,10 @@ void ptz_hori_stop(unsigned short int time)
{ {
OSTimeDlyHMSM(0u, 0u, 0u, time); OSTimeDlyHMSM(0u, 0u, 0u, time);
ptz_hori_stop_count = 0; ptz_hori_stop_count = 0;
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
// , WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0) servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); //失能电机 , WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority); //失能电机
} }
ptz_hori_stop_count ++; ptz_hori_stop_count ++;
//电子稳定 //电子稳定
@ -222,6 +222,8 @@ static char ptz_hori_rotate_monitor_task()
static float BNP_D;//刹车最近点与当前位置的距离 static float BNP_D;//刹车最近点与当前位置的距离
static float BFP_D;//刹车最远点与当前位置的距离 static float BFP_D;//刹车最远点与当前位置的距离
static unsigned int k; static unsigned int k;
// OSSemPend(g_horiMotorMutex, 100, &err);
switch(g_ptz.hori_rotate_monitor_switch) switch(g_ptz.hori_rotate_monitor_switch)
{ {
case 0://关闭 case 0://关闭
@ -313,7 +315,7 @@ static char ptz_hori_rotate_monitor_task()
if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction) if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
{ {
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed; g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR // set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
} }
else//转向不同，则直接修改设置的转速 else//转向不同，则直接修改设置的转速
{ {

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

@ -61,6 +61,15 @@ void ptz_send_speed(char dev, char speed)
*/ */
void servoLinkListMemPut(uint8_t motorType) void servoLinkListMemPut(uint8_t motorType)
{ {
static BSP_OS_SEM servoLinkListMemPutMutex;
static uint8_t flag = 1;
if (flag == 1) {
BSP_OS_SemCreate(&servoLinkListMemPutMutex, 1, "servoLinkListMemPutMutex");
flag = 0;
}
BSP_OS_SemWait(&servoLinkListMemPutMutex, 0);
//水平电机 //水平电机
if (motorType == horiMotorType) { if (motorType == horiMotorType) {
//当前发送完成的数据为高优先级链表中的数据 //当前发送完成的数据为高优先级链表中的数据
@ -76,8 +85,8 @@ void servoLinkListMemPut(uint8_t motorType)
g_servoMotorLinkList.horiMotor.LinkListTail_H = NULL; g_servoMotorLinkList.horiMotor.LinkListTail_H = NULL;
} }
g_servoMotorLinkList.horiMotor.linkListNum--; g_servoMotorLinkList.horiMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr); OSMemPut(g_memPtr, ptr);
} }
} }
//当前发送完成的数据为低优先级链表中的数据 //当前发送完成的数据为低优先级链表中的数据
else if (g_servoMotorLinkList.horiMotor.linkListPriority == lowPriority) { else if (g_servoMotorLinkList.horiMotor.linkListPriority == lowPriority) {
@ -92,7 +101,7 @@ void servoLinkListMemPut(uint8_t motorType)
g_servoMotorLinkList.horiMotor.LinkListTail_L = NULL; g_servoMotorLinkList.horiMotor.LinkListTail_L = NULL;
} }
g_servoMotorLinkList.horiMotor.linkListNum--; g_servoMotorLinkList.horiMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr); OSMemPut(g_memPtr, ptr);
} }
} }
// g_servoMotorLinkList.horiMotor.linkListNum--; // g_servoMotorLinkList.horiMotor.linkListNum--;
@ -110,7 +119,7 @@ void servoLinkListMemPut(uint8_t motorType)
g_servoMotorLinkList.vertMotor.LinkListTail_H = NULL; g_servoMotorLinkList.vertMotor.LinkListTail_H = NULL;
} }
g_servoMotorLinkList.vertMotor.linkListNum--; g_servoMotorLinkList.vertMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr); OSMemPut(g_memPtr, ptr);
} }
} }
@ -126,14 +135,14 @@ void servoLinkListMemPut(uint8_t motorType)
g_servoMotorLinkList.vertMotor.LinkListTail_L = NULL; g_servoMotorLinkList.vertMotor.LinkListTail_L = NULL;
} }
g_servoMotorLinkList.vertMotor.linkListNum--; g_servoMotorLinkList.vertMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr); OSMemPut(g_memPtr, ptr);
} }
} }
// g_servoMotorLinkList.vertMotor.linkListNum--; // g_servoMotorLinkList.vertMotor.linkListNum--;
} }
BSP_OS_SemPost(&servoLinkListMemPutMutex);
} }
/*! /*!
@ -149,6 +158,15 @@ void servoLinkListMemPut(uint8_t motorType)
*/ */
bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority) bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority)
{ {
static BSP_OS_SEM servoSendDataMutex;
static uint8_t flag = 1;
if (flag == 1) {
BSP_OS_SemCreate(&servoSendDataMutex, 1, "servoLinkListMemPutMutex");
flag = 0;
}
BSP_OS_SemWait(&servoSendDataMutex, 0);
if ((motor != horiMotorType) && (motor!= vertMotorType)) { if ((motor != horiMotorType) && (motor!= vertMotorType)) {
return false; return false;
} }
@ -172,17 +190,18 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
} }
ptr->length = dataLen; ptr->length = dataLen;
memcpy(ptr->data, data, dataLen); memcpy(ptr->data, data, dataLen);
ptr->next = NULL;
//将节点添加进入链表中 //将节点添加进入链表中
if (motor == horiMotorType) { if (motor == horiMotorType) {
if (priority == highPriority) { if (priority == highPriority) {
if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber) { if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber - 1) {
OSMemPut(g_memPtr, ptr); OSMemPut(g_memPtr, ptr);
return false; return false;
} }
} }
else { else {
if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber - 1) { if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber - 2) {
OSMemPut(g_memPtr, ptr); OSMemPut(g_memPtr, ptr);
return false; return false;
} }
@ -201,7 +220,6 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
g_servoMotorLinkList.horiMotor.LinkListHead_H g_servoMotorLinkList.horiMotor.LinkListHead_H
= g_servoMotorLinkList.horiMotor.LinkListTail_H; = g_servoMotorLinkList.horiMotor.LinkListTail_H;
} }
g_servoMotorLinkList.horiMotor.linkListNum++;
} }
else { else {
if (g_servoMotorLinkList.horiMotor.LinkListTail_L == NULL) { if (g_servoMotorLinkList.horiMotor.LinkListTail_L == NULL) {
@ -216,21 +234,21 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
g_servoMotorLinkList.horiMotor.LinkListHead_L g_servoMotorLinkList.horiMotor.LinkListHead_L
= g_servoMotorLinkList.horiMotor.LinkListTail_L; = g_servoMotorLinkList.horiMotor.LinkListTail_L;
} }
g_servoMotorLinkList.horiMotor.linkListNum++;
} }
g_servoMotorLinkList.horiMotor.linkListNum++;
//释放信号量，通知能发送一次 //释放信号量，通知能发送一次
OSSemPost(g_horiSpeedSem); OSSemPost(g_horiSpeedSem);
} }
else { else {
if (priority == highPriority) { if (priority == highPriority) {
if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber) { if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber - 1) {
OSMemPut(g_memPtr, ptr); OSMemPut(g_memPtr, ptr);
return false; return false;
} }
} }
else { else {
if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber - 1) { if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber - 2) {
OSMemPut(g_memPtr, ptr); OSMemPut(g_memPtr, ptr);
return false; return false;
} }
@ -249,7 +267,6 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
g_servoMotorLinkList.vertMotor.LinkListHead_H g_servoMotorLinkList.vertMotor.LinkListHead_H
= g_servoMotorLinkList.vertMotor.LinkListTail_H; = g_servoMotorLinkList.vertMotor.LinkListTail_H;
} }
g_servoMotorLinkList.vertMotor.linkListNum++;
} }
else { else {
if (g_servoMotorLinkList.vertMotor.LinkListTail_L == NULL) { if (g_servoMotorLinkList.vertMotor.LinkListTail_L == NULL) {
@ -263,13 +280,14 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
if (g_servoMotorLinkList.vertMotor.LinkListHead_L == NULL) { if (g_servoMotorLinkList.vertMotor.LinkListHead_L == NULL) {
g_servoMotorLinkList.vertMotor.LinkListHead_L g_servoMotorLinkList.vertMotor.LinkListHead_L
= g_servoMotorLinkList.vertMotor.LinkListTail_L; = g_servoMotorLinkList.vertMotor.LinkListTail_L;
} }
g_servoMotorLinkList.vertMotor.linkListNum++;
} }
g_servoMotorLinkList.vertMotor.linkListNum++;
//释放信号量，通知能发送一次 //释放信号量，通知能发送一次
OSSemPost(g_vertSpeedSem); OSSemPost(g_vertSpeedSem);
} }
BSP_OS_SemPost(&servoSendDataMutex);
return true; return true;
} }
@ -462,6 +480,15 @@ void servoCommSoftWareTimInit()
*/ */
void stopTimeOut(uint8_t motorType) void stopTimeOut(uint8_t motorType)
{ {
static BSP_OS_SEM stopTimeOutMutex;
static uint8_t flag = 1;
if (flag == 1) {
BSP_OS_SemCreate(&stopTimeOutMutex, 1, "servoLinkListMemPutMutex");
flag = 0;
}
BSP_OS_SemWait(&stopTimeOutMutex, 0);
if (motorType == horiMotorType) { if (motorType == horiMotorType) {
CPU_INT08U err; CPU_INT08U err;
OSTmrStop(horiServoSoftWareTim, OS_TMR_OPT_NONE, (void *)0, &err); OSTmrStop(horiServoSoftWareTim, OS_TMR_OPT_NONE, (void *)0, &err);
@ -476,6 +503,8 @@ void stopTimeOut(uint8_t motorType)
pdebug(DEBUG_LEVEL_FATAL,"start vertServoSoftWareTim failed...\n\r"); pdebug(DEBUG_LEVEL_FATAL,"start vertServoSoftWareTim failed...\n\r");
} }
} }
BSP_OS_SemPost(&stopTimeOutMutex);
} }
/*! /*!
@ -488,6 +517,15 @@ void stopTimeOut(uint8_t motorType)
*/ */
void startTimeOut(uint8_t motorType) void startTimeOut(uint8_t motorType)
{ {
static BSP_OS_SEM startTimeOutMutex;
static uint8_t flag = 1;
if (flag == 1) {
BSP_OS_SemCreate(&startTimeOutMutex, 1, "servoLinkListMemPutMutex");
flag = 0;
}
BSP_OS_SemWait(&startTimeOutMutex, 0);
if (motorType == horiMotorType) { if (motorType == horiMotorType) {
CPU_INT08U err; CPU_INT08U err;
OSTmrStart(horiServoSoftWareTim, &err); OSTmrStart(horiServoSoftWareTim, &err);
@ -502,6 +540,7 @@ void startTimeOut(uint8_t motorType)
pdebug(DEBUG_LEVEL_FATAL,"start vertServoSoftWareTim failed...\n\r"); pdebug(DEBUG_LEVEL_FATAL,"start vertServoSoftWareTim failed...\n\r");
} }
} }
BSP_OS_SemPost(&startTimeOutMutex);
} }
void init_speed_module(void) void init_speed_module(void)

14
APP/Device/device_Other/device_adc_collect.c
View File

@ -501,14 +501,12 @@ static char ptz_data_collect_task()
if(j >= 100) if(j >= 100)
{ {
//读取水平电机实时速度
servoSendData(horiMotorType, ReadMotorOneReg(horiMotorType, READ_MOTOR_SPEED_NOW)
// //读取水平电机实时速度 , READ_ONE_REG_FRAME_NUM, lowPriority);
// servoSendData(horiMotorType, ReadMotorOneReg(horiMotorType, READ_MOTOR_SPEED_NOW) //读取俯仰电机实时速度
// , READ_ONE_REG_FRAME_NUM, lowPriority); servoSendData(vertMotorType, ReadMotorOneReg(vertMotorType, READ_MOTOR_SPEED_NOW)
// //读取俯仰电机实时速度 , READ_ONE_REG_FRAME_NUM, lowPriority);
// servoSendData(vertMotorType, ReadMotorOneReg(vertMotorType, READ_MOTOR_SPEED_NOW)
// , READ_ONE_REG_FRAME_NUM, lowPriority);
j=0; j=0;
//云台不自检关闭，打开采集任务 //云台不自检关闭，打开采集任务
#ifndef PTZ_NO_SELF_CHECK #ifndef PTZ_NO_SELF_CHECK