///Copyright (c) 2022, 四川汇源光通信有限公司 
///All rights reserved. 
///@file        service_selfcheck.C 
///@brief       云台开机自检程序，包括水平全范围自检、简易自检，垂直全范围、简易
///           自检
///
///@details     
///@note        
///@author      ypc
///@date        2019/05/23 
///
///@version		v1.0     2019/05/23 初始版本
///             Hyt2.1.0 2022/06/02  更换GD32单片，文件移植

#include "service_selfcheck.h"
#include "bsp_os.h"
#include "ptz_global_variable.h"
#include "angle_poweroffsave.h"
#include "comm_cfginfo.h"
#include "tmc2160.h"
#include "service_error_count.h"

#include "rotate_servo.h"
#include "speed_to_servoMotor.h"

#ifdef  PTZ_BLDC_MOTOR
/// 云台全范围自检
static unsigned char ptz_hori_complete_self_check_task()
{
  switch(g_ptz.hori_self_check)
  {
    //首先让云台水平向右转动
    case PTZ_HORI_SELF_CHECK_COMPLETE_STEP:
      if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
      {
        ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1;
      }
      else//没有被挡住
      {
        g_ptz.hori_ps_sw3_right_fall = 0; 
        g_ptz.hori_ps_sw3_left_rise  = 0; 
        g_ptz.hori_ps_sw3_right_rise = 0; 
        g_ptz.hori_ps_sw3_left_fall  = 0; 
        ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2;
      }
      break;
    
    case PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1:
      if(g_ptz.hori_ps_sw3_state == PS_NO_COVER)
      {
        OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
        ptz_hori_stop(PTZ_HORI_STOP_TIME);
        g_ptz.hori_ps_sw3_right_fall = 0; 
        g_ptz.hori_ps_sw3_left_rise  = 0; 
        g_ptz.hori_ps_sw3_right_rise = 0; 
        g_ptz.hori_ps_sw3_left_fall  = 0; 
        ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2;
      }
      break;
   
    //等待第一次水平光电开关下降沿中断
    case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2):
      if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
      {
        g_ptz.hori_as5047d.as5047d_data_reset = 1;
        g_ptz.hori_ps_sw3_right_fall = 0;
        g_ptz.hori_self_check++;
        
        OSTimeDlyHMSM(0u, 0u, 5u, 0u);
        g_ptz.hori_ps_sw3_right_fall = 0;
      }
      break;
      
    //等待第二次水平光电开关下降沿中断
    case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 3):
      if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
      { 
         g_ptz.hori_as5047d.as5047d_cycle_num = g_ptz.hori_as5047d.as5047d_cycle_count;
         g_ptz.hori_as5047d.as5047d_ptz_angle_max = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
         g_ptz.hori_as5047d.as5047d_ptz_angle_K = 360.0 / g_ptz.hori_as5047d.as5047d_ptz_angle_max;
         
         g_ptz.hori_as5047d.as5047d_remain_angle = g_ptz.hori_as5047d.as5047d_ptz_angle_max - 
         g_ptz.hori_as5047d.as5047d_cycle_num * 360.0;
         g_ptz.hori_angleP.angle_allow_max = 360.0;
         g_ptz.hori_angleP.angle_allow_min = 0.0;
         g_ptz.hori_as5047d.as5047d_state = 1;
  
         g_ptz.hori_ps_sw3_right_fall = 0; 
         g_ptz.hori_ps_sw3_left_rise  = 0; 
         g_ptz.hori_ps_sw3_right_rise = 0; 
         g_ptz.hori_ps_sw3_left_fall  = 0; 
         ptz_hori_stop(PTZ_HORI_STOP_TIME);
         g_ptz.hori_angle_state = 1;
         g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE;
      }
      break;
  }
  return g_ptz.hori_self_check;
}

/// 云台水平简化自检
static unsigned char ptz_hori_simplify_self_check_task()
{
  switch(g_ptz.hori_self_check)
  {
    case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP:
      if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
      {
        ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1;
      }
      else//没有挡住
      {
        g_ptz.hori_ps_sw3_right_fall = 0; 
        g_ptz.hori_ps_sw3_left_rise  = 0; 
        g_ptz.hori_ps_sw3_right_rise = 0; 
        g_ptz.hori_ps_sw3_left_fall  = 0; 
        ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2;
      }
      break;
      
    case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1://向左转动，直到光电开关脱离挡片遮挡
      if(g_ptz.hori_ps_sw3_state == PS_NO_COVER)
      {
        OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
        ptz_hori_stop(PTZ_HORI_STOP_TIME);
        g_ptz.hori_ps_sw3_right_fall = 0; 
        g_ptz.hori_ps_sw3_left_rise  = 0; 
        g_ptz.hori_ps_sw3_right_rise = 0; 
        g_ptz.hori_ps_sw3_left_fall  = 0; 
        ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2;
      }
      break; 
     
    case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2:
      //等待水平光电开关右转下降沿
      if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count = 0;
        g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
        g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
        g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
        g_ptz.hori_as5047d.as5047d_state = 1;
        g_ptz.hori_ps_sw3_right_fall = 0; 
        g_ptz.hori_ps_sw3_left_rise  = 0; 
        g_ptz.hori_ps_sw3_right_rise = 0; 
        g_ptz.hori_ps_sw3_left_fall  = 0; 
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3;
      }
      break; 
      
    case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3://转动到0位
      OSTimeDlyHMSM(0u, 0u, 1u, 0u);//延时转动
      g_ptz.hori_angle_control = 0.0;
      g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
      ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
      g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_A;
      g_ptz.hori_angle_state = 1;
      g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
      break;  
  }
  return g_ptz.hori_self_check;
}


/// 云台垂直全范围自检
static unsigned char ptz_vert_complete_self_check_task()
{
   switch(g_ptz.vert_self_check)
  {
    ///首先读取光电开关的状态，初步判断垂直状态
    case PTZ_VERT_SELF_CHECK_COMPLETE_STEP:
      g_ptz.vert_ps_sw1_down_fall = 0;
      g_ptz.vert_ps_sw1_up_rise = 0;
      g_ptz.vert_ps_sw2_up_fall = 0;
      g_ptz.vert_ps_sw2_down_rise = 0;
      if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置
      {
        ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1; 
      }
      if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置
      {
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2;
      }
      if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER)
      //云台处于中间位置
      {
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2;
      }
      break;
      
    case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1):
      if(g_ptz.vert_ps_sw1_state == PS_NO_COVER)
      {
        OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
        ptz_vert_stop(PTZ_VERT_STOP_TIME);
        g_ptz.vert_ps_sw1_down_fall = 0;
        g_ptz.vert_ps_sw1_up_rise = 0;
        g_ptz.vert_ps_sw2_up_fall = 0;
        g_ptz.vert_ps_sw2_down_rise = 0;
        g_ptz.vert_self_check ++;
      }
      break;
      //开启向下转动，查找下俯极限位置
    case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2):
      ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
      g_ptz.vert_self_check  ++;
      break;
      
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
    //判断是否到达下俯极限位置,然后云台开始向上转动
    case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3):  
        if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
           g_ptz.vert_ps_sw1_state == PS_COVER)
        {
          g_ptz.vert_ps_sw1_down_fall = 0;
          g_ptz.vert_as5047d.as5047d_data_reset = 1;
          ptz_vert_stop(PTZ_VERT_STOP_TIME);
          ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
          g_ptz.vert_self_check  = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10;
        }
      break;
#endif
      
#ifdef PTZ_SW1_UP_RISE_UPDATE
     //判断是否到达下俯极限位置,然后云台开始向上转动
    case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3):  
        if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
           g_ptz.vert_ps_sw1_state == PS_COVER)
        {
          g_ptz.vert_ps_sw1_down_fall = 0;
          g_ptz.vert_ps_sw1_up_rise = 0;
          OSTimeDlyHMSM(0u, 0u, 0u, 300u);
          ptz_vert_stop(PTZ_VERT_STOP_TIME);
          ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
          g_ptz.vert_self_check++;
        }
      break; 
    //等待上仰上升沿  
    case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 4):  
      if(g_ptz.vert_ps_sw1_up_rise >= 1 ||
         g_ptz.vert_ps_sw1_state == PS_NO_COVER)
      {
        g_ptz.vert_as5047d.as5047d_data_reset = 1;
        g_ptz.vert_ps_sw1_down_fall = 0;
        g_ptz.vert_ps_sw1_up_rise = 0;
        g_ptz.vert_self_check  = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10;
      }
      break;
#endif

      //等待SW2下降沿,计算垂直转动范围
    case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10):
      if(g_ptz.vert_ps_sw2_up_fall >= 1 ||
         g_ptz.vert_ps_sw2_state == PS_COVER)
      {
        g_ptz.vert_as5047d.as5047d_cycle_num = g_ptz.vert_as5047d.as5047d_cycle_count;
        g_ptz.vert_as5047d.as5047d_ptz_angle_max = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
        
        g_ptz.vert_angleP.angle_range = 
          g_ptz.vert_as5047d.as5047d_ptz_angle_max / PTZ_VERT_BIG_GEAR_RATIO;
        
        g_ptz.vert_angleP.angle_allow_min =  g_ptz.vert_angleP.angle_range / 2.0 * (-1);
        g_ptz.vert_angleP.angle_allow_max =  g_ptz.vert_angleP.angle_range / 2.0;
        
        g_ptz.vert_as5047d.as5047d_ptz_angle_K = 1 / PTZ_VERT_BIG_GEAR_RATIO;
          
         
        g_ptz.vert_as5047d.as5047d_remain_angle = g_ptz.vert_as5047d.as5047d_ptz_angle_max - 
        g_ptz.vert_as5047d.as5047d_cycle_num * 360.0;
           
        g_ptz.vert_as5047d.as5047d_state = 1;
         
        g_ptz.vert_ps_sw1_down_fall = 0;
        g_ptz.vert_ps_sw1_up_rise = 0;
        g_ptz.vert_ps_sw2_up_fall = 0;
        g_ptz.vert_ps_sw2_down_rise = 0;

        ptz_vert_stop(PTZ_VERT_STOP_TIME); 
        g_ptz.vert_angle_state = 1;
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE;
      }
      break;
  }
  return g_ptz.vert_self_check;
}

/// 云台垂直简化自检
static unsigned char ptz_vert_simplify_self_check_task()
{
  switch(g_ptz.vert_self_check)
  {
    case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP:
      g_ptz.vert_ps_sw1_down_fall = 0;
      g_ptz.vert_ps_sw1_up_rise = 0;
      g_ptz.vert_ps_sw2_up_fall = 0;
      g_ptz.vert_ps_sw2_down_rise = 0;
      if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置
      {
        ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1; 
      }
      if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置
      {
        ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
      }
      if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER)
      //云台处于中间位置
      {
        ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
      }
      break;
      
    case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1:
      if(g_ptz.vert_ps_sw1_state == PS_NO_COVER)
      {
        OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
        ptz_vert_stop(PTZ_VERT_STOP_TIME);
        g_ptz.vert_ps_sw1_down_fall = 0;
        g_ptz.vert_ps_sw1_up_rise = 0;
        g_ptz.vert_ps_sw2_up_fall = 0;
        g_ptz.vert_ps_sw2_down_rise = 0;
        ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
      }
      break;

#ifdef PTZ_SW1_DOWN_FALL_UPDATE      
    case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2:
            if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
               g_ptz.vert_ps_sw1_state == PS_COVER)
            {//等待SW1光电开关被遮挡，同时更新当前位置
              g_ptz.vert_as5047d.as5047d_cycle_count = 0;
              g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
              g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
              g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
              g_ptz.vert_as5047d.as5047d_state = 1;
              g_ptz.vert_ps_sw1_down_fall = 0;
              g_ptz.vert_ps_sw1_up_rise = 0;
              g_ptz.vert_ps_sw2_up_fall = 0;
              g_ptz.vert_ps_sw2_down_rise = 0;
              ptz_vert_stop(PTZ_VERT_STOP_TIME);
              g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10;
            }
      break;
#endif
      
#ifdef PTZ_SW1_UP_RISE_UPDATE 
      //判断是否到达下俯极限位置,然后云台开始向上转动
    case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2): 
        if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
           g_ptz.vert_ps_sw1_state == PS_COVER)
        {
          g_ptz.vert_ps_sw1_down_fall = 0;
          g_ptz.vert_ps_sw1_up_rise = 0;
          g_ptz.vert_ps_sw2_up_fall = 0;
          g_ptz.vert_ps_sw2_down_rise = 0;
          OSTimeDlyHMSM(0u, 0u, 0u, 300u);
          ptz_vert_stop(PTZ_VERT_STOP_TIME);
          ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
          g_ptz.vert_self_check++;
        }
      break; 
      
    //等待上仰上升沿  
    case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 3):  
        if(g_ptz.vert_ps_sw1_up_rise >= 1 ||
           g_ptz.vert_ps_sw1_state == PS_NO_COVER)
        {
          //等待SW1光电开关被遮挡，同时更新当前位置
          g_ptz.vert_as5047d.as5047d_cycle_count = 0;
          g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
          g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
          g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
          g_ptz.vert_as5047d.as5047d_state = 1;
          g_ptz.vert_ps_sw1_down_fall = 0;
          g_ptz.vert_ps_sw1_up_rise = 0;
          g_ptz.vert_ps_sw2_up_fall = 0;
          g_ptz.vert_ps_sw2_down_rise = 0;
          g_ptz.vert_self_check  = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10;
        } 
      break; 
#endif
      
    case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10:
      OSTimeDlyHMSM(0u, 0u, 1u, 0u);
      g_ptz.vert_angle_control = 0.0;
      g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED;
      ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
      g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_A;
      g_ptz.vert_angle_state = 1;
      g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
      break;
  }   
  return g_ptz.vert_self_check;
}

#endif

#ifdef PTZ_STEP_MOTOR
  /// 云台全范围自检
  static unsigned char ptz_hori_complete_self_check_task()
  {
    TMC2160Control data;
    switch(g_ptz.hori_self_check)
    {
      //首先让云台水平向右转动
      case PTZ_HORI_SELF_CHECK_COMPLETE_STEP:
        data = ptz_hori_choice_microstep(PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_tmc2160 = data;
        if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
        {
          ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1;
        }
        else//没有被挡住
        {
          g_ptz.hori_ps_sw3_right_fall = 0; 
          g_ptz.hori_ps_sw3_left_rise  = 0; 
          g_ptz.hori_ps_sw3_right_rise = 0; 
          g_ptz.hori_ps_sw3_left_fall  = 0; 
          ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2;
        }
        break;
      
      case PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1:
        if(g_ptz.hori_ps_sw3_state == PS_NO_COVER)
        {
          OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
          ptz_hori_stop(PTZ_HORI_STOP_TIME);
          g_ptz.hori_ps_sw3_right_fall = 0; 
          g_ptz.hori_ps_sw3_left_rise  = 0; 
          g_ptz.hori_ps_sw3_right_rise = 0; 
          g_ptz.hori_ps_sw3_left_fall  = 0; 
          ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2;
        }
        break;
     
      //等待第一次水平光电开关下降沿中断
      case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2):
        if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
        {
          g_ptz.hori_as5047d.as5047d_data_reset = 1;
          g_ptz.hori_ps_sw3_right_fall = 0;
          g_ptz.hori_self_check++;
          
          OSTimeDlyHMSM(0u, 0u, 5u, 0u);
          g_ptz.hori_ps_sw3_right_fall = 0;
        }
        break;
        
      //等待第二次水平光电开关下降沿中断
      case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 3):
        if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
        { 
           g_ptz.hori_as5047d.as5047d_cycle_num = g_ptz.hori_as5047d.as5047d_cycle_count;
           g_ptz.hori_as5047d.as5047d_ptz_angle_max = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
           g_ptz.hori_as5047d.as5047d_ptz_angle_K = 360.0 / g_ptz.hori_as5047d.as5047d_ptz_angle_max;
           
           g_ptz.hori_as5047d.as5047d_remain_angle = g_ptz.hori_as5047d.as5047d_ptz_angle_max - 
             g_ptz.hori_as5047d.as5047d_cycle_num * 360.0;
           g_ptz.hori_angleP.angle_allow_max = 360.0;
           g_ptz.hori_angleP.angle_allow_min = 0.0;
           g_ptz.hori_as5047d.as5047d_state = 1;
    
           g_ptz.hori_ps_sw3_right_fall = 0; 
           g_ptz.hori_ps_sw3_left_rise  = 0; 
           g_ptz.hori_ps_sw3_right_rise = 0; 
           g_ptz.hori_ps_sw3_left_fall  = 0; 
           ptz_hori_stop(PTZ_HORI_STOP_TIME);
           g_ptz.hori_angle_state = 1;
           g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE;
        }
        break;
    }
    return g_ptz.hori_self_check;
  }

  /// 云台水平简化自检
  static unsigned char ptz_hori_simplify_self_check_task()
  {
    TMC2160Control data;
    switch(g_ptz.hori_self_check)
    {
      case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP:
        data = ptz_hori_choice_microstep(PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_tmc2160 = data;
        if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
        {
          ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1;
        }
        else//没有挡住
        {
          g_ptz.hori_ps_sw3_right_fall = 0; 
          g_ptz.hori_ps_sw3_left_rise  = 0; 
          g_ptz.hori_ps_sw3_right_rise = 0; 
          g_ptz.hori_ps_sw3_left_fall  = 0; 
          ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2;
        }
        break;
        
      case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1://向左转动，直到光电开关脱离挡片遮挡
        if(g_ptz.hori_ps_sw3_state == PS_NO_COVER)
        {
          OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
          ptz_hori_stop(PTZ_HORI_STOP_TIME);
          g_ptz.hori_ps_sw3_right_fall = 0; 
          g_ptz.hori_ps_sw3_left_rise  = 0; 
          g_ptz.hori_ps_sw3_right_rise = 0; 
          g_ptz.hori_ps_sw3_left_fall  = 0; 
          ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2;
        }
        break; 
       
      case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2:
        //等待水平光电开关右转下降沿
        if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
        {
          g_ptz.hori_as5047d.as5047d_cycle_count = 0;
          g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
          g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
          g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
          g_ptz.hori_as5047d.as5047d_state = 1;
          g_ptz.hori_ps_sw3_right_fall = 0; 
          g_ptz.hori_ps_sw3_left_rise  = 0; 
          g_ptz.hori_ps_sw3_right_rise = 0; 
          g_ptz.hori_ps_sw3_left_fall  = 0; 
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3;
        }
        break; 
        
      case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3://转动到0位
        OSTimeDlyHMSM(0u, 0u, 1u, 0u);//延时转动
        g_ptz.hori_angle_control = 0.0;
        g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
        ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_A;
        g_ptz.hori_angle_state = 1;
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
        break;  
    }
    return g_ptz.hori_self_check;
  }


  /// 云台垂直全范围自检
  static unsigned char ptz_vert_complete_self_check_task()
  {
    TMC2160Control data;
    switch(g_ptz.vert_self_check)
    {
      ///首先读取光电开关的状态，初步判断垂直状态
      case PTZ_VERT_SELF_CHECK_COMPLETE_STEP:
        data = ptz_vert_choice_microstep(PTZ_VERT_SELF_CHECK_SPEED);
        g_ptz.vert_tmc2160 = data;
        g_ptz.vert_ps_sw1_down_fall = 0;
        g_ptz.vert_ps_sw1_up_rise = 0;
        g_ptz.vert_ps_sw2_up_fall = 0;
        g_ptz.vert_ps_sw2_down_rise = 0;
        if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置
        {
          ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1; 
        }
        if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置
        {
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2;
        }
        if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER)
        //云台处于中间位置
        {
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2;
        }
        break;
        
      case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1):
        if(g_ptz.vert_ps_sw1_state == PS_NO_COVER)
        {
          OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
          ptz_vert_stop(PTZ_VERT_STOP_TIME);
          g_ptz.vert_ps_sw1_down_fall = 0;
          g_ptz.vert_ps_sw1_up_rise = 0;
          g_ptz.vert_ps_sw2_up_fall = 0;
          g_ptz.vert_ps_sw2_down_rise = 0;
          g_ptz.vert_self_check ++;
        }
        break;
        //开启向下转动，查找下俯极限位置
      case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2):
        ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
        g_ptz.vert_self_check  ++;
        break;
        
  #ifdef PTZ_SW1_DOWN_FALL_UPDATE
      //判断是否到达下俯极限位置,然后云台开始向上转动
      case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3):  
          if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
             g_ptz.vert_ps_sw1_state == PS_COVER)
          {
            g_ptz.vert_ps_sw1_down_fall = 0;
            g_ptz.vert_as5047d.as5047d_data_reset = 1;
            ptz_vert_stop(PTZ_VERT_STOP_TIME);
            ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
            g_ptz.vert_self_check  = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10;
          }
        break;
  #endif
        
  #ifdef PTZ_SW1_UP_RISE_UPDATE
       //判断是否到达下俯极限位置,然后云台开始向上转动
      case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3):  
          if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
             g_ptz.vert_ps_sw1_state == PS_COVER)
          {
            g_ptz.vert_ps_sw1_down_fall = 0;
            g_ptz.vert_ps_sw1_up_rise = 0;
            OSTimeDlyHMSM(0u, 0u, 0u, 300u);
            ptz_vert_stop(PTZ_VERT_STOP_TIME);
            ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
            g_ptz.vert_self_check++;
          }
        break; 
      //等待上仰上升沿  
      case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 4):  
        if(g_ptz.vert_ps_sw1_up_rise >= 1 ||
           g_ptz.vert_ps_sw1_state == PS_NO_COVER)
        {
          g_ptz.vert_as5047d.as5047d_data_reset = 1;
          g_ptz.vert_ps_sw1_down_fall = 0;
          g_ptz.vert_ps_sw1_up_rise = 0;
          g_ptz.vert_self_check  = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10;
        }
        break;
  #endif

        //等待SW2下降沿,计算垂直转动范围
      case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10):
        if(g_ptz.vert_ps_sw2_up_fall >= 1 ||
           g_ptz.vert_ps_sw2_state == PS_COVER)
        {
          g_ptz.vert_as5047d.as5047d_cycle_num = g_ptz.vert_as5047d.as5047d_cycle_count;
          g_ptz.vert_as5047d.as5047d_ptz_angle_max = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
          
          g_ptz.vert_angleP.angle_range = 
            g_ptz.vert_as5047d.as5047d_ptz_angle_max / PTZ_VERT_BIG_GEAR_RATIO;
          
          g_ptz.vert_angleP.angle_allow_min =  g_ptz.vert_angleP.angle_range / 2.0 * (-1);
          g_ptz.vert_angleP.angle_allow_max =  g_ptz.vert_angleP.angle_range / 2.0;
          
          g_ptz.vert_as5047d.as5047d_ptz_angle_K = 1 / PTZ_VERT_BIG_GEAR_RATIO;
            
           
          g_ptz.vert_as5047d.as5047d_remain_angle = g_ptz.vert_as5047d.as5047d_ptz_angle_max - 
          g_ptz.vert_as5047d.as5047d_cycle_num * 360.0;
             
          g_ptz.vert_as5047d.as5047d_state = 1;
           
          g_ptz.vert_ps_sw1_down_fall = 0;
          g_ptz.vert_ps_sw1_up_rise = 0;
          g_ptz.vert_ps_sw2_up_fall = 0;
          g_ptz.vert_ps_sw2_down_rise = 0;

          ptz_vert_stop(PTZ_VERT_STOP_TIME); 
          g_ptz.vert_angle_state = 1;
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE;
        }
        break;
    }
    return g_ptz.vert_self_check;
  }

  /// 云台垂直简化自检
  static unsigned char ptz_vert_simplify_self_check_task()
  {
    TMC2160Control data;
    switch(g_ptz.vert_self_check)
    {
      case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP:
        data = ptz_vert_choice_microstep(PTZ_VERT_SELF_CHECK_SPEED);
        g_ptz.vert_tmc2160 = data;
        g_ptz.vert_ps_sw1_down_fall = 0;
        g_ptz.vert_ps_sw1_up_rise = 0;
        g_ptz.vert_ps_sw2_up_fall = 0;
        g_ptz.vert_ps_sw2_down_rise = 0;
        if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置
        {
          ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1; 
        }
        if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置
        {
          ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
        }
        if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER)
        //云台处于中间位置
        {
          ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
        }
        break;
        
      case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1:
        if(g_ptz.vert_ps_sw1_state == PS_NO_COVER)
        {
          OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
          ptz_vert_stop(PTZ_VERT_STOP_TIME);
          g_ptz.vert_ps_sw1_down_fall = 0;
          g_ptz.vert_ps_sw1_up_rise = 0;
          g_ptz.vert_ps_sw2_up_fall = 0;
          g_ptz.vert_ps_sw2_down_rise = 0;
          ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
        }
        break;

  #ifdef PTZ_SW1_DOWN_FALL_UPDATE      
      case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2:
              if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
                 g_ptz.vert_ps_sw1_state == PS_COVER)
              {//等待SW1光电开关被遮挡，同时更新当前位置
                g_ptz.vert_as5047d.as5047d_cycle_count = 0;
                g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
                g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
                g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
                g_ptz.vert_as5047d.as5047d_state = 1;
                g_ptz.vert_ps_sw1_down_fall = 0;
                g_ptz.vert_ps_sw1_up_rise = 0;
                g_ptz.vert_ps_sw2_up_fall = 0;
                g_ptz.vert_ps_sw2_down_rise = 0;
                ptz_vert_stop(PTZ_VERT_STOP_TIME);
                g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10;
              }
        break;
  #endif
        
  #ifdef PTZ_SW1_UP_RISE_UPDATE 
        //判断是否到达下俯极限位置,然后云台开始向上转动
      case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2): 
          if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
             g_ptz.vert_ps_sw1_state == PS_COVER)
          {
            g_ptz.vert_ps_sw1_down_fall = 0;
            g_ptz.vert_ps_sw1_up_rise = 0;
            g_ptz.vert_ps_sw2_up_fall = 0;
            g_ptz.vert_ps_sw2_down_rise = 0;
            OSTimeDlyHMSM(0u, 0u, 0u, 300u);
            ptz_vert_stop(PTZ_VERT_STOP_TIME);
            ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
            g_ptz.vert_self_check++;
          }
        break; 
      //等待上仰上升沿  
      case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 3):  
          if(g_ptz.vert_ps_sw1_up_rise >= 1 ||
             g_ptz.vert_ps_sw1_state == PS_NO_COVER)
          {
            //等待SW1光电开关被遮挡，同时更新当前位置
            g_ptz.vert_as5047d.as5047d_cycle_count = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
            g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
            g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
            g_ptz.vert_as5047d.as5047d_state = 1;
            g_ptz.vert_ps_sw1_down_fall = 0;
            g_ptz.vert_ps_sw1_up_rise = 0;
            g_ptz.vert_ps_sw2_up_fall = 0;
            g_ptz.vert_ps_sw2_down_rise = 0;
            g_ptz.vert_self_check  = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10;
          } 
        break; 
  #endif
        
      case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10:
        OSTimeDlyHMSM(0u, 0u, 1u, 0u);
        data = ptz_vert_choice_microstep(PTZ_VERT_BEST_SPEED);
        g_ptz.vert_tmc2160 = data;
        g_ptz.vert_angle_control = 0.0;
        g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED;
        ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
        g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_A;
        g_ptz.vert_angle_state = 1;
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
        break;
    }   
    return g_ptz.vert_self_check;
  }

#endif


#ifdef PTZ_SERVO_MOTOR

/// 云台全范围自检
static unsigned char ptz_hori_complete_self_check_task()
{
  switch(g_ptz.hori_self_check)
  {
    //首先让云台水平向右转动
    case PTZ_HORI_SELF_CHECK_COMPLETE_STEP:
        set_speed_to_servoMotor(horiMotorType, PTZ_HORI_SELF_CHECK_SPEED);
      if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
      {
        ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1;
      }
      else//没有被挡住
      {
        g_ptz.hori_ps_sw3_right_fall = 0; 
        g_ptz.hori_ps_sw3_left_rise  = 0; 
        g_ptz.hori_ps_sw3_right_rise = 0; 
        g_ptz.hori_ps_sw3_left_fall  = 0; 
        ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2;
      }
      break;
    
    case PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1:
      if(g_ptz.hori_ps_sw3_state == PS_NO_COVER)
      {
        OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
        ptz_hori_stop(PTZ_HORI_STOP_TIME);
        g_ptz.hori_ps_sw3_right_fall = 0; 
        g_ptz.hori_ps_sw3_left_rise  = 0; 
        g_ptz.hori_ps_sw3_right_rise = 0; 
        g_ptz.hori_ps_sw3_left_fall  = 0; 
        ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2;
      }
      break;
   
    //等待第一次水平光电开关下降沿中断
    case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2):
      if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
      {
        g_ptz.hori_as5047d.as5047d_data_reset = 1;
        g_ptz.hori_ps_sw3_right_fall = 0;
        g_ptz.hori_self_check++;
        
        OSTimeDlyHMSM(0u, 0u, 5u, 0u);
        g_ptz.hori_ps_sw3_right_fall = 0;
      }
      break;
      
    //等待第二次水平光电开关下降沿中断
    case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 3):
      if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
      { 
         g_ptz.hori_as5047d.as5047d_cycle_num = g_ptz.hori_as5047d.as5047d_cycle_count;
         g_ptz.hori_as5047d.as5047d_ptz_angle_max = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
         g_ptz.hori_as5047d.as5047d_ptz_angle_K = 360.0 / g_ptz.hori_as5047d.as5047d_ptz_angle_max;
         
         g_ptz.hori_as5047d.as5047d_remain_angle = g_ptz.hori_as5047d.as5047d_ptz_angle_max - 
         g_ptz.hori_as5047d.as5047d_cycle_num * 360.0;
         g_ptz.hori_angleP.angle_allow_max = 360.0;
         g_ptz.hori_angleP.angle_allow_min = 0.0;
         g_ptz.hori_as5047d.as5047d_state = 1;
  
         g_ptz.hori_ps_sw3_right_fall = 0; 
         g_ptz.hori_ps_sw3_left_rise  = 0; 
         g_ptz.hori_ps_sw3_right_rise = 0; 
         g_ptz.hori_ps_sw3_left_fall  = 0; 
         ptz_hori_stop(PTZ_HORI_STOP_TIME);
         g_ptz.hori_angle_state = 1;
         g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE;
      }
      break;
  }
  return g_ptz.hori_self_check;
}

/// 云台水平简化自检
static unsigned char ptz_hori_simplify_self_check_task()
{
  switch(g_ptz.hori_self_check)
  {
    case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP:
        set_speed_to_servoMotor(horiMotorType, PTZ_HORI_SELF_CHECK_SPEED);
    if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
      {
        ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1;
      }
      else//没有挡住
      {
        g_ptz.hori_ps_sw3_right_fall = 0; 
        g_ptz.hori_ps_sw3_left_rise  = 0; 
        g_ptz.hori_ps_sw3_right_rise = 0; 
        g_ptz.hori_ps_sw3_left_fall  = 0; 
        ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2;
      }
      break;
      
    case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1://向左转动，直到光电开关脱离挡片遮挡
      if(g_ptz.hori_ps_sw3_state == PS_NO_COVER)
      {
        OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
        ptz_hori_stop(PTZ_HORI_STOP_TIME);
        g_ptz.hori_ps_sw3_right_fall = 0; 
        g_ptz.hori_ps_sw3_left_rise  = 0; 
        g_ptz.hori_ps_sw3_right_rise = 0; 
        g_ptz.hori_ps_sw3_left_fall  = 0; 
        ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2;
      }
      break; 
     
    case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2:
      //等待水平光电开关右转下降沿
      if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
      {
        g_ptz.hori_as5047d.as5047d_cycle_count = 0;
        g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
        g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
        g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
        g_ptz.hori_as5047d.as5047d_state = 1;
        g_ptz.hori_ps_sw3_right_fall = 0; 
        g_ptz.hori_ps_sw3_left_rise  = 0; 
        g_ptz.hori_ps_sw3_right_rise = 0; 
        g_ptz.hori_ps_sw3_left_fall  = 0; 
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3;
      }
      break; 
      
    case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3://转动到0位
      OSTimeDlyHMSM(0u, 0u, 1u, 0u);//延时转动
      g_ptz.hori_angle_control = 0.0;
      g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
      ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
      g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_A;
      g_ptz.hori_angle_state = 1;
      g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
      break;  
  }
  return g_ptz.hori_self_check;
}


/// 云台垂直全范围自检
static unsigned char ptz_vert_complete_self_check_task()
{
   switch(g_ptz.vert_self_check)
  {
    ///首先读取光电开关的状态，初步判断垂直状态
    case PTZ_VERT_SELF_CHECK_COMPLETE_STEP:
//      set_speed_to_servoMotor(vertMotorType, PTZ_VERT_SELF_CHECK_SPEED);
      g_ptz.vert_ps_sw1_down_fall = 0;
      g_ptz.vert_ps_sw1_up_rise = 0;
      g_ptz.vert_ps_sw2_up_fall = 0;
      g_ptz.vert_ps_sw2_down_rise = 0;
      if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置
      {
        ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1; 
      }
      if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置
      {
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2;
      }
      if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER)
      //云台处于中间位置
      {
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2;
      }
      break;
      
    case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1):
      if(g_ptz.vert_ps_sw1_state == PS_NO_COVER)
      {
        OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
        ptz_vert_stop(PTZ_VERT_STOP_TIME);
        g_ptz.vert_ps_sw1_down_fall = 0;
        g_ptz.vert_ps_sw1_up_rise = 0;
        g_ptz.vert_ps_sw2_up_fall = 0;
        g_ptz.vert_ps_sw2_down_rise = 0;
        g_ptz.vert_self_check ++;
      }
      break;
      //开启向下转动，查找下俯极限位置
    case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2):
      ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
      g_ptz.vert_self_check  ++;
      break;
      
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
    //判断是否到达下俯极限位置,然后云台开始向上转动
    case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3):  
        if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
           g_ptz.vert_ps_sw1_state == PS_COVER)
        {
          g_ptz.vert_ps_sw1_down_fall = 0;
          g_ptz.vert_as5047d.as5047d_data_reset = 1;
          ptz_vert_stop(PTZ_VERT_STOP_TIME);
          ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
          g_ptz.vert_self_check  = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10;
        }
      break;
#endif
      
#ifdef PTZ_SW1_UP_RISE_UPDATE
     //判断是否到达下俯极限位置,然后云台开始向上转动
    case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3):  
        if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
           g_ptz.vert_ps_sw1_state == PS_COVER)
        {
          g_ptz.vert_ps_sw1_down_fall = 0;
          g_ptz.vert_ps_sw1_up_rise = 0;
          OSTimeDlyHMSM(0u, 0u, 0u, 300u);
          ptz_vert_stop(PTZ_VERT_STOP_TIME);
          OSTimeDlyHMSM(0u, 0u, 0u, 10u);
          ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
          g_ptz.vert_self_check++;
        }
      break; 
    //等待上仰上升沿  
    case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 4):  
      if(g_ptz.vert_ps_sw1_up_rise >= 1 ||
         g_ptz.vert_ps_sw1_state == PS_NO_COVER)
      {
        g_ptz.vert_as5047d.as5047d_data_reset = 1;
        g_ptz.vert_ps_sw1_down_fall = 0;
        g_ptz.vert_ps_sw1_up_rise = 0;
        g_ptz.vert_self_check  = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10;
      }
      break;
#endif

      //等待SW2下降沿,计算垂直转动范围
    case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10):
      if(g_ptz.vert_ps_sw2_up_fall >= 1 ||
         g_ptz.vert_ps_sw2_state == PS_COVER)
      {
        g_ptz.vert_as5047d.as5047d_cycle_num = g_ptz.vert_as5047d.as5047d_cycle_count;
        g_ptz.vert_as5047d.as5047d_ptz_angle_max = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
        
        g_ptz.vert_angleP.angle_range = 
          g_ptz.vert_as5047d.as5047d_ptz_angle_max / PTZ_VERT_BIG_GEAR_RATIO;
        
        g_ptz.vert_angleP.angle_allow_min =  g_ptz.vert_angleP.angle_range / 2.0 * (-1);
        g_ptz.vert_angleP.angle_allow_max =  g_ptz.vert_angleP.angle_range / 2.0;
        
        g_ptz.vert_as5047d.as5047d_ptz_angle_K = 1 / PTZ_VERT_BIG_GEAR_RATIO;
          
         
        g_ptz.vert_as5047d.as5047d_remain_angle = g_ptz.vert_as5047d.as5047d_ptz_angle_max - 
        g_ptz.vert_as5047d.as5047d_cycle_num * 360.0;
           
        g_ptz.vert_as5047d.as5047d_state = 1;
         
        g_ptz.vert_ps_sw1_down_fall = 0;
        g_ptz.vert_ps_sw1_up_rise = 0;
        g_ptz.vert_ps_sw2_up_fall = 0;
        g_ptz.vert_ps_sw2_down_rise = 0;

        ptz_vert_stop(PTZ_VERT_STOP_TIME); 
        g_ptz.vert_angle_state = 1;
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE;
      }
      break;
  }
  return g_ptz.vert_self_check;
}

/// 云台垂直简化自检
static unsigned char ptz_vert_simplify_self_check_task()
{
  switch(g_ptz.vert_self_check)
  {
    case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP:
        set_speed_to_servoMotor(vertMotorType, PTZ_VERT_SELF_CHECK_SPEED);
      g_ptz.vert_ps_sw1_down_fall = 0;
      g_ptz.vert_ps_sw1_up_rise = 0;
      g_ptz.vert_ps_sw2_up_fall = 0;
      g_ptz.vert_ps_sw2_down_rise = 0;
      if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置
      {
        ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1; 
      }
      if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置
      {
        ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
      }
      if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER)
      //云台处于中间位置
      {
        ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
      }
      break;
      
    case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1:
      if(g_ptz.vert_ps_sw1_state == PS_NO_COVER)
      {
        OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
        ptz_vert_stop(PTZ_VERT_STOP_TIME);
        g_ptz.vert_ps_sw1_down_fall = 0;
        g_ptz.vert_ps_sw1_up_rise = 0;
        g_ptz.vert_ps_sw2_up_fall = 0;
        g_ptz.vert_ps_sw2_down_rise = 0;
        ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
      }
      break;

#ifdef PTZ_SW1_DOWN_FALL_UPDATE      
    case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2:
            if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
               g_ptz.vert_ps_sw1_state == PS_COVER)
            {//等待SW1光电开关被遮挡，同时更新当前位置
              g_ptz.vert_as5047d.as5047d_cycle_count = 0;
              g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
              g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
              g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
              g_ptz.vert_as5047d.as5047d_state = 1;
              g_ptz.vert_ps_sw1_down_fall = 0;
              g_ptz.vert_ps_sw1_up_rise = 0;
              g_ptz.vert_ps_sw2_up_fall = 0;
              g_ptz.vert_ps_sw2_down_rise = 0;
              ptz_vert_stop(PTZ_VERT_STOP_TIME);
              g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10;
            }
      break;
#endif
      
#ifdef PTZ_SW1_UP_RISE_UPDATE 
      //判断是否到达下俯极限位置,然后云台开始向上转动
    case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2): 
        if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
           g_ptz.vert_ps_sw1_state == PS_COVER)
        {
          g_ptz.vert_ps_sw1_down_fall = 0;
          g_ptz.vert_ps_sw1_up_rise = 0;
          g_ptz.vert_ps_sw2_up_fall = 0;
          g_ptz.vert_ps_sw2_down_rise = 0;
          OSTimeDlyHMSM(0u, 0u, 0u, 300u);
          ptz_vert_stop(PTZ_VERT_STOP_TIME);
          OSTimeDlyHMSM(0u, 0u, 0u, 10u);
          ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
          g_ptz.vert_self_check++;
        }
      break; 
      
    //等待上仰上升沿  
    case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 3):  
        if(g_ptz.vert_ps_sw1_up_rise >= 1 ||
           g_ptz.vert_ps_sw1_state == PS_NO_COVER)
        {
          //等待SW1光电开关被遮挡，同时更新当前位置
          g_ptz.vert_as5047d.as5047d_cycle_count = 0;
          g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
          g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
          g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
          g_ptz.vert_as5047d.as5047d_state = 1;
          g_ptz.vert_ps_sw1_down_fall = 0;
          g_ptz.vert_ps_sw1_up_rise = 0;
          g_ptz.vert_ps_sw2_up_fall = 0;
          g_ptz.vert_ps_sw2_down_rise = 0;
          g_ptz.vert_self_check  = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10;
        } 
      break; 
#endif
      
    case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10:
      OSTimeDlyHMSM(0u, 0u, 1u, 0u);
      g_ptz.vert_angle_control = 0.0;
      g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED;
      ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
      g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_A;
      g_ptz.vert_angle_state = 1;
      g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
      break;
  }   
  return g_ptz.vert_self_check;
}
#endif

static void  ptz_hori_self_check_task()
{
  while(1)
  {
    if(error_conut_state == 0)
    {//误差计算结束后开始自检
      ptz_hori_complete_self_check_task();
      ptz_hori_simplify_self_check_task();
    }
    //保存自检参数
    if(g_ptz.vert_self_check == PTZ_VERT_SELF_CHECK_DATA_SAVE &&
       g_ptz.hori_self_check == PTZ_HORI_SELF_CHECK_DATA_SAVE)
    {
      ptz_self_check_data_save();//保存数据
      //转到垂直0位
      g_ptz.vert_angle_control = 0.0;
      g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED;
      ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
      g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_A;
      g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
      //转到水平0位
      g_ptz.hori_angle_control = 0.0;
      g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
      ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
      g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_A;
      g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
    }
    
    if(g_ptz.hori_self_check == PTZ_HORI_SELF_CHECK_END && 
       g_ptz.vert_self_check == PTZ_VERT_SELF_CHECK_END)
    {
 
#ifdef PTZ_CMD_SAVE_BEFORE
      ptz_cmd_before_start();//查看是否让云台自动进行工作
#endif
      OSTaskDel(OS_PRIO_SELF);//关闭该任务
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 1u);
  }
}

static void  ptz_vert_self_check_task()
{
  while(1)
  {
    if(error_conut_state == 0)
    {//误差计算结束后开始自检
      ptz_vert_complete_self_check_task();
      ptz_vert_simplify_self_check_task();
    }
    if(g_ptz.vert_self_check == PTZ_VERT_SELF_CHECK_END)
    {
      if(g_ptz.offset_angle.vert_offset_switch == 2)
      {
        g_ptz.vert_angleP.angle_allow_min =  g_ptz.vert_angleP.angle_range / 2.0 * (-1);
        g_ptz.vert_angleP.angle_allow_max =  g_ptz.vert_angleP.angle_range / 2.0;
        g_ptz.vert_angleP.angle_allow_min = 
          g_ptz.vert_angleP.angle_allow_min - g_ptz.offset_angle.vert_zero_offset_angle;
        g_ptz.vert_angleP.angle_allow_max = 
          g_ptz.vert_angleP.angle_allow_max - g_ptz.offset_angle.vert_zero_offset_angle;
      }
      OSTaskDel(OS_PRIO_SELF);//关闭该任务
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 1u);
  }
}
//水平自检任务
static OS_STK  task_hori_self_check_stk[TASK_HORI_SELF_CHECK_STK_SIZE];
static  void  creat_task_hori_self_check(void)
{ 
   CPU_INT08U  task_err;   
   CPU_INT08U  name_err;
   
   task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_self_check_task,           
		   (void           *) 0,
		   (OS_STK         *)&task_hori_self_check_stk[TASK_HORI_SELF_CHECK_STK_SIZE - 1],
		   (INT8U           ) TASK_HORI_SELF_CHECK_PRIO,                   
		   (INT16U          ) TASK_HORI_SELF_CHECK_PRIO,
		   (OS_STK         *)&task_hori_self_check_stk[0],
		   (INT32U          ) TASK_HORI_SELF_CHECK_STK_SIZE,
		   (void           *) 0,
		   (INT16U          )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));     
    #if (OS_TASK_NAME_EN > 0)
    OSTaskNameSet(TASK_HORI_SELF_CHECK_PRIO, "ptz_hori_self_check_task", &name_err);
    #endif
    if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
        pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_self_check_task  success...\n\r");
	} else { 
        pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_self_check_task  failed...\n\r");
	}  
} 

//垂直自检任务
static OS_STK  task_vert_self_check_stk[TASK_VERT_SELF_CHECK_STK_SIZE];
static  void  creat_task_vert_self_check(void)
{ 
   CPU_INT08U  task_err;   
   CPU_INT08U  name_err;
   
   task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_self_check_task,           
		   (void           *) 0,
		   (OS_STK         *)&task_vert_self_check_stk[TASK_VERT_SELF_CHECK_STK_SIZE - 1],
		   (INT8U           ) TASK_VERT_SELF_CHECK_PRIO,                   
		   (INT16U          ) TASK_VERT_SELF_CHECK_PRIO,
		   (OS_STK         *)&task_vert_self_check_stk[0],
		   (INT32U          ) TASK_VERT_SELF_CHECK_STK_SIZE,
		   (void           *) 0,
		   (INT16U          )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));     
    #if (OS_TASK_NAME_EN > 0)
    OSTaskNameSet(TASK_VERT_SELF_CHECK_PRIO, "ptz_vert_self_check_task", &name_err);
    #endif
    if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
        pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_self_check_task  success...\n\r");
	} else { 
        pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_self_check_task  failed...\n\r");
	}
} 

/*
  @brief 自检参数初始化和自检方式选择
  @param
  @return
  @note   20180903 v1.0
          20220602 v2.0 lqc 
*/
/*
static void ptz_self_check_data_init()
{
  if(ptz_self_check_data_read() == 1)//自检参数读取成功，进行简易自检
  {
#ifdef PTZ_NO_SELF_CHECK 
    if(ptz_power_off_data_read() == 1)
    {
      g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
      g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
      g_ptz.hori_angle_state = 1;
      g_ptz.vert_angle_state = 1;
      g_ptz.no_self_check_state = 1;//读取成功
      return;
    }
    g_ptz.no_self_check_state = 0;//读取失败
#endif
    
    g_ptz.hori_angle_state = 0;
    g_ptz.vert_angle_state = 0;
    g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
    g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
    
    
#ifdef PTZ_VERT_SIMPLIFY_CHECK_OFF//不要垂直简易自检
    g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
    g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num / 2;
    g_ptz.vert_as5047d.as5047d_state = 1;
#endif
  
#ifdef PTZ_HORI_SIMPLIFY_CHECK_OFF//不要水平简易自检  
    g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
    g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num / 2;
    g_ptz.hori_as5047d.as5047d_state = 1;
#endif
  }
  else//自检参数失败进行全范围自检
  {
    g_ptz.hori_angle_state = 0;
    g_ptz.vert_angle_state = 0;
    g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP;
    g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP;
    
    
#ifdef PTZ_VERT_COMPLETE_CHECK_OFF//不要垂直全范围自检
    g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE;
    g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num / 2;
    g_ptz.vert_as5047d.as5047d_state = 1;
#endif
  
#ifdef PTZ_HORI_COMPLETE_CHECK_OFF//不要水平全范围自检  
    g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE;
    g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num / 2;
    g_ptz.hori_as5047d.as5047d_state = 1;
#endif
  }  
}
*/

/************通过判断配置信息来初始化自检方式***********/

#ifdef ONE
//自检参数初始化和自检方式选择
static void ptz_self_check_data_init()
{
  if(strstr((const char*)system_info.complete_self_check, "enable"))
  {//全自检使能
    if(ptz_self_check_data_read() == 1)//自检参数读取成功，进行简易自检
    {//自检参数读取成功，进入简易自检
      
      if(strstr((const char*)system_info.simplify_self_check, "both_enable"))
      {//双轴简易自检
        g_ptz.no_self_check_state = 0;//读取失败
        g_ptz.hori_angle_state = 0;
        g_ptz.vert_angle_state = 0;
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
        g_ptz.hori_as5047d.as5047d_state = 0;
        g_ptz.vert_as5047d.as5047d_state = 0;
        return;
      }
      if(strstr((const char*)system_info.simplify_self_check, "hori_enable"))
      {//水平简易自检、垂直不自检
        if(ptz_power_off_data_read() == 1)
        {//掉电数据读取成功
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
          g_ptz.hori_angle_state = 0;
          g_ptz.vert_angle_state = 1;
          g_ptz.no_self_check_state = 1;//读取成功
          g_ptz.hori_as5047d.as5047d_state = 0;
          
        }else{//掉电数据读取失败，垂直赋默认值，水平简易自检
          g_ptz.no_self_check_state = 0;//读取失败
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
          g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num / 2;
          g_ptz.vert_as5047d.as5047d_state = 1;
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
          g_ptz.hori_as5047d.as5047d_state = 0;
        }
        return;
      }
      if(strstr((const char*)system_info.simplify_self_check, "vert_enable"))
      {//垂直简易自检、水平不自检
        if(ptz_power_off_data_read() == 1)
        {//掉电数据读取成功
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
          g_ptz.hori_angle_state = 1;
          g_ptz.vert_angle_state = 0;
          g_ptz.no_self_check_state = 1;//读取成功
          g_ptz.vert_as5047d.as5047d_state = 0;
          
        }else{//掉电数据读取失败，垂直赋默认值，水平简易自检
          g_ptz.no_self_check_state = 0;//读取失败
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
          g_ptz.vert_as5047d.as5047d_state = 0;
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
          g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num / 2;
          g_ptz.hori_as5047d.as5047d_state = 1;
        }
        return;
      }
      if(strstr((const char*)system_info.simplify_self_check, "both_disable"))      
      {//双轴不简易自检
        if(ptz_power_off_data_read() == 1)
        {//掉电数据读取成功，结束自检
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
          g_ptz.hori_angle_state = 1;
          g_ptz.vert_angle_state = 1;
          g_ptz.no_self_check_state = 1;//读取成功
          
        }else{//掉电数据读取失败，赋默认值，结束自检
          g_ptz.no_self_check_state = 0;//读取失败
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
          g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num / 2;
          g_ptz.vert_as5047d.as5047d_state = 1;
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
          g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num / 2;
          g_ptz.hori_as5047d.as5047d_state = 1;
        }
        return;
      }
      
    }else{
      //自检参数失败进行全范围自检
      g_ptz.hori_angle_state = 0;
      g_ptz.vert_angle_state = 0;
      g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP;
      g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP;
      return;
    }
  }else{//任何情况下都不自检
    if((ptz_self_check_data_read() == 1) && (ptz_power_off_data_read() == 1))
    {//自检参数读取成功、且掉电数据读取成功
      g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
      g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
      g_ptz.hori_angle_state = 1;
      g_ptz.vert_angle_state = 1;
      g_ptz.no_self_check_state = 1;//读取成功
      return;
    }else{
      g_ptz.no_self_check_state = 0;//读取失败
      g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE;
      g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num / 2;
      g_ptz.vert_as5047d.as5047d_state = 1;
      
      g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE;
      g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num / 2;
      g_ptz.hori_as5047d.as5047d_state = 1;
      return;
    }
  }
  
}

#endif

//自检参数初始化和自检方式选择
static void ptz_self_check_data_init()
{
  if(strstr((const char*)system_info.complete_self_check, "enable"))
  {//全自检使能
    if(ptz_self_check_data_read() == 1)//自检参数读取成功，进行简易自检
    {//自检参数读取成功，进入简易自检
      error_conut_state = 0;//不进行误差计算
      if(strstr((const char*)system_info.simplify_self_check, "both_enabled"))
      {//双轴简易自检
        g_ptz.no_self_check_state = 0;//读取失败
        g_ptz.hori_angle_state = 0;
        g_ptz.vert_angle_state = 0;
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
        g_ptz.hori_as5047d.as5047d_state = 0;//编码器状态为0时，角度计算不可用，该值保存于自检参数以及掉电保护数据中，自检完成后置1，
        g_ptz.vert_as5047d.as5047d_state = 0;
        return;
      }
      if(strstr((const char*)system_info.simplify_self_check, "hori_enabled"))
      {//水平简易自检、垂直不自检
        if(ptz_power_off_data_read() == 1)
        {//掉电数据读取成功
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
          g_ptz.hori_angle_state = 0;
          g_ptz.vert_angle_state = 1;
          g_ptz.no_self_check_state = 1;//读取成功
          g_ptz.hori_as5047d.as5047d_state = 0;
          
        }else{//掉电数据读取失败，垂直赋默认值，水平简易自检
          g_ptz.no_self_check_state = 0;//读取失败
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
          g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num / 2;
          g_ptz.vert_as5047d.as5047d_state = 1;
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
          g_ptz.hori_as5047d.as5047d_state = 0;
          g_ptz.hori_angle_state = 0;//掉电参数读取失败，角度状态先置故障
          g_ptz.vert_angle_state = 0;
        }
        return;
      }
      if(strstr((const char*)system_info.simplify_self_check, "vert_enabled"))
      {//垂直简易自检、水平不自检
        if(ptz_power_off_data_read() == 1)
        {//掉电数据读取成功
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
          g_ptz.hori_angle_state = 1;
          g_ptz.vert_angle_state = 0;
          g_ptz.no_self_check_state = 1;//读取成功
          g_ptz.vert_as5047d.as5047d_state = 0;
          
        }else{//掉电数据读取失败，水平赋默认值，垂直简易自检
          g_ptz.no_self_check_state = 0;//读取失败
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
          g_ptz.vert_as5047d.as5047d_state = 0;
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
          g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num / 2;
          g_ptz.hori_as5047d.as5047d_state = 1;
          g_ptz.hori_angle_state = 0;//掉电参数读取失败，角度状态先置故障
          g_ptz.vert_angle_state = 0;
        }
        return;
      }
      if(strstr((const char*)system_info.simplify_self_check, "both_disabled"))      
      {//双轴不简易自检
        if(ptz_power_off_data_read() == 1)
        {//掉电数据读取成功，结束自检
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
          g_ptz.hori_angle_state = 1;
          g_ptz.vert_angle_state = 1;
          g_ptz.no_self_check_state = 1;//读取成功
          
        }else{//掉电数据读取失败，赋默认值，结束自检
          g_ptz.no_self_check_state = 0;//读取失败
          g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
          g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num / 2;
          g_ptz.vert_as5047d.as5047d_state = 1;
          g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
          g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num / 2;
          g_ptz.hori_as5047d.as5047d_state = 1;
          g_ptz.hori_angle_state = 0;//掉电参数读取失败，角度状态先置故障
          g_ptz.vert_angle_state = 0;
        }
        return;
      }
      
    }else{
      error_conut_state = 1;//开始进行误差计算
      //自检参数失败进行全范围自检
      g_ptz.hori_angle_state = 0;
      g_ptz.vert_angle_state = 0;
      g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP;
      g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP;
      return;
    }
  }else{//任何情况下都不自检
    error_conut_state = 0;//不进行误差计算
    if(ptz_self_check_data_read() == 1)
    {//自检参数读取成功
      if(ptz_power_off_data_read() == 1)
      {//掉电数据读取成功
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
        g_ptz.hori_angle_state = 1;
        g_ptz.vert_angle_state = 1;
        g_ptz.no_self_check_state = 1;//读取成功
        return;
      }else{//掉电数据读取失败
        g_ptz.no_self_check_state = 0;//读取失败
        g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
        g_ptz.vert_as5047d.as5047d_cycle_count =  g_ptz.vert_as5047d.as5047d_cycle_num / 2;
        g_ptz.vert_as5047d.as5047d_state = 1;
        
        g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
        g_ptz.hori_as5047d.as5047d_cycle_count =  g_ptz.hori_as5047d.as5047d_cycle_num / 2;
        g_ptz.hori_as5047d.as5047d_state = 1;
        
        g_ptz.hori_angle_state = 0;//角度状态先置故障
        g_ptz.vert_angle_state = 0;
        return;
      }
      
    }else{//自检参数读取失败
      g_ptz.no_self_check_state = 0;//读取失败
      g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE;
      /* 自检参数读取失败，重新赋值经验值进行保存 */
      //中型蜗轮蜗杆步进电机云台
      g_ptz.vert_as5047d.as5047d_ptz_angle_max = 9600.0;
      g_ptz.vert_as5047d.as5047d_ptz_angle_K = 1.0/80;
      g_ptz.vert_as5047d.as5047d_remain_angle = 230.0;
      g_ptz.vert_as5047d.as5047d_cycle_num = 26;
      
      g_ptz.vert_angleP.angle_min = -60.0;//云台角度最小值
      g_ptz.vert_angleP.angle_max = 60.0;//云台角度最大值
      g_ptz.vert_angleP.angle_allow_min = -60.0;//云台可用角度最小值
      g_ptz.vert_angleP.angle_allow_max = 60.0;//云台可用角度最大值
      g_ptz.vert_angleP.angle_range = 120.0;//云台角度范围
      
      g_ptz.vert_as5047d.as5047d_state = 1;
      
      g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE;
      /* 自检参数读取失败，重新赋值经验值进行保存 */
      g_ptz.hori_as5047d.as5047d_ptz_angle_max = 28800.0;
      g_ptz.hori_as5047d.as5047d_ptz_angle_K = 1.0/80;
      g_ptz.hori_as5047d.as5047d_remain_angle = 0.0;
      g_ptz.hori_as5047d.as5047d_cycle_num = 80;
      
      g_ptz.hori_angleP.angle_min = 0.0;//云台角度最小值
      g_ptz.hori_angleP.angle_max = 360.0;//云台角度最大值
      g_ptz.hori_angleP.angle_allow_min = 0.0;//云台可用角度最小值
      g_ptz.hori_angleP.angle_allow_max = 360.0;//云台可用角度最大值
      g_ptz.hori_angleP.angle_range = 360.0;//云台角度范围
      
      g_ptz.hori_as5047d.as5047d_state = 1;
      
      g_ptz.hori_angle_state = 0;//角度状态先置故障
      g_ptz.vert_angle_state = 0;
    }
  }
  
}




//自检任务初始化
void init_self_check_module(void)
{
//  g_ptz.hori_self_check = 255;
//  g_ptz.vert_self_check = 255;
  
  ptz_self_check_data_init();
//  init_error_count_task();//误差计算初始化
  creat_task_hori_self_check();
  creat_task_vert_self_check();
}





//计算效验码
unsigned int ptz_self_check_data_crc(PtzSelfCheckData data)
{
  float crc = 0;
  unsigned int crc1 = 0;
  crc = crc + data.hori_as5047d.as5047d_ptz_angle_max;
  crc = crc + data.hori_as5047d.as5047d_ptz_angle_K;
  crc = crc + data.hori_as5047d.as5047d_remain_angle;
  crc = crc + data.hori_as5047d.as5047d_cycle_num;
  
  crc = crc + data.vert_as5047d.as5047d_ptz_angle_max;
  crc = crc + data.vert_as5047d.as5047d_ptz_angle_K;
  crc = crc + data.vert_as5047d.as5047d_remain_angle;
  crc = crc + data.vert_as5047d.as5047d_cycle_num;

  crc = crc + data.hori_angleP.angle_min;//云台角度最小值
  crc = crc + data.hori_angleP.angle_max;//云台角度最大值
  crc = crc + data.hori_angleP.angle_allow_min;//云台可用角度最小值
  crc = crc + data.hori_angleP.angle_allow_max;//云台可用角度最大值
  crc = crc + data.hori_angleP.angle_range;//云台角度范围
 
  crc = crc + data.vert_angleP.angle_min;//云台角度最小值
  crc = crc + data.vert_angleP.angle_max;//云台角度最大值
  crc = crc + data.vert_angleP.angle_allow_min;//云台可用角度最小值
  crc = crc + data.vert_angleP.angle_allow_max;//云台可用角度最大值
  crc = crc + data.vert_angleP.angle_range;//云台角度范围
  
  crc = crc + data.hori_angle_error;//水平结构误差
  crc1 = (unsigned int)crc;
  return crc1;
}

static PtzSelfCheckData self_check_data;
static PtzSelfCheckData self_check_data_a;
//保存自检参数
char ptz_self_check_data_save()
{
  char i,j;
  //复制数据
  memcpy(&self_check_data.hori_as5047d, &g_ptz.hori_as5047d, sizeof(g_ptz.hori_as5047d));
  memcpy(&self_check_data.hori_angleP,  &g_ptz.hori_angleP,  sizeof(g_ptz.hori_angleP));
  memcpy(&self_check_data.vert_as5047d, &g_ptz.vert_as5047d, sizeof(g_ptz.vert_as5047d));
  memcpy(&self_check_data.vert_angleP,  &g_ptz.vert_angleP,  sizeof(g_ptz.vert_angleP));
  
  self_check_data.hori_angle_error = g_ptz.hori_angle_error;//存储水平结构误差
  
  //计算效验码
  self_check_data.crc = ptz_self_check_data_crc(self_check_data);
  
  //写入主存储区
  for(i = 0; i < SELF_CHECK_DATA_SAVE_FLASH_NUM; i++)
  {
    memset(&self_check_data_a, 0, sizeof(self_check_data_a));
    write_many_data(sizeof(self_check_data),(unsigned char *)(&self_check_data),SELF_CHECK_DATA_FLASH_ADD);
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
    Flash_Read((unsigned char *)(&self_check_data_a),SELF_CHECK_DATA_FLASH_ADD,sizeof(self_check_data_a));
    if(memcmp(&self_check_data, &self_check_data_a, sizeof(self_check_data)) == 0)//判断写入数据是否正确
    {
      break;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
  }
  //写入备份存储区
  for(j = 0; j < SELF_CHECK_DATA_SAVE_FLASH_NUM; j++)
  {
    memset(&self_check_data_a, 0, sizeof(self_check_data_a));
    write_many_data(sizeof(self_check_data),(unsigned char *)(&self_check_data),SELF_CHECK_DATA_BACKUP_FLASH_ADD);
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
    Flash_Read((unsigned char *)(&self_check_data_a),SELF_CHECK_DATA_BACKUP_FLASH_ADD,sizeof(self_check_data_a));
    if(memcmp(&self_check_data, &self_check_data_a, sizeof(self_check_data)) == 0)//判断写入数据是否正确
    {
      break;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
  }
  
  if(i < SELF_CHECK_DATA_SAVE_FLASH_NUM || j < SELF_CHECK_DATA_SAVE_FLASH_NUM)//判断写入数据是否成功
  {
    //写入成功
    return 1;
  }
  //写入失败
  return 0;
}

//自检参数读取
char ptz_self_check_data_read()
{
  char i,j;
  //首先从主存储区读取数据
  for(i = 0; i < SELF_CHECK_DATA_READ_FLASH_NUM; i++)
  {
    memset(&self_check_data, 0, sizeof(self_check_data));
    Flash_Read((unsigned char *)(&self_check_data),SELF_CHECK_DATA_FLASH_ADD,sizeof(self_check_data));
    if(self_check_data.crc == ptz_self_check_data_crc(self_check_data)
       && self_check_data.crc > 0)
    {
      g_ptz.hori_as5047d.as5047d_ptz_angle_max = self_check_data.hori_as5047d.as5047d_ptz_angle_max;
      g_ptz.hori_as5047d.as5047d_ptz_angle_K   = self_check_data.hori_as5047d.as5047d_ptz_angle_K;
      g_ptz.hori_as5047d.as5047d_remain_angle  = self_check_data.hori_as5047d.as5047d_remain_angle;
      g_ptz.hori_as5047d.as5047d_cycle_num     = self_check_data.hori_as5047d.as5047d_cycle_num;
      
      g_ptz.vert_as5047d.as5047d_ptz_angle_max = self_check_data.vert_as5047d.as5047d_ptz_angle_max;
      g_ptz.vert_as5047d.as5047d_ptz_angle_K   = self_check_data.vert_as5047d.as5047d_ptz_angle_K;
      g_ptz.vert_as5047d.as5047d_remain_angle  = self_check_data.vert_as5047d.as5047d_remain_angle;
      g_ptz.vert_as5047d.as5047d_cycle_num     = self_check_data.vert_as5047d.as5047d_cycle_num;
      
      g_ptz.hori_angleP.angle_min       = self_check_data.hori_angleP.angle_min;//云台角度最小值
      g_ptz.hori_angleP.angle_max       = self_check_data.hori_angleP.angle_max;//云台角度最大值
      g_ptz.hori_angleP.angle_allow_min = self_check_data.hori_angleP.angle_allow_min;//云台可用角度最小值
      g_ptz.hori_angleP.angle_allow_max = self_check_data.hori_angleP.angle_allow_max;//云台可用角度最大值
      g_ptz.hori_angleP.angle_range     = self_check_data.hori_angleP.angle_range;//云台角度范围
     
      g_ptz.vert_angleP.angle_min       = self_check_data.vert_angleP.angle_min;//云台角度最小值
      g_ptz.vert_angleP.angle_max       = self_check_data.vert_angleP.angle_max;//云台角度最大值
      g_ptz.vert_angleP.angle_allow_min = self_check_data.vert_angleP.angle_allow_min;//云台可用角度最小值
      g_ptz.vert_angleP.angle_allow_max = self_check_data.vert_angleP.angle_allow_max;//云台可用角度最大值
      g_ptz.vert_angleP.angle_range     = self_check_data.vert_angleP.angle_range;//云台角度范围
      
      g_ptz.hori_angle_error = self_check_data.hori_angle_error;//赋值水平结构误差
      return 1;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 2u);
  }
  //如果主存储区读取的数据是错误的，则从备份存储区读取数据
  for(j = 0; j < SELF_CHECK_DATA_READ_FLASH_NUM; j++)
  {
    memset(&self_check_data, 0, sizeof(self_check_data));
    Flash_Read((unsigned char *)(&self_check_data),SELF_CHECK_DATA_BACKUP_FLASH_ADD,sizeof(self_check_data));
    if(self_check_data.crc == ptz_self_check_data_crc(self_check_data)
       && self_check_data.crc > 0)
    {
      g_ptz.hori_as5047d.as5047d_ptz_angle_max = self_check_data.hori_as5047d.as5047d_ptz_angle_max;
      g_ptz.hori_as5047d.as5047d_ptz_angle_K   = self_check_data.hori_as5047d.as5047d_ptz_angle_K;
      g_ptz.hori_as5047d.as5047d_remain_angle  = self_check_data.hori_as5047d.as5047d_remain_angle;
      g_ptz.hori_as5047d.as5047d_cycle_num     = self_check_data.hori_as5047d.as5047d_cycle_num;
      
      g_ptz.vert_as5047d.as5047d_ptz_angle_max = self_check_data.vert_as5047d.as5047d_ptz_angle_max;
      g_ptz.vert_as5047d.as5047d_ptz_angle_K   = self_check_data.vert_as5047d.as5047d_ptz_angle_K;
      g_ptz.vert_as5047d.as5047d_remain_angle  = self_check_data.vert_as5047d.as5047d_remain_angle;
      g_ptz.vert_as5047d.as5047d_cycle_num     = self_check_data.vert_as5047d.as5047d_cycle_num;
      
      g_ptz.hori_angleP.angle_min       = self_check_data.hori_angleP.angle_min;//云台角度最小值
      g_ptz.hori_angleP.angle_max       = self_check_data.hori_angleP.angle_max;//云台角度最大值
      g_ptz.hori_angleP.angle_allow_min = self_check_data.hori_angleP.angle_allow_min;//云台可用角度最小值
      g_ptz.hori_angleP.angle_allow_max = self_check_data.hori_angleP.angle_allow_max;//云台可用角度最大值
      g_ptz.hori_angleP.angle_range     = self_check_data.hori_angleP.angle_range;//云台角度范围
     
      g_ptz.vert_angleP.angle_min       = self_check_data.vert_angleP.angle_min;//云台角度最小值
      g_ptz.vert_angleP.angle_max       = self_check_data.vert_angleP.angle_max;//云台角度最大值
      g_ptz.vert_angleP.angle_allow_min = self_check_data.vert_angleP.angle_allow_min;//云台可用角度最小值
      g_ptz.vert_angleP.angle_allow_max = self_check_data.vert_angleP.angle_allow_max;//云台可用角度最大值
      g_ptz.vert_angleP.angle_range     = self_check_data.vert_angleP.angle_range;//云台角度范围
      
      g_ptz.hori_angle_error = self_check_data.hori_angle_error;//赋值水平结构误差
      return 1;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 2u);
  }
  return 0;
}

//擦除自检数据
char ptz_self_check_data_erase()
{
  char i, j;
  memset(&self_check_data_a, 0, sizeof(self_check_data_a));
  memset(&self_check_data, 0, sizeof(self_check_data));
  
#ifdef PTZ_NO_SELF_CHECK
      ptz_power_off_data_erase(PTZ_MB85RC64_ADD_A);
      ptz_power_off_data_erase(PTZ_MB85RC64_ADD_B);
      OSTimeDlyHMSM(0u, 0u, 0u, 5u);
#endif
  
  //擦除主存储区
  for(i = 0; i < SELF_CHECK_DATA_SAVE_FLASH_NUM; i++)
  {
    memset(&self_check_data_a, 0, sizeof(self_check_data_a));
    write_many_data(sizeof(self_check_data),(unsigned char *)(&self_check_data),SELF_CHECK_DATA_FLASH_ADD);
    asm("nop");
    asm("nop");
    Flash_Read((unsigned char *)(&self_check_data_a),SELF_CHECK_DATA_FLASH_ADD,sizeof(self_check_data_a));
    if(memcmp(&self_check_data, &self_check_data_a, sizeof(self_check_data)) == 0)//判断擦除数据是否正确
    {
      break;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
  }
  //擦除备份存储区
  for(j = 0; j < SELF_CHECK_DATA_SAVE_FLASH_NUM; j++)
  {
    memset(&self_check_data_a, 0, sizeof(self_check_data_a));
    write_many_data(sizeof(self_check_data),(unsigned char *)(&self_check_data),SELF_CHECK_DATA_BACKUP_FLASH_ADD);
    asm("nop");
    asm("nop");
    Flash_Read((unsigned char *)(&self_check_data_a),SELF_CHECK_DATA_BACKUP_FLASH_ADD,sizeof(self_check_data_a));
    if(memcmp(&self_check_data, &self_check_data_a, sizeof(self_check_data)) == 0)//判断擦除数据是否正确
    {
      break;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
  }
  
  if(i < SELF_CHECK_DATA_SAVE_FLASH_NUM || j < SELF_CHECK_DATA_SAVE_FLASH_NUM)//判断擦除数据是否成功
  {
    //擦除成功
    return 1;
  }
  //擦除失败
  return 0;
}




















