diff --git a/APP/Device/Device_rotate/rotate_servo.c b/APP/Device/Device_rotate/rotate_servo.c index 2b80aba..f670f08 100644 --- a/APP/Device/Device_rotate/rotate_servo.c +++ b/APP/Device/Device_rotate/rotate_servo.c @@ -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://关闭 @@ -313,7 +315,7 @@ static char ptz_hori_rotate_monitor_task() if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction) { 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//转向不同，则直接修改设置的转速 { diff --git a/APP/Device/Device_speed/speed_to_servoMotor.c b/APP/Device/Device_speed/speed_to_servoMotor.c index 0f6ff5f..bc0317c 100644 --- a/APP/Device/Device_speed/speed_to_servoMotor.c +++ b/APP/Device/Device_speed/speed_to_servoMotor.c @@ -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,8 +85,8 @@ 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); + } } //当前发送完成的数据为低优先级链表中的数据 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.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) { @@ -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) { 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) diff --git a/APP/Device/device_Other/device_adc_collect.c b/APP/Device/device_Other/device_adc_collect.c index 090e4d9..d91988e 100644 --- a/APP/Device/device_Other/device_adc_collect.c +++ b/APP/Device/device_Other/device_adc_collect.c @@ -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