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

8
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);
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)
, 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 ++;
//电子稳定
@ -222,6 +222,8 @@ static char ptz_hori_rotate_monitor_task()
static float BNP_D;//刹车最近点与当前位置的距离
static float BFP_D;//刹车最远点与当前位置的距离
static unsigned int k;
// OSSemPend(g_horiMotorMutex, 100, &err);
switch(g_ptz.hori_rotate_monitor_switch)
{
case 0://关闭

65
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)
{
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) {
//当前发送完成的数据为高优先级链表中的数据
@ -76,7 +85,7 @@ void servoLinkListMemPut(uint8_t motorType)
g_servoMotorLinkList.horiMotor.LinkListTail_H = NULL;
}
g_servoMotorLinkList.horiMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr);
OSMemPut(g_memPtr, ptr);
}
}
//当前发送完成的数据为低优先级链表中的数据
@ -92,7 +101,7 @@ void servoLinkListMemPut(uint8_t motorType)
g_servoMotorLinkList.horiMotor.LinkListTail_L = NULL;
}
g_servoMotorLinkList.horiMotor.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr);
OSMemPut(g_memPtr, ptr);
}
}
// g_servoMotorLinkList.horiMotor.linkListNum--;
@ -110,7 +119,7 @@ void servoLinkListMemPut(uint8_t motorType)
g_servoMotorLinkList.vertMotor.LinkListTail_H = NULL;
}
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.linkListNum--;
OSMemPut(g_memPtr, (uint8_t *)ptr);
OSMemPut(g_memPtr, ptr);
}
}
// 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)
{
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)) {
return false;
}
@ -172,17 +190,18 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
}
ptr->length = dataLen;
memcpy(ptr->data, data, dataLen);
ptr->next = NULL;
//将节点添加进入链表中
if (motor == horiMotorType) {
if (priority == highPriority) {
if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber) {
if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber - 1) {
OSMemPut(g_memPtr, ptr);
return false;
}
}
else {
if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber - 1) {
if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber - 2) {
OSMemPut(g_memPtr, ptr);
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.LinkListTail_H;
}
g_servoMotorLinkList.horiMotor.linkListNum++;
}
else {
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.LinkListTail_L;
}
g_servoMotorLinkList.horiMotor.linkListNum++;
}
g_servoMotorLinkList.horiMotor.linkListNum++;
//释放信号量，通知能发送一次
OSSemPost(g_horiSpeedSem);
}
else {
if (priority == highPriority) {
if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber) {
if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber - 1) {
OSMemPut(g_memPtr, ptr);
return false;
}
}
else {
if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber - 1) {
if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber - 2) {
OSMemPut(g_memPtr, ptr);
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.LinkListTail_H;
}
g_servoMotorLinkList.vertMotor.linkListNum++;
}
else {
if (g_servoMotorLinkList.vertMotor.LinkListTail_L == NULL) {
@ -264,12 +281,13 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
g_servoMotorLinkList.vertMotor.LinkListHead_L
= g_servoMotorLinkList.vertMotor.LinkListTail_L;
}
g_servoMotorLinkList.vertMotor.linkListNum++;
}
g_servoMotorLinkList.vertMotor.linkListNum++;
//释放信号量，通知能发送一次
OSSemPost(g_vertSpeedSem);
}
BSP_OS_SemPost(&servoSendDataMutex);
return true;
}
@ -462,6 +480,15 @@ void servoCommSoftWareTimInit()
*/
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) {
CPU_INT08U 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");
}
}
BSP_OS_SemPost(&stopTimeOutMutex);
}
/*!
@ -488,6 +517,15 @@ void stopTimeOut(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) {
CPU_INT08U err;
OSTmrStart(horiServoSoftWareTim, &err);
@ -502,6 +540,7 @@ void startTimeOut(uint8_t motorType)
pdebug(DEBUG_LEVEL_FATAL,"start vertServoSoftWareTim failed...\n\r");
}
}
BSP_OS_SemPost(&startTimeOutMutex);
}
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)
{
// //读取水平电机实时速度
// 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(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);
j=0;
//云台不自检关闭，打开采集任务
#ifndef PTZ_NO_SELF_CHECK