#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