#include "speed_to_servoMotor.h"
#include "ptz_header_file.h"
#include "servoMotor.h"

#ifdef PTZ_SERVO_MOTOR


//发送云台实际转速
void ptz_send_speed(char dev, char speed)
{
    unsigned char  Speed1[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00};
    unsigned char  Speed2[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00};
    unsigned short int HSpeed = 0;
    unsigned short int VSpeed = 0;
    Speed1[1] = g_ptz.address;
    Speed2[1] = g_ptz.address;
    
    HSpeed = (unsigned short int)(g_ptz.hori_speed_actual * 100 + 0.5);
    Speed1[3] = 0X03;//水平电机
    Speed1[4] = (unsigned char)(HSpeed >> 8);
    Speed1[5] = (unsigned char)(HSpeed & 0x00ff);
    Speed1[6] = MotorCalPelcoDSUM(Speed1,sizeof(Speed1));
    
    VSpeed = (unsigned short int)(g_ptz.vert_speed_actual * 100 + 0.5);
    Speed2[3] = 0X04;//垂直电机
    Speed2[4] = (unsigned char)(VSpeed >> 8);
    Speed2[5] = (unsigned char)(VSpeed & 0x00ff);
    Speed2[6] = MotorCalPelcoDSUM(Speed2,sizeof(Speed2));
    
    switch(speed)
    {
        case 1://只查询水平转速
        ptz_send_data(dev,Speed1,sizeof(Speed1));        
        break;
        
        case 2://只查询垂直转速
        ptz_send_data(dev,Speed2,sizeof(Speed2));        
        break;
        
        default://水平转速垂直转速都查询
        ptz_send_data(dev,Speed1,sizeof(Speed1));
        ptz_send_data(dev,Speed2,sizeof(Speed2));        
        break;
    }
}


static void ptz_hori_step_speed_task()
{  
    while(1)
    {
        if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
        {
        }
        
        if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
        {
        }
        OSTimeDlyHMSM(0u, 0u, 0u, 10);    
    }
}


static void ptz_vert_step_speed_task()
{
    while(1)
    {
        if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
        {
        }
        if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
        {
        }        
        OSTimeDlyHMSM(0u, 0u, 0u, 10);
    }
}



static OS_STK  task_hori_step_speed_stk[TASK_HORI_PID_STK_SIZE];
static  void  creat_task_hori_step_speed_task(void)
{ 
   CPU_INT08U  task_err;   
   CPU_INT08U  name_err;
   
   task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_step_speed_task,           
		   (void           *) 0,
		   (OS_STK         *)&task_hori_step_speed_stk[TASK_HORI_PID_STK_SIZE - 1],
		   (INT8U           ) TASK_HORI_PID_PRIO,                   
		   (INT16U          ) TASK_HORI_PID_PRIO,
		   (OS_STK         *)&task_hori_step_speed_stk[0],
		   (INT32U          ) TASK_HORI_PID_STK_SIZE,
		   (void           *) 0,
		   (INT16U          )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));     
    #if (OS_TASK_NAME_EN > 0)
    OSTaskNameSet(TASK_HORI_PID_PRIO, "ptz_hori_step_speed_task", &name_err);
    #endif
       
    if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
        pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_step_speed_task  success...\n\r");
	} else { 
        pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_step_speed_task  failed...\n\r");
	}
}



static OS_STK  task_vert_step_speed_stk[TASK_VERT_PID_STK_SIZE];
static  void  creat_task_vert_step_speed_task(void)
{ 
   CPU_INT08U  task_err;   
   CPU_INT08U  name_err;
   
   task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_step_speed_task,           
		   (void           *) 0,
		   (OS_STK         *)&task_vert_step_speed_stk[TASK_VERT_PID_STK_SIZE - 1],
		   (INT8U           ) TASK_VERT_PID_PRIO,                   
		   (INT16U          ) TASK_VERT_PID_PRIO,
		   (OS_STK         *)&task_vert_step_speed_stk[0],
		   (INT32U          ) TASK_VERT_PID_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_vert_step_speed_task", &name_err);
    #endif
       
    if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
        pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_step_speed_task  success...\n\r");
	} else { 
        pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_step_speed_task  failed...\n\r");
	}
}





void init_speed_module(void)
{
    servoMotorInit();
    creat_task_hori_step_speed_task();
    creat_task_vert_step_speed_task();
}



#endif