104 lines
2.8 KiB
C
104 lines
2.8 KiB
C
|
#include "speed_to_servoMotor.h"
|
||
|
|
#include "ptz_header_file.h"
|
||
|
|
#include "servoMotor.h"
|
||
|
|
|
||
|
|
#ifdef PTZ_SERVO_MOTOR
|
||
|
|
|
||
|
|
//电机数据解析任务互斥量
|
||
|
|
BSP_OS_SEM g_horiMotorMutex;
|
||
|
|
BSP_OS_SEM g_vertMotorMutex;
|
||
|
|
|
||
|
|
/*!
|
||
|
|
\brief 水平电机接收任务
|
||
|
|
\param[in] none
|
||
|
|
\param[out] none
|
||
|
|
\retval none
|
||
|
|
*/
|
||
|
|
static void ptz_recv_hori_servo_task()
|
||
|
|
{
|
||
|
|
CPU_INT08U err;
|
||
|
|
|
||
|
|
while(1) {
|
||
|
|
OSSemPend(g_horiMotorMutex, 0, &err);
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/*!
|
||
|
|
\brief 俯仰电机接收任务
|
||
|
|
\param[in] none
|
||
|
|
\param[out] none
|
||
|
|
\retval none
|
||
|
|
*/
|
||
|
|
static void ptz_recv_vert_servo_task()
|
||
|
|
{
|
||
|
|
CPU_INT08U err;
|
||
|
|
|
||
|
|
while(1) {
|
||
|
|
OSSemPend(g_vertMotorMutex, 0, &err);
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static OS_STK task_recv_hori_servo_stk[TASK_RECV_HORI_SERVO_STK_SIZE];
|
||
|
|
static OS_STK task_recv_vert_servo_stk[TASK_RECV_HORI_SERVO_STK_SIZE];
|
||
|
|
static void creat_task_servo_recv_task(void)
|
||
|
|
{
|
||
|
|
CPU_INT08U task_err;
|
||
|
|
CPU_INT08U name_err;
|
||
|
|
|
||
|
|
task_err = OSTaskCreateExt((void (*)(void *)) ptz_recv_hori_servo_task,
|
||
|
|
(void *) 0,
|
||
|
|
(OS_STK *)&task_recv_hori_servo_stk[TASK_RECV_HORI_SERVO_STK_SIZE - 1],
|
||
|
|
(INT8U ) TASK_RECV_HORI_SERVO_PRIO,
|
||
|
|
(INT16U ) TASK_RECV_HORI_SERVO_PRIO,
|
||
|
|
(OS_STK *)&task_recv_hori_servo_stk[0],
|
||
|
|
(INT32U ) TASK_RECV_HORI_SERVO_STK_SIZE,
|
||
|
|
(void *) 0,
|
||
|
|
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
|
||
|
|
#if (OS_TASK_NAME_EN > 0)
|
||
|
|
OSTaskNameSet(TASK_VERT_PID_PRIO, "ptz_recv_hori_servo_task", &name_err);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
|
||
|
|
pdebug(DEBUG_LEVEL_INFO,"create ptz_recv_hori_servo_task success...\n\r");
|
||
|
|
} else {
|
||
|
|
pdebug(DEBUG_LEVEL_FATAL,"create ptz_recv_hori_servo_task failed...\n\r");
|
||
|
|
}
|
||
|
|
|
||
|
|
task_err = OSTaskCreateExt((void (*)(void *)) ptz_recv_vert_servo_task,
|
||
|
|
(void *) 0,
|
||
|
|
(OS_STK *)&task_recv_vert_servo_stk[TASK_RECV_HORI_SERVO_STK_SIZE - 1],
|
||
|
|
(INT8U ) TASK_RECV_VERT_SERVO_PRIO,
|
||
|
|
(INT16U ) TASK_RECV_VERT_SERVO_PRIO,
|
||
|
|
(OS_STK *)&task_recv_vert_servo_stk[0],
|
||
|
|
(INT32U ) task_recv_vert_servo_stk,
|
||
|
|
(void *) 0,
|
||
|
|
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
|
||
|
|
#if (OS_TASK_NAME_EN > 0)
|
||
|
|
OSTaskNameSet(TASK_VERT_PID_PRIO, "ptz_recv_vert_servo_task", &name_err);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
|
||
|
|
pdebug(DEBUG_LEVEL_INFO,"create ptz_recv_vert_servo_task success...\n\r");
|
||
|
|
} else {
|
||
|
|
pdebug(DEBUG_LEVEL_FATAL,"create ptz_recv_vert_servo_task failed...\n\r");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void Init_ServoMotorRecv(void)
|
||
|
|
{
|
||
|
|
g_horiMotorMutex = OSSemCreate(1);
|
||
|
|
g_vertMotorMutex = OSSemCreate(1);
|
||
|
|
|
||
|
|
creat_task_servo_recv_task();
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|