#include "ptz_header_file.h"
#include "Timer.h"
#include "ptz_default_value.h"
#include "ptz_type_select.h"
#include "rotate_step.h"
#include "speed_to_step.h"

#ifdef PTZ_STEP_MOTOR


static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁
static BSP_OS_SEM ptz_vert_stop_mutex;


static char ptz_hori_stop_count;//水平停止计数，防止多次停止多次延时
static char ptz_vert_stop_count;//垂直停止计数，防止多次停止多次延时


void ptz_sem_post_stop_mutex()//释放云台启停共享资源锁
{
  BSP_OS_SemPost(&ptz_hori_stop_mutex);
  BSP_OS_SemPost(&ptz_vert_stop_mutex);
}

//转速r/min转化为步进电机步进频率,step 细分程度1,2,4,8,16,32,64,128,256
float ptz_hori_step_v_to_f(float v, unsigned short int microstep_alue)
{
  float f;
  f = v * 6.0 * (float)microstep_alue / PTZ_HORI_MOTOR_STEP;
  return f;
}

//步进电机步进频率转化为转速r/min
float ptz_hori_step_f_to_v(float f, unsigned short int microstep_alue)
{
  float v;
  v = f * PTZ_HORI_MOTOR_STEP / ((float)microstep_alue * 6.0);
  return v;
}

//转速r/min转化为步进电机步进频率,step 细分程度1,2,4,8,16,32,64,128,256
float ptz_vert_step_v_to_f(float v, unsigned short int microstep_alue)
{
  float f;
  f = v * 6.0 * (float)microstep_alue / PTZ_VERT_MOTOR_STEP;
  return f;
}

//步进电机步进频率转化为转速r/min
float ptz_vert_step_f_to_v(float f, unsigned short int microstep_alue)
{
  float v;
  v = f * PTZ_VERT_MOTOR_STEP / ((float)microstep_alue * 6.0);
  return v;
}

#ifdef TMC2160

extern TMC2160Parameter tmc2160_parameter;

float ptz_hori_break_angle()
{
  float s,s1,s2,v2;
  s1 = 3.0 * PTZ_HORI_BREAK_SPEED * PTZ_HORI_BREAK_SPEED * (float)tmc2160_parameter.hori_dec_time_max / PTZ_HORI_MAX_SPEED / 1000.0;
  v2 = ptz_hori_step_f_to_v(/*g_ptz.hori_drv8711.set_f*/g_ptz.hori_tmc2160.f, g_ptz.hori_tmc2160.microstep_vlue) / PTZ_HORI_RATIO;
  s2 = 3.0 * v2 * v2 * (float)tmc2160_parameter.hori_dec_time_max / PTZ_HORI_MAX_SPEED / 1000.0;
  
  s = s2 -s1;
  g_ptz.hori_tmc2160.stop_angle = s;
  return s;
}

float ptz_vert_break_angle()
{
  float s,s1,s2,v2;
  s1 = 3.0 * PTZ_VERT_BREAK_SPEED * PTZ_VERT_BREAK_SPEED * (float)tmc2160_parameter.vert_dec_time_max / PTZ_VERT_MAX_SPEED / 1000.0;
  v2 = ptz_vert_step_f_to_v(/*g_ptz.vert_drv8711.set_f*/g_ptz.vert_tmc2160.f, g_ptz.vert_tmc2160.microstep_vlue) / PTZ_VERT_RATIO;
  s2 = 3.0 * v2 * v2 * (float)tmc2160_parameter.vert_dec_time_max / PTZ_VERT_MAX_SPEED / 1000.0;
  
  s = s2 -s1;
  g_ptz.vert_tmc2160.stop_angle = s;
  return s;
}


//根据转速判断采用的细分程度,输入速度v是云台的转速
TMC2160Control ptz_hori_choice_microstep(float ptz_v)
{
  float motor_v;
  TMC2160Control data_tmc2160;
  motor_v = ptz_v * PTZ_HORI_RATIO;
  if(motor_v > PTZ_HORI_MOTOR_MAX_SPEED)
  {
    motor_v = PTZ_HORI_MOTOR_MAX_SPEED;
  }
  if(motor_v < PTZ_HORI_MOTOR_MIN_SPEED)
  {
    motor_v = PTZ_HORI_MOTOR_MIN_SPEED;
  }
  
  data_tmc2160.set_v = motor_v;
  
  //给一个预设的默认值
  data_tmc2160.microstep = TMC2160_1_16_STEP;
  data_tmc2160.microstep_vlue = 16;
  
  if(tmc2160_parameter.hori_step_1_256_en == 1 &&            
     motor_v >= tmc2160_parameter.hori_step_1_256_v_min && 
     motor_v <= tmc2160_parameter.hori_step_1_256_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_256_STEP;
    data_tmc2160.microstep_vlue = 256;
  }
  
  if(tmc2160_parameter.hori_step_1_128_en == 1 &&            
     motor_v >= tmc2160_parameter.hori_step_1_128_v_min && 
     motor_v <= tmc2160_parameter.hori_step_1_128_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_128_STEP;
    data_tmc2160.microstep_vlue = 128;
  }
  
  if(tmc2160_parameter.hori_step_1_64_en == 1 &&            
     motor_v > tmc2160_parameter.hori_step_1_64_v_min && 
     motor_v <= tmc2160_parameter.hori_step_1_64_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_64_STEP;
    data_tmc2160.microstep_vlue = 64;
  }
  
  if(tmc2160_parameter.hori_step_1_32_en == 1 &&            
     motor_v > tmc2160_parameter.hori_step_1_32_v_min && 
     motor_v <= tmc2160_parameter.hori_step_1_32_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_32_STEP;
    data_tmc2160.microstep_vlue = 32;
  }
  
  if(tmc2160_parameter.hori_step_1_16_en == 1 &&            
     motor_v > tmc2160_parameter.hori_step_1_16_v_min && 
     motor_v <= tmc2160_parameter.hori_step_1_16_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_16_STEP;
    data_tmc2160.microstep_vlue = 16;
  }
  
  if(tmc2160_parameter.hori_step_1_8_en == 1 &&            
     motor_v > tmc2160_parameter.hori_step_1_8_v_min && 
     motor_v <= tmc2160_parameter.hori_step_1_8_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_8_STEP;
    data_tmc2160.microstep_vlue = 8;
  }
  
  if(tmc2160_parameter.hori_step_1_4_en == 1 &&            
     motor_v > tmc2160_parameter.hori_step_1_4_v_min && 
     motor_v <= tmc2160_parameter.hori_step_1_4_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_4_STEP;
    data_tmc2160.microstep_vlue = 4;
  }
  
  if(tmc2160_parameter.hori_step_1_2_en == 1 &&            
     motor_v > tmc2160_parameter.hori_step_1_2_v_min && 
     motor_v <= tmc2160_parameter.hori_step_1_2_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_2_STEP;
    data_tmc2160.microstep_vlue = 2;
  }
 

  if(tmc2160_parameter.hori_step_1_1_en == 1 &&            
     motor_v > tmc2160_parameter.hori_step_1_1_v_min && 
     motor_v <= tmc2160_parameter.hori_step_1_1_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_1_STEP;
    data_tmc2160.microstep_vlue = 1;
  }
 
  
  switch(data_tmc2160.microstep)
  {
    case TMC2160_1_1_STEP:
      data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 1);
      break;
      
    case TMC2160_1_2_STEP:
      data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 2);
      break;
      
    case TMC2160_1_4_STEP:
      data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 4);
      break;
      
    case TMC2160_1_8_STEP:
      data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 8);
      break;
      
    case TMC2160_1_16_STEP:
      data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 16);
      break;
      
    case TMC2160_1_32_STEP:
      data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 32);
      break;
      
    case TMC2160_1_64_STEP:
      data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 64);
      break;
      
    case TMC2160_1_128_STEP:
      data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 128);
      break;
      
    case TMC2160_1_256_STEP:
      data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 256);
      break;
  }
  
  return data_tmc2160;
}


TMC2160Control ptz_vert_choice_microstep(float ptz_v)
{
  float motor_v;
  TMC2160Control data_tmc2160;
  motor_v = ptz_v * PTZ_VERT_RATIO;
  if(motor_v > PTZ_VERT_MOTOR_MAX_SPEED)
  {
    motor_v = PTZ_VERT_MOTOR_MAX_SPEED;
  }
  if(motor_v < PTZ_VERT_MOTOR_MIN_SPEED)
  {
    motor_v = PTZ_VERT_MOTOR_MIN_SPEED;
  }
  
  data_tmc2160.set_v = motor_v;
  
  //给一个预设的默认值
  data_tmc2160.microstep = TMC2160_1_16_STEP;
  data_tmc2160.microstep_vlue = 16;
  
  if(tmc2160_parameter.vert_step_1_256_en == 1 &&            
     motor_v >= tmc2160_parameter.vert_step_1_256_v_min && 
     motor_v <= tmc2160_parameter.vert_step_1_256_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_256_STEP;
    data_tmc2160.microstep_vlue = 256;
  }
  
  if(tmc2160_parameter.vert_step_1_128_en == 1 &&            
     motor_v >= tmc2160_parameter.vert_step_1_128_v_min && 
     motor_v <= tmc2160_parameter.vert_step_1_128_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_128_STEP;
    data_tmc2160.microstep_vlue = 128;
  }
  
  if(tmc2160_parameter.vert_step_1_64_en == 1 &&            
     motor_v >= tmc2160_parameter.vert_step_1_64_v_min && 
     motor_v <= tmc2160_parameter.vert_step_1_64_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_64_STEP;
    data_tmc2160.microstep_vlue = 64;
  }
  
  if(tmc2160_parameter.vert_step_1_32_en == 1 &&            
     motor_v >= tmc2160_parameter.vert_step_1_32_v_min && 
     motor_v <= tmc2160_parameter.vert_step_1_32_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_32_STEP;
    data_tmc2160.microstep_vlue = 32;
  }
  
  if(tmc2160_parameter.vert_step_1_16_en == 1 &&            
     motor_v >= tmc2160_parameter.vert_step_1_16_v_min && 
     motor_v <= tmc2160_parameter.vert_step_1_16_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_16_STEP;
    data_tmc2160.microstep_vlue = 16;
  }
  
  if(tmc2160_parameter.vert_step_1_8_en == 1 &&            
     motor_v >= tmc2160_parameter.vert_step_1_8_v_min && 
     motor_v <= tmc2160_parameter.vert_step_1_8_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_8_STEP;
    data_tmc2160.microstep_vlue = 8;
  }
  
  if(tmc2160_parameter.vert_step_1_4_en == 1 &&            
     motor_v >= tmc2160_parameter.vert_step_1_4_v_min && 
     motor_v <= tmc2160_parameter.vert_step_1_4_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_4_STEP;
    data_tmc2160.microstep_vlue = 4;
  }
  
  if(tmc2160_parameter.vert_step_1_2_en == 1 &&            
     motor_v >= tmc2160_parameter.vert_step_1_2_v_min && 
     motor_v <= tmc2160_parameter.vert_step_1_2_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_2_STEP;
    data_tmc2160.microstep_vlue = 2;
  }
 

  if(tmc2160_parameter.vert_step_1_1_en == 1 &&            
     motor_v >= tmc2160_parameter.vert_step_1_1_v_min && 
     motor_v <= tmc2160_parameter.vert_step_1_1_v_max)
  {
    data_tmc2160.microstep = TMC2160_1_1_STEP;
    data_tmc2160.microstep_vlue = 1;
  }
 
  
  switch(data_tmc2160.microstep)
  {
    case TMC2160_1_1_STEP:
      data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 1);
      break;
      
    case TMC2160_1_2_STEP:
      data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 2);
      break;
      
    case TMC2160_1_4_STEP:
      data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 4);
      break;
      
    case TMC2160_1_8_STEP:
      data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 8);
      break;
      
    case TMC2160_1_16_STEP:
      data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 16);
      break;
      
    case TMC2160_1_32_STEP:
      data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 32);
      break;
      
    case TMC2160_1_64_STEP:
      data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 64);
      break;
      
    case TMC2160_1_128_STEP:
      data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 128);
      break;
      
    case TMC2160_1_256_STEP:
      data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 256);
      break;
  }
  
  return data_tmc2160;
}




void ptz_hori_start(char direction, float speed)
{
  BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
  
  g_ptz.hori_dec_mode = PTZ_DEC_MAX;
  
  //设定转动速度
  g_ptz.hori_speed_set = speed;
  //设定转动方向
  g_ptz.hori_direction_last = g_ptz.hori_direction_set;
  g_ptz.hori_direction_set = direction;
  g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
  
  
  tmc2160_set_hori_dir(direction);
  //驱动芯片寄存器写数据
  tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, g_ptz.hori_tmc2160.microstep);
  g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep;
  
  g_ptz.hori_tmc2160.f  = 0;
  tmc2160_hori_motor_enable(TMC2160_MOTOR_ENABLE);
  g_ptz.hori_tmc2160.off_on =1;
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  g_ptz.hori_start_stop_set = PTZ_HORI_START;
  g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
    
  ptz_hori_stop_count = 0;
  BSP_OS_SemPost(&ptz_hori_stop_mutex);
}


void ptz_hori_stop(unsigned short int time)
{
  BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
  
  g_ptz.hori_dec_mode = PTZ_DEC_MAX;
  
  //设定转动速度
  g_ptz.hori_speed_set = 0;
  g_ptz.hori_speed_actual = 0;
  


  tmc2160_hori_motor_enable(TMC2160_MOTOR_DISABLE);
  g_ptz.hori_tmc2160.off_on = 0;
  
  ptz_hori_timer_stop(); 
    
  g_ptz.hori_tmc2160.f = 0;
  g_ptz.hori_speed_actual = 0;
  g_ptz.hori_tmc2160.off_on = 0;
  
  //停止电机
  g_ptz.hori_start_stop_set = PTZ_HORI_STOP;
  g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
  
  g_ptz.hori_direction_actual = PTZ_HORI_DIR_STOP;
  
//#ifdef PTZ_ELECTRIC_STABLE_ON//是否打开电子自锁
//    drv8711_hori_change_register(DRV8711_TORQUE_TORQUE, drv8711_parameter.hori_self_lock_torque);
//    drv8711_hori_change_register(DRV8711_CTRL_MODE, drv8711_parameter.hori_self_lock_micr);
//    g_ptz.hori_drv8711.actual_microstep = drv8711_parameter.hori_self_lock_micr;
//    drv8711_hori_enbl(DRV8711_MOTOR_ENABLE);
//#endif 
  
  if(ptz_hori_stop_count <= 0)
  {
    OSTimeDlyHMSM(0u, 0u, 0u, time);
    ptz_hori_stop_count = 0;
  }
  ptz_hori_stop_count ++;

  BSP_OS_SemPost(&ptz_hori_stop_mutex);
}



/// 云台水平转动监控
static char ptz_hori_rotate_monitor_task()
{
  static float BNP_D;//刹车最近点与当前位置的距离
  static float BFP_D;//刹车最远点与当前位置的距离
  static unsigned int k;
  TMC2160Control data;
  
  switch(g_ptz.hori_rotate_monitor_switch)
  {
    case 0://关闭
      
      break;
      
    case PTZ_HORI_RIGHT_KEEP://向右转动，不刹车
      data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed);
      //关闭转动监控
      g_ptz.hori_rotate_monitor_switch = 0;
      //首先判断云台是否在转动
      if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
      {
        
        
        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
        {
//          if(g_ptz.hori_tmc2160.actual_microstep == data.microstep)
//          {
            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//细分相同，直接赋值速度
//          }else{
//            
//            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//先改细分再赋值速度，，，解决转动过程中改变速度引起的停顿
//            g_ptz.hori_tmc2160 = data;
//            tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, g_ptz.hori_tmc2160.microstep);
//            g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep;
//          }
          
        }else{
          //如果转向不同
          //则先刹车
          if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
          {
            g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
            g_ptz.hori_dec_mode = PTZ_DEC_DEF;
          }
          for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms
          {
            if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
            {
              break;
            }
            OSTimeDlyHMSM(0u, 0u, 0u, 2u);
          }

          ptz_hori_stop(PTZ_HORI_STOP_TIME);
          //再启动
          g_ptz.hori_direction_set = PTZ_HORI_DIR_RIGHT;
          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
          g_ptz.hori_tmc2160 = data;
          ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set);
        }
        
        
//        //判断转动方向是否相同
//        //如果转向相同且细分程度相同，则直接修改设置的转速
//        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT && 
//           g_ptz.hori_tmc2160.actual_microstep == data.microstep)
//        {
//          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
//          g_ptz.hori_tmc2160 = data;
//        }
//        else//如果转向不同
//        {
//          //则先刹车
//          
//          if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
//          {
//            g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
//            g_ptz.hori_dec_mode = PTZ_DEC_DEF;
//          }
//          for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms
//          {
//            if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
//            {
//              break;
//            }
//            OSTimeDlyHMSM(0u, 0u, 0u, 2u);
//          }
//
//          ptz_hori_stop(PTZ_HORI_STOP_TIME);
//          //再启动
//          g_ptz.hori_direction_set = PTZ_HORI_DIR_RIGHT;
//          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
//          g_ptz.hori_tmc2160 = data;
//          ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set);
//        }
      }
      else//没有转动
      { 
        g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
        g_ptz.hori_tmc2160 = data;
        ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set);
      }
      break;
      
    case PTZ_HORI_LEFT_KEEP://向左转动，不刹车
      data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed);
      //关闭转动监控
      g_ptz.hori_rotate_monitor_switch = 0;
      //首先判断云台是否在转动
      if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
      {
        
        //判断转动方向是否相同
        //如果转向相同且细分程度相同，则直接修改设置的转速
        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
        {
//          if(g_ptz.hori_tmc2160.actual_microstep == data.microstep)
//          {
            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//细分相同，直接赋值速度
//          }else{
//            g_ptz.hori_tmc2160 = data;
//            tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, g_ptz.hori_tmc2160.microstep);
//            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//先改细分再赋值速度
//            g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep;
//          }
        }
        else//如果转向不同
        {
          //则先刹车
          if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
          {
            g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
            g_ptz.hori_dec_mode = PTZ_DEC_DEF;
          }
          for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms
          {
            if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
            {
              break;
            }
            OSTimeDlyHMSM(0u, 0u, 0u, 2u);
          }

          ptz_hori_stop(PTZ_HORI_STOP_TIME);
          //再启动
          g_ptz.hori_direction_set = PTZ_HORI_DIR_LEFT;
          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
          g_ptz.hori_tmc2160 = data;
          ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set);
        }
        
        
        
        
        
//        //判断转动方向是否相同
//        //如果转向相同且细分程度相同，则直接修改设置的转速
//        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT && 
//           g_ptz.hori_tmc2160.actual_microstep == data.microstep)
//        {
//          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
////          g_ptz.hori_tmc2160 = data;
//        }
//        else//如果转向不同
//        {
//          //则先刹车
//          if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
//          {
//            g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
//            g_ptz.hori_dec_mode = PTZ_DEC_DEF;
//          }
//          for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms
//          {
//            if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
//            {
//              break;
//            }
//            OSTimeDlyHMSM(0u, 0u, 0u, 2u);
//          }
//
//          ptz_hori_stop(PTZ_HORI_STOP_TIME);
//          //再启动
//          g_ptz.hori_direction_set = PTZ_HORI_DIR_LEFT;
//          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
//          g_ptz.hori_tmc2160 = data;
//          ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set);
//        }
      }
      else//没有转动
      {
        g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
        g_ptz.hori_tmc2160 = data;
        ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set);
      }
      break;
      
    case PTZ_HORI_GO_TO_ANGLE_A://角度定位
    case PTZ_HORI_RIGHT_ANGLE:
    case PTZ_HORI_LEFT_ANGLE:
    case PTZ_HORI_MIN_DISTANCE:
    case PTZ_HORI_MAX_DISTANCE:
    data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed);
      //首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动
      if(g_ptz.hori_rotate_plan.rotate_switch == 1)
      {
         g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
        //如果不是处于同一个位置
        //判断是否转动
        //处于转动
        if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
        {
          //判断转向是否相同
         if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
          {

//            if(g_ptz.hori_tmc2160.actual_microstep == data.microstep)
//            {//如果转向相同且细分程度相同
              g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//细分相同，直接赋值速度
//            }else{
//              g_ptz.hori_tmc2160 = data;
//              tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, g_ptz.hori_tmc2160.microstep);
//              g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//先改细分再赋值速度
//              g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep;
//            }
            
          }
          else//转向不同，则直接修改设置的转速
          {
            //则先刹车
            if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
            {
              g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              g_ptz.hori_dec_mode = PTZ_DEC_DEF;
            }
            for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms
            {
              if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
              {
                break;
              }
              OSTimeDlyHMSM(0u, 0u, 0u, 2u);
            }
            ptz_hori_stop(PTZ_HORI_STOP_TIME);
            //再启动
            g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
            g_ptz.hori_tmc2160 = data;
            ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
          }
        }
        else//没有转动
        {
          g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
          g_ptz.hori_tmc2160 = data;
          ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
        }
        
        //如果离最近刹车距离小于一定程度，则以最小转速转动
        //data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED);
        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
        {
          if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
          {
            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
          else
          {
            if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
        }
        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
        {
          if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
          {
            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
          else
          {
            if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= 
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
        }
        
       
      }
      else//g_ptz.hori_rotate_plan.rotate_switch == 0;
      {
        g_ptz.hori_rotate_monitor_switch = 0;
        //不需要转动则刹车停止，关闭监控
        g_ptz.hori_arrive_flag = 0;//转动到指定位置
        ptz_location_return_return(LOCATION_HORI);//定位回传
        ptz_preset_bit_location_return_return();//预置位到位回传
        if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
        {
          ptz_hori_stop(PTZ_HORI_STOP_TIME);
        }
      }
      break;
    
    case PTZ_HORI_RIGHT_CYCLE:
    case PTZ_HORI_LEFT_CYCLE:
      if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
      {
        //判断转向是否相同
        //如果转向相同
        if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
        {
          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
        }
        else//转向不同，则直接修改设置的转速
        {
          //则先刹车
          ptz_hori_stop(PTZ_HORI_STOP_TIME);
          //再启动
          g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
          ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
        }
      }
      else//没有转动
      {
        g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
        g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
        ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
      }
      g_ptz.hori_rotate_monitor_switch = PTZ_HORI_CYCLE;
      break;
      
    case PTZ_HORI_CYCLE:
      BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle);
      if(BNP_D > 360.0 / 2.0)
      {
        BNP_D = 360.0 - BNP_D;
      }
      BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle);
      if(BFP_D > 360.0 / 2.0)
      {
        BFP_D = 360.0 - BFP_D;
      }
      /*定位*/
      if(BNP_D + BFP_D > (g_ptz.hori_rotate_plan.stop_angle_range + 2.0))
      {
        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
      }
      break;  
      
    case PTZ_HORI_GO_TO_ANGLE_B:
      BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle);
      if(BNP_D > 360.0 / 2.0)
      {
        BNP_D = 360.0 - BNP_D;
      }
      BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle);
      if(BFP_D > 360.0 / 2.0)
      {
        BFP_D = 360.0 - BFP_D;
      }
      /*定位*/
      if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01))
      {
        if(g_ptz.hori_repeat_locate_switch == 0)//重复定位关闭
        {
          g_ptz.hori_arrive_flag = 0;//转到指定位置
          ptz_location_return_return(LOCATION_HORI);//定位回传
          ptz_preset_bit_location_return_return();//预置位到位回传
        }
        g_ptz.hori_rotate_monitor_switch = 0;
        if(g_ptz.hori_repeat_locate_switch == 1)//重复定位打开
        {
          g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_A;
        }
        ptz_hori_stop(PTZ_HORI_STOP_TIME);
      }
      //如果离最近刹车距离小于一定程度，则以最小转速转动
        //data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED);
        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
        {
          if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
          {
            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
          else
          {
            if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
        }
        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
        {
          if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
          {
            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
          else
          {
            if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= 
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
        }
      break;
      
    case PTZ_HORI_BRAKE://停止转动
      g_ptz.hori_rotate_monitor_switch = 0;
      ptz_hori_stop(PTZ_HORI_STOP_TIME);
      break;
    
    case PTZ_HORI_DEC_BRAKE_A://减速刹车
      //g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
      data = ptz_hori_choice_microstep(PTZ_HORI_BREAK_SPEED);
      if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
      {
        g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
        g_ptz.hori_dec_mode = PTZ_DEC_MAX;
      }
      g_ptz.hori_rotate_monitor_switch = PTZ_HORI_DEC_BRAKE_B;
      k = 0;
      break;
      
    case PTZ_HORI_DEC_BRAKE_B:
       k++;//K增加1，时间增加1ms
      if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1 ||
         k >= (tmc2160_parameter.hori_dec_time_max / 1.0))
      {
        k = 0;
        g_ptz.hori_rotate_monitor_switch = 0;
        ptz_hori_stop(PTZ_HORI_STOP_TIME);
      }

      break;
    
    //重复定位路径规划
    case  PTZ_HORI_REPEAT_LOCATE_A:
      ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
      g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_B;
      break;
    //重复定位启动控制
    case  PTZ_HORI_REPEAT_LOCATE_B:
      //首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动
      data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed);
      if(g_ptz.hori_rotate_plan.rotate_switch == 1)
      {
        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_C;
        //如果不是处于同一个位置
        //判断是否转动
        //处于转动
        if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
        {
          //判断转向是否相同
          //如果转向相同且细分程度相同
          if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction && 
             g_ptz.hori_tmc2160.actual_microstep == data.microstep)
          {
            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
//            g_ptz.hori_tmc2160 = data;
          }
          else//转向不同，则直接修改设置的转速
          {
            //则先刹车
            if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
            {
              g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              g_ptz.hori_dec_mode = PTZ_DEC_DEF;
            }
            for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms
            {
              if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
              {
                break;
              }
              OSTimeDlyHMSM(0u, 0u, 0u, 2u);
            }
            
            ptz_hori_stop(PTZ_HORI_STOP_TIME);
            //再启动
            g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
            g_ptz.hori_tmc2160 = data;
            ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
          }
        }
        else//没有转动
        {
          g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
          g_ptz.hori_tmc2160 = data;
          ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
        }
        
        //如果离最近刹车距离小于一定程度，则以最小转速转动
        //data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED);
        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
        {
          if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
          {
            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
          else
          {
            if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
        }
        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
        {
          if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
          {
            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
          else
          {
            if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= 
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
        }
        
      }
      else//g_ptz.hori_rotate_plan.rotate_switch == 0;
      {
        g_ptz.hori_rotate_monitor_switch = 0;
        //不需要转动则刹车停止，关闭监控
        g_ptz.hori_arrive_flag = 0;//转到指定预位置
        ptz_location_return_return(LOCATION_HORI);//定位回传
        ptz_preset_bit_location_return_return();//预置位到位回传
        if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
        {
          ptz_hori_stop(PTZ_HORI_STOP_TIME);
        }
      }
      break;
    //重复定位判断是否达到定位位置
    case  PTZ_HORI_REPEAT_LOCATE_C:
      BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle);
      if(BNP_D > 360.0 / 2.0)
      {
        BNP_D = 360.0 - BNP_D;
      }
      BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle);
      if(BFP_D > 360.0 / 2.0)
      {
        BFP_D = 360.0 - BFP_D;
      }
      /*定位*/
      if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01))
      {
        g_ptz.hori_arrive_flag = 0;//转到指定位置
        ptz_location_return_return(LOCATION_HORI);//定位回传
        ptz_preset_bit_location_return_return();//预置位到位回传
        g_ptz.hori_rotate_monitor_switch = 0;
        ptz_hori_stop(PTZ_HORI_STOP_TIME);
      }
      //如果离最近刹车距离小于一定程度，则以最小转速转动
      //data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED);
      if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
      {
        if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
        {
          if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
             PTZ_HORI_BREAK_SPEED_ANGLE)
          {
            if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
            {
              g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
            }
          }
        }
        else
        {
          if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
             PTZ_HORI_BREAK_SPEED_ANGLE)
          {
            if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
            {
              g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
            }
          }
        }
      }
      if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
      {
        if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
        {
          if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
             PTZ_HORI_BREAK_SPEED_ANGLE)
          {
            if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
            {
              g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
            }
          }
        }
        else
        {
          if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= 
             PTZ_HORI_BREAK_SPEED_ANGLE)
          {
            if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
            {
              g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
            }
          }
        }
      }
      break;
      
    case PTZ_HORI_RIGHT_ANGLE_INC:
      data = ptz_hori_choice_microstep(PTZ_HORI_BREAK_SPEED);
      //首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动
      if(g_ptz.hori_rotate_plan.rotate_switch == 1)
      {
        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
        //如果不是处于同一个位置
        //判断是否转动
        //处于转动
        if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
        {
          //判断转向是否相同
          //如果转向相同
          if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction && 
             g_ptz.hori_tmc2160.actual_microstep == data.microstep)
          {
            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
//            g_ptz.hori_tmc2160 = data;
          }
          else//转向不同，则直接修改设置的转速
          {
            //则先刹车
            if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
            {
              g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              g_ptz.hori_dec_mode = PTZ_DEC_DEF;
            }
            for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms
            {
              if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
              {
                break;
              }
              OSTimeDlyHMSM(0u, 0u, 0u, 2u);
            }
            
            ptz_hori_stop(PTZ_HORI_STOP_TIME);
            //再启动
            g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
            g_ptz.hori_tmc2160 = data;
            ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
          }
        }
        else//没有转动
        {
          g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
          ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
        }
        
        
        //data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED);
        //如果离最近刹车距离小于一定程度，则以最小转速转动
        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
        {
          if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
          {
            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
          else
          {
            if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
        }
        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
        {
          if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
          {
            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
          else
          {
            if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= 
               PTZ_HORI_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
              {
                g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
              }
            }
          }
        }
        
        
      }
      else//g_ptz.hori_rotate_plan.rotate_switch == 0;
      {
        g_ptz.hori_rotate_monitor_switch = 0;
        //不需要转动则刹车停止，关闭监控
        g_ptz.hori_arrive_flag = 0;//转动到指定位置
        ptz_location_return_return(LOCATION_HORI);//定位回传
        ptz_preset_bit_location_return_return();//预置位到位回传
        if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
        {
          ptz_hori_stop(PTZ_HORI_STOP_TIME);
        }
      }
      break;
      
    case PTZ_HORI_LEFT_ANGLE_INC:
      
      break;
    
  }
  return 1;
}




void ptz_vert_start(char direction, float speed)
{
  BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
  
  g_ptz.vert_dec_mode = PTZ_DEC_MAX;
  
  //设定转动速度
  g_ptz.vert_speed_set = speed;
  //设定转动方向
  g_ptz.vert_direction_last = g_ptz.vert_direction_set;
  g_ptz.vert_direction_set = direction;
  g_ptz.vert_direction_actual = g_ptz.vert_direction_set;
  
  
  
  tmc2160_set_vert_dir(direction);
  //写寄存器数据
  tmc2160_write_register(TMC2160_VERT, TMC2160_CHOPCONF, g_ptz.vert_tmc2160.microstep);
  g_ptz.vert_tmc2160.actual_microstep = g_ptz.vert_tmc2160.microstep;

  g_ptz.vert_tmc2160.f = 0;
  tmc2160_vert_motor_enable(TMC2160_MOTOR_ENABLE);
  g_ptz.vert_tmc2160.off_on = 1;
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  g_ptz.vert_start_stop_set = PTZ_VERT_START;
  g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set;
    
  ptz_vert_stop_count = 0;
  BSP_OS_SemPost(&ptz_vert_stop_mutex);
}


void ptz_vert_stop(unsigned short int time)
{
  BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
  
  g_ptz.vert_dec_mode = PTZ_DEC_MAX;
  
  if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
  {
    //设定转动速度
    g_ptz.vert_speed_set = 0;
    g_ptz.vert_speed_actual = 0;


    //若需要自锁，则驱动器要保持使能状态
    tmc2160_vert_motor_enable(TMC2160_MOTOR_DISABLE);
    g_ptz.vert_tmc2160.off_on = 0;
    
    ptz_vert_timer_stop(); 
    
    g_ptz.vert_tmc2160.f = 0;
    g_ptz.vert_speed_actual = 0;
    g_ptz.vert_tmc2160.off_on = 0;
    
     //停止电机
    g_ptz.vert_start_stop_set = PTZ_VERT_STOP;
    g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set;
    
    g_ptz.vert_direction_actual = PTZ_VERT_DIR_STOP;
    
//#ifdef PTZ_ELECTRIC_STABLE_ON//是否打开电子自锁
//    drv8711_vert_change_register(DRV8711_TORQUE_TORQUE, drv8711_parameter.vert_self_lock_torque);
//    drv8711_vert_change_register(DRV8711_CTRL_MODE, drv8711_parameter.vert_self_lock_micr);
//    g_ptz.vert_drv8711.actual_microstep = drv8711_parameter.vert_self_lock_micr;
//    drv8711_vert_enbl(DRV8711_MOTOR_ENABLE);
//#endif
    
    if(ptz_vert_stop_count <= 0)
    {
      OSTimeDlyHMSM(0u, 0u, 0u, time);
      ptz_vert_stop_count = 0;
    }
    ptz_vert_stop_count ++;
  }
  
  BSP_OS_SemPost(&ptz_vert_stop_mutex);
}


/// 云台垂直转动监控
static char ptz_vert_rotate_monitor_task()
{
  static float BNP_D;//刹车最近点与当前位置的距离
  static float BFP_D;//刹车最远点与当前位置的距离
  static char cmd_type;
  static unsigned int k;
  TMC2160Control data;
  switch(g_ptz.vert_rotate_monitor_switch)
  {
    case 0://关闭
      
      break;
      
    case PTZ_VERT_GO_TO_ANGLE_A://转动到指定角度启动
    case PTZ_VERT_ANGLE:
    case PTZ_VERT_UP_KEEP:
    case PTZ_VERT_DOWN_KEEP:
      data = ptz_vert_choice_microstep(g_ptz.vert_rotate_plan.max_speed);
      cmd_type = g_ptz.vert_rotate_monitor_switch;
      g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_B;
      if(g_ptz.vert_rotate_plan.rotate_switch == 1)
      {
        //首先判断是否转动
        //处于转动
        if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
        {
          //判断转向是否相同
          //如果转向相同且步进电机细分程度相同
          if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction)
          {
//            g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
            
//            if(g_ptz.vert_tmc2160.actual_microstep == data.microstep)
//            {//如果转向相同且细分程度相同
              g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;//细分相同，直接赋值速度
//            }else{
//              g_ptz.vert_tmc2160 = data;
//              tmc2160_write_register(TMC2160_VERT, TMC2160_CHOPCONF, g_ptz.vert_tmc2160.microstep);
//              g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;//先改细分再赋值速度
//              g_ptz.vert_tmc2160.actual_microstep = g_ptz.vert_tmc2160.microstep;
//            }
            
          }
          else//转向不同
          {
            //则先刹车
            
            if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
            {
              g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
              g_ptz.vert_dec_mode = PTZ_DEC_DEF;
            }
            for(k = 0; k <= (PTZ_VERT_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms
            {
              if(g_ptz.vert_speed_actual <= PTZ_VERT_BREAK_SPEED * 1.1)
              {
                break;
              }
              
#ifdef PTZ_PHOTOELECTRIC_SWITCH
              if(g_ptz.vert_ps_sw1_state == PS_COVER)//下俯极限位置
              {
                if(g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN)
                {
                  break;
                }
              }
              if(g_ptz.vert_ps_sw2_state == PS_COVER)//上仰极限位置
              {
                if(g_ptz.vert_direction_set == PTZ_VERT_DIR_UP)
                {
                  break;
                }
              }
#endif
                 
              OSTimeDlyHMSM(0u, 0u, 0u, 2u);
            }
            
            
            ptz_vert_stop(PTZ_VERT_STOP_TIME);
            //再启动
            g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
            g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
            g_ptz.vert_tmc2160 = data;
            
            //如果离最近刹车距离小于一定程度，则以最小转速转动
            if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
               PTZ_VERT_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
              {
                g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
                data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED);
                 g_ptz.vert_tmc2160 = data;
              }
            }
            
            ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
          }
        }
        else//没有转动
        {
          g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
          g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
          g_ptz.vert_tmc2160 = data;
          
          //如果离最近刹车距离小于一定程度，则以最小转速转动
          if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
             PTZ_VERT_BREAK_SPEED_ANGLE)
          {
            if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
            {
              g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
              data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED);
              g_ptz.vert_tmc2160 = data;
            }
          }
          
          ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
        }
      }
      else//g_ptz.vert_rotate_plan.rotate_switch == 0
      {
        //刹车关闭转动
        g_ptz.vert_arrive_flag = 0;//转到指定预位置
        ptz_location_return_return(LOCATION_VERT);//定位回传
        ptz_preset_bit_location_return_return();//预置位到位回传
        g_ptz.vert_rotate_monitor_switch = 0;
        if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
        {
          ptz_vert_stop(PTZ_VERT_STOP_TIME);
        }
      }
      break;
      
    case PTZ_VERT_GO_TO_ANGLE_B://转动到指定角度定位
      BNP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle);
      BFP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_far_angle);
      /*定位*/
      if(BNP_D + BFP_D <= (g_ptz.vert_rotate_plan.stop_angle_range + 0.01))
      {
        if(g_ptz.vert_repeat_locate_switch == 0 &&
           cmd_type != PTZ_VERT_UP_KEEP &&
           cmd_type != PTZ_VERT_DOWN_KEEP)
        {
          g_ptz.vert_arrive_flag = 0;//转到指定预位置
          ptz_location_return_return(LOCATION_VERT);//定位回传
          ptz_preset_bit_location_return_return();//预置位到位回传
        }
        g_ptz.vert_rotate_monitor_switch = 0;
        if(g_ptz.vert_repeat_locate_switch == 1 &&
           cmd_type != PTZ_VERT_UP_KEEP &&
           cmd_type != PTZ_VERT_DOWN_KEEP)
        {
          g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_A;
        }
        ptz_vert_stop(PTZ_VERT_STOP_TIME);
      }
      //如果离最近刹车距离小于一定程度，则以最小转速转动
      //data = ptz_choice_microstep(PTZ_VERT_BREAK_SPEED);
      if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
         PTZ_VERT_BREAK_SPEED_ANGLE)
      {
        if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
        {
          g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
          //g_ptz.vert_drv8711.set_f = data.set_f;
        }
      }
      break;
      
    case PTZ_VERT_BRAKE://刹车
      g_ptz.vert_rotate_monitor_switch = 0;
      ptz_vert_stop(PTZ_VERT_STOP_TIME);
      break;
      
    case PTZ_VERT_DEC_BRAKE_A://垂直减速刹车
      g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
      data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED);
      if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
      {
        g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
        g_ptz.vert_dec_mode = PTZ_DEC_MAX;
      }
      g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DEC_BRAKE_B;
      k = 0;
      break;
      
    case PTZ_VERT_DEC_BRAKE_B: 
      k ++;//K增加1,时间增加1ms 
      if(g_ptz.vert_speed_actual <= PTZ_VERT_BREAK_SPEED * 1.1 ||
         k >= (tmc2160_parameter.vert_dec_time_max / 1.0))
      {
        k = 0;
        g_ptz.vert_rotate_monitor_switch = 0;
        ptz_vert_stop(PTZ_VERT_STOP_TIME);
      }
      break;
      
     //重复定位路径规划
    case PTZ_VERT_REPEAT_LOCATE_A:
      ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
      g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_B;
      break;
    //重复定位转动启动  
    case PTZ_VERT_REPEAT_LOCATE_B:
      data = ptz_vert_choice_microstep(g_ptz.vert_rotate_plan.max_speed);
      if(g_ptz.vert_rotate_plan.rotate_switch == 1)
      {
        g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_C;
        //首先判断是否转动
        //处于转动
        if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
        {
          //判断转向是否相同
          //如果转向相同
          if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction && 
             g_ptz.vert_tmc2160.actual_microstep == data.microstep)
          {
            g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
          }
          else//转向不同，则直接修改设置的转速
          {
            //则先刹车
            if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
            {
              g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
              g_ptz.vert_dec_mode = PTZ_DEC_DEF;
            }
            for(k = 0; k <= (PTZ_VERT_DEC_TIME_DEF / 2.0); k++)//K增加1，时间增加2ms
            {
              if(g_ptz.vert_speed_actual <= PTZ_VERT_BREAK_SPEED * 1.1)
              {
                break;
              }
              
#ifdef PTZ_PHOTOELECTRIC_SWITCH
              if(g_ptz.vert_ps_sw1_state == PS_COVER)//下俯极限位置
              {
                if(g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN)
                {
                  break;
                }
              }
              if(g_ptz.vert_ps_sw2_state == PS_COVER)//上仰极限位置
              {
                if(g_ptz.vert_direction_set == PTZ_VERT_DIR_UP)
                {
                  break;
                }
              }
#endif
            
             
              OSTimeDlyHMSM(0u, 0u, 0u, 2u);
            }
            
            ptz_vert_stop(PTZ_VERT_STOP_TIME);
            //再启动
            g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
            g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
            g_ptz.vert_tmc2160 = data;
            
            //如果离最近刹车距离小于一定程度，则以最小转速转动
            if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
               PTZ_VERT_BREAK_SPEED_ANGLE)
            {
              if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
              {
                g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
                data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED);
                g_ptz.vert_tmc2160 = data;
              }
            }
            
            ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
          }
        }
        else//没有转动
        {
          g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
          g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
          g_ptz.vert_tmc2160 = data;
          
          //如果离最近刹车距离小于一定程度，则以最小转速转动
          if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
             PTZ_VERT_BREAK_SPEED_ANGLE)
          {
            if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
            {
              g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
              data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED);
              g_ptz.vert_tmc2160 = data;
            }
          }
          
          ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
        }
      }
      else//g_ptz.vert_rotate_plan.rotate_switch == 0
      {
        //刹车关闭转动
        g_ptz.vert_arrive_flag = 0;//转到指定位置
        ptz_location_return_return(LOCATION_VERT);//定位回传
        ptz_preset_bit_location_return_return();//预置位到位回传
        g_ptz.vert_rotate_monitor_switch = 0;
        if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
        {
          ptz_vert_stop(PTZ_VERT_STOP_TIME);
        }
      }
      break;
      
    case PTZ_VERT_REPEAT_LOCATE_C:
      BNP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle);
      BFP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_far_angle);
      /*定位*/
      if(BNP_D + BFP_D <= (g_ptz.vert_rotate_plan.stop_angle_range + 0.01))
      {
        g_ptz.vert_arrive_flag = 0;//预置位扫描，转到指定预置位
        ptz_location_return_return(LOCATION_VERT);//定位回传
        ptz_preset_bit_location_return_return();//预置位到位回传
        g_ptz.vert_rotate_monitor_switch = 0;
        ptz_vert_stop(PTZ_VERT_STOP_TIME);
      }
      //如果离最近刹车距离小于一定程度，则以最小转速转动
      if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
         PTZ_VERT_BREAK_SPEED_ANGLE)
      {
        if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
        {
          g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
        }
      }
      break;
  }

  //当云台垂直方向处于极限位置时，防止垂直方向角度越界
#ifdef PTZ_PHOTOELECTRIC_SWITCH
  if(g_ptz.vert_ps_sw1_state == PS_COVER)//下俯极限位置
  {
    if(g_ptz.vert_start_stop_set == PTZ_VERT_START 
       && g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN)
    {
      g_ptz.vert_arrive_flag = 0;
      g_ptz.vert_rotate_monitor_switch = 0;
      ptz_vert_stop(PTZ_VERT_STOP_TIME);
    }
  }
  if(g_ptz.vert_ps_sw2_state == PS_COVER)//上仰极限位置
  {
    if(g_ptz.vert_start_stop_set == PTZ_VERT_START 
       && g_ptz.vert_direction_set == PTZ_VERT_DIR_UP)
    {
      g_ptz.vert_arrive_flag = 0;
      g_ptz.vert_rotate_monitor_switch = 0;
      ptz_vert_stop(PTZ_VERT_STOP_TIME);
    }
  }
#endif
  
  return 1;
}



#endif






static void ptz_hori_rotate_task()
{
  while(1)
  {
    ptz_hori_rotate_monitor_task();
    OSTimeDlyHMSM(0u, 0u, 0u, 1u);
  }
}


static void ptz_vert_rotate_task()
{
  while(1)
  {
    ptz_vert_rotate_monitor_task();
    OSTimeDlyHMSM(0u, 0u, 0u, 1u);
  }
}

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



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



void init_rotate_monitor_module(void)
{
  BSP_OS_SemCreate(&ptz_hori_stop_mutex,1u,"ptz_hori_stop_mutex");
  BSP_OS_SemCreate(&ptz_vert_stop_mutex,1u,"ptz_vert_stop_mutex");
#ifdef TMC2160
  tmc2160_init();
  h_tmc2160_power_on();
#endif
  creat_task_hori_rotate();
  creat_task_vert_rotate();
}


#endif














