#include "ptz_type_select.h"
#include "ptz_struct.h"
#include "ptz_default_value.h"
#include "tmc2160.h"
#include "w25q128.h"


#ifdef TMC2160
TMC2160Parameter tmc2160_parameter;
TMC2160Regis hori_regis;
TMC2160Regis vert_regis;
unsigned long int version_hori=0;
unsigned long int version_vert=0;

static BSP_OS_SEM ptz_tmc2160_mutex;//共享资源锁
static void delay_time(int time)
{
  for(int i = 0; i < time; i ++)
    {//主频200MHZ
       asm("nop");
       asm("nop");
       asm("nop");
       asm("nop");
       asm("nop");
       asm("nop");
       asm("nop");
       asm("nop");
       asm("nop");
       asm("nop");
       asm("nop");
       asm("nop");
       asm("nop");
       asm("nop");
       asm("nop");
       asm("nop");
    }  
}


///===== 初始化驱动器 =====
///驱动器重新上电后自动复位，全部置零
void tmc2160_init()
{
  BSP_OS_SemCreate(&ptz_tmc2160_mutex,1u,"ptz_tmc2160_mutex");
  
   //配置引脚时钟
  rcu_periph_clock_enable(RCU_GPIOA);
  rcu_periph_clock_enable(RCU_GPIOC);
  rcu_periph_clock_enable(RCU_GPIOD);
  rcu_periph_clock_enable(RCU_GPIOE);
  
  //GD改版后驱动策略进行了修改————新增驱动器电源控制引脚，便于驱动器的断电复位等
  gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_0);//水平电源控制引脚
  gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_1);//垂直电源控制引脚
  gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_0| GPIO_PIN_1);
  //电源打开
  TMC2160_H_POWER_ON;
  TMC2160_V_POWER_ON;
  
  //====初始化驱动器参数
  tmc2160_data_init();
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
 
  //SPI
  gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_13);//CLK
  gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_14);//SDI主出从入
  gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_15);//SDO主入从出
  gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_13| GPIO_PIN_14);
  //====水平驱动器初始化
  gpio_mode_set(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_15);//H_CSN
  gpio_mode_set(GPIOD, GPIO_MODE_INPUT, GPIO_PUPD_PULLUP, GPIO_PIN_14);//DIAG1
  gpio_mode_set(GPIOD, GPIO_MODE_INPUT, GPIO_PUPD_PULLUP, GPIO_PIN_13);//DIAG0
  gpio_mode_set(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_PULLDOWN, GPIO_PIN_10);//DRVEN
  gpio_mode_set(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_11);//DIR
  gpio_output_options_set(GPIOD, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_10| GPIO_PIN_11| GPIO_PIN_5);
  //====垂直驱动器初始化
  gpio_mode_set(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_11);//V_CSN
  gpio_mode_set(GPIOA, GPIO_MODE_INPUT, GPIO_PUPD_PULLUP, GPIO_PIN_8);//DIAG1
  gpio_mode_set(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_PULLUP, GPIO_PIN_9);//DIAG0
  gpio_mode_set(GPIOC, GPIO_MODE_OUTPUT, GPIO_PUPD_PULLDOWN, GPIO_PIN_7);//DRVEN
  gpio_mode_set(GPIOC, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO_PIN_8);//DIR
  gpio_output_options_set(GPIOC, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_7| GPIO_PIN_8);
  gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, GPIO_PIN_11);
  
  TMC2160_HORI_CSN_DISABLE;
  TMC2160_HORI_DISABLE;
  TMC2160_VERT_CSN_DISABLE;
  TMC2160_VERT_DISABLE;
  
  //====上电清除全局状态标志
  tmc2160_write_register(TMC2160_HORI, TMC2160_GSTAT, 0);
  OSTimeDlyHMSM(0u, 0u, 0u, 1u);
  tmc2160_write_register(TMC2160_VERT, TMC2160_GSTAT, 0);
  OSTimeDlyHMSM(0u, 0u, 0u, 1u);
  //====寄存器通用配置
  //水平
  tmc2160_write_register(TMC2160_HORI, TMC2160_GLOBAL_SCALER, tmc2160_parameter.hori_global_scaler);
  tmc2160_write_register(TMC2160_HORI, TMC2160_GCONF, 0);
  tmc2160_write_register(TMC2160_HORI, TMC2160_PWMCONF, 0);
  tmc2160_write_register(TMC2160_HORI, TMC2160_DRV_CONF, 0);
  tmc2160_write_register(TMC2160_HORI, TMC2160_TPOWERDOWN, 0);
  tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, TMC2160_1_16_STEP);//细分默认16
  tmc2160_write_register(TMC2160_HORI, TMC2160_IHOLD_IRUN, tmc2160_parameter.hori_work_torque);//电流默认值=20/32*2.3A
  
  tmc2160_write_register(TMC2160_HORI, TMC2160_TPWMTHRS, 0);
  #ifdef PTZ_MEDIUM_WORM_STEP_TMC2160_AS5047D_24V
      tmc2160_write_register(TMC2160_HORI, TMC2160_SHORT_CONF, 0);
  #endif
  
  
//  tmc2160_write_register(TMC2160_HORI, TMC2160_COOLCONF, 0);
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  //垂直
  tmc2160_write_register(TMC2160_VERT, TMC2160_GLOBAL_SCALER, tmc2160_parameter.vert_global_scaler);
  tmc2160_write_register(TMC2160_VERT, TMC2160_GCONF, 0);
  tmc2160_write_register(TMC2160_VERT, TMC2160_PWMCONF, 0);
  tmc2160_write_register(TMC2160_VERT, TMC2160_DRV_CONF, 0);
  tmc2160_write_register(TMC2160_VERT, TMC2160_TPOWERDOWN, 0);
  tmc2160_write_register(TMC2160_VERT, TMC2160_CHOPCONF, TMC2160_1_16_STEP);//细分默认16
  tmc2160_write_register(TMC2160_VERT, TMC2160_IHOLD_IRUN, tmc2160_parameter.vert_work_torque);//电流默认值=20/32*2.3A
  
  tmc2160_write_register(TMC2160_VERT, TMC2160_TPWMTHRS, 0);
  #ifdef PTZ_MEDIUM_WORM_STEP_TMC2160_AS5047D_24V
      tmc2160_write_register(TMC2160_VERT, TMC2160_SHORT_CONF, 0);
  #endif
//  tmc2160_write_register(TMC2160_VERT, TMC2160_COOLCONF, 0);
  
//  vert_regis.g_conf = tmc2160_write_hori_data(TMC2160_HORI,TMC2160_DRV_STATUS,0,0,0,0);
}

///===== 读写寄存器，SPI位40位，8位地址，32位数据
unsigned long int tmc2160_write_hori_data(char choice, unsigned char x1, unsigned char x2, unsigned char x3,unsigned char x4,unsigned char x5)
{//choice——驱动器选择，x2/x3/x4/x5——写寄存器的数据
  SPIpacka packa;
  unsigned long int dat=0;
  unsigned char datak[5] = {0};
   char i,j,k;
  unsigned char temp=0;
  packa.data[0] = x1;//寄存器地址位
  packa.data[1] = x2;//寄存器数据位
  packa.data[2] = x3;//寄存器数据位
  packa.data[3] = x4;//寄存器数据位
  packa.data[4] = x5;//寄存器数据位
  for(k = 0; k < 2; k++)
  {//连续2次写入，一方面确保寄存器写入成功，二是当前SPI写入时，同时寄存器会返回上一次写入数据，由此实现返回数据为当前写入数据
    TMC2160_VERT_CSN_DISABLE;
    TMC2160_HORI_CSN_ENABLE;
    delay_time(2);
    TMC2160_SCLK_HIGH;
    delay_time(2);
//    //片选
//    if(choice == TMC2160_HORI)
//    {
//      TMC2160_HORI_CSN_ENABLE;
//    }else{//TMC2160_VERT
//      TMC2160_VERT_CSN_ENABLE;
//    }
    delay_time(2);
    TMC2160_SCLK_LOW;
     
    delay_time(10);

    for(j = 0; j < 5; j++)
    {
      for(i = 0; i < 8; i++)
      {
        TMC2160_SCLK_LOW;
        if((packa.data[j] & 0x80) != 0)
        {
          TMC2160_SDATI_1;
        }else{
          TMC2160_SDATI_0;
        }
        packa.data[j] = packa.data[j] << 1;
        delay_time(2);
        TMC2160_SCLK_HIGH;
        temp = TMC2160_SDATO_READ;
        datak[j] = datak[j] | (temp << (7-i));
        delay_time(2);
      }
    }
    TMC2160_SCLK_HIGH;
    delay_time(2); 
//片选失能
    TMC2160_HORI_CSN_DISABLE;
    
    dat = datak[0];
    dat = (dat << 8) | datak[1];
    dat = (dat << 8) | datak[2];
    dat = (dat << 8) | datak[3];
    dat = (dat << 8) | datak[4];
//    dat = datak[4];
    
  }
  return dat;
}

///===== SPI位40位，8位地址，32位数据
unsigned long int tmc2160_write_vert_data(char choice, unsigned char x1, unsigned char x2, unsigned char x3,unsigned char x4,unsigned char x5)
{//choice——驱动器选择，x2/x3/x4/x5——写寄存器的数据
  SPIpacka packa;
  unsigned long int dat=0;
  unsigned char datak[5] = {0};
   char i,j,k;
  unsigned char temp=0;
  packa.data[0] = x1;//0x90;
  packa.data[1] = x2;//0xAB;
  packa.data[2] = x3;//0xef;
  packa.data[3] = x4;//0xCD;
  packa.data[4] = x5;//0xbc;
  for(k = 0; k < 2; k++)
  {//连续3次写入
    TMC2160_HORI_CSN_DISABLE;
    TMC2160_VERT_CSN_ENABLE;
    delay_time(2);
    TMC2160_SCLK_HIGH;
    delay_time(2);
//    //片选
//    if(choice == TMC2160_HORI)
//    {
//      TMC2160_HORI_CSN_ENABLE;
//    }else{//TMC2160_VERT
//      TMC2160_VERT_CSN_ENABLE;
//    }
    delay_time(2);
    TMC2160_SCLK_LOW;
     
    delay_time(10);

    for(j = 0; j < 5; j++)
    {
      for(i = 0; i < 8; i++)
      {
        TMC2160_SCLK_LOW;
        if((packa.data[j] & 0x80) != 0)
        {
          TMC2160_SDATI_1;
        }else{
          TMC2160_SDATI_0;
        }
        packa.data[j] = packa.data[j] << 1;
        delay_time(2);
        TMC2160_SCLK_HIGH;
        temp = TMC2160_SDATO_READ;
        datak[j] = datak[j] | (temp << (7-i));
        delay_time(2);
      }
    }
    TMC2160_SCLK_HIGH;
    delay_time(2); 
//片选失能
    TMC2160_VERT_CSN_DISABLE;
    
    dat = datak[0];
    dat = (dat << 8) | datak[1];
    dat = (dat << 8) | datak[2];
    dat = (dat << 8) | datak[3];
    dat = (dat << 8) | datak[4];
//    dat = datak[4];
    
  }
  return dat;
}

///===== 水平电机转向设置
char tmc2160_set_hori_dir(char dir)
{
  if (dir == TMC2160_MOTOR_DIR_FWD)
  {//电机正转
    TMC2160_HORI_DIR_F; return 1;
  }
  if (dir == TMC2160_MOTOR_DIR_REV)
  {//电机反转
    TMC2160_HORI_DIR_R; return 1;
  }
  delay_time(2);
  return 0;
}
///===== 垂直电机转向设置
char tmc2160_set_vert_dir(char dir)
{
  if (dir == TMC2160_MOTOR_DIR_FWD)
  {//电机正转
    TMC2160_VERT_DIR_F; return 1;
  }
  if (dir == TMC2160_MOTOR_DIR_REV)
  {//电机反转
    TMC2160_VERT_DIR_R; return 1;
  }
  delay_time(2);
  return 0;
}

///===== 水平驱动器使能
char tmc2160_hori_motor_enable(char state)
{
  if (state == TMC2160_MOTOR_ENABLE)
  {//启动电机
    TMC2160_HORI_ENABLE; return 1;   
  }
  if (state == TMC2160_MOTOR_DISABLE)
  {//关闭电机
    TMC2160_HORI_DISABLE; return 1;
  }
  delay_time(2);
  return 0;
}
///===== 垂直驱动器使能
char tmc2160_vert_motor_enable(char state)
{
  if (state == TMC2160_MOTOR_ENABLE)
  {//启动电机
    TMC2160_VERT_ENABLE; return 1;   
  }
  if (state == TMC2160_MOTOR_DISABLE)
  {//关闭电机
    TMC2160_VERT_DISABLE; return 1;
  }
  delay_time(2);
  return 0;
}


///===== 写寄存器,mode为寄存器地址,data为电流或细分等级，choice为片选
char tmc2160_write_register(char choice, unsigned char mode, unsigned char data)
{
  u_int8_t buff[5] = {0x00,0x00,0x00,0x00,0x00};
  BSP_OS_SemWait(&ptz_tmc2160_mutex, 0u);

  switch (mode)
  {
    case TMC2160_GCONF:
      buff[0] = TMC2160_GCONF + 0x80;
      buff[1] = 0x00;
      buff[2] = 0x00;
#ifdef PTZ_SUPER_LIGHT_WORM_STEP_TMC2160_AS5047D_24V
      buff[3] = 0x01;//01——磁滞1/16
#endif
      
#ifdef PTZ_MEDIUM_WORM_STEP_TMC2160_AS5047D_24V
      buff[3] = 0x41;//41——磁滞1/32
#endif
      buff[4] = 0x4c;//DIAG0输出过热预警，DIAG1输出电机堵转信号，低电平有效，预警时指示灯亮48
      if(choice == TMC2160_HORI)
      {//读即写
        hori_regis.g_conf = tmc2160_write_hori_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }else{
        vert_regis.g_conf = tmc2160_write_vert_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }
      break;
      
    case TMC2160_GSTAT://全局状态标志，写1清除
      buff[0] = TMC2160_GSTAT +0x80;
      buff[1] = 0xff;
      buff[2] = 0xff;
      buff[3] = 0xff;
      buff[4] = 0xff;
      if(choice == TMC2160_HORI)
      {
        hori_regis.gstae = tmc2160_write_hori_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }else{
        vert_regis.gstae = tmc2160_write_vert_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }
      break;
      
    case TMC2160_DRV_CONF://设置斩波噪音，温度等
      buff[0] = TMC2160_DRV_CONF +0x80;
      buff[1] = 0x00;      
#ifdef PTZ_SUPER_LIGHT_WORM_STEP_TMC2160_AS5047D_24V
      buff[2] = 0x24;//28,08
      buff[3] = 0x04;//02
#endif
      
#ifdef PTZ_MEDIUM_WORM_STEP_TMC2160_AS5047D_24V
      buff[2] = 0x24;//38,
      buff[3] = 0x04;//02
#endif
      buff[4] = 0x10;//00
      if(choice == TMC2160_HORI)
      {
        hori_regis.drv_conf = tmc2160_write_hori_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }else{
        vert_regis.drv_conf = tmc2160_write_vert_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }
      break;
      
    case TMC2160_GLOBAL_SCALER://电流的全局缩放，满量程0
      buff[0] = TMC2160_GLOBAL_SCALER +0x80;
      buff[1] = 0x00;
      buff[2] = 0x00;
      buff[3] = 0x00;
      buff[4] = data;//取值255
      if(choice == TMC2160_HORI)
      {
        hori_regis.global_scaler = tmc2160_write_hori_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }else{
        vert_regis.global_scaler = tmc2160_write_vert_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }
      break;
    
    case TMC2160_IHOLD_IRUN://电机电流设置
      buff[0] = TMC2160_IHOLD_IRUN +0x80;
      buff[1] = 0x00;
      buff[2] = 0x01;
      buff[3] = data;//运行电流=data/32*4.7A对应采样电阻0.05欧
      buff[4] = (data>>1);//静止电流0x00,自锁电流为运行电流的一半
      if(choice == TMC2160_HORI)
      {
        hori_regis.i_run = tmc2160_write_hori_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }else{
        vert_regis.i_run = tmc2160_write_vert_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }
      break;
      
    case TMC2160_TPOWERDOWN://电流降延时时间，2
      buff[0] = TMC2160_TPOWERDOWN +0x80;
      buff[1] = 0x00;
      buff[2] = 0x00;
      buff[3] = 0x00;
      buff[4] = 0x0a;
      if(choice == TMC2160_HORI)
      {
        hori_regis.tpowerdown = tmc2160_write_hori_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }else{
        vert_regis.tpowerdown = tmc2160_write_vert_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }
      break;
      
    case TMC2160_TPWMTHRS:      //暂时不设置,,速度上限
      buff[0] = TMC2160_TPWMTHRS +0x80;
      buff[1] = 0x00;
      buff[2] = 0x00;
      buff[3] = 0x30;
      buff[4] = 0xd4;
      if(choice == TMC2160_HORI)
      {
        hori_regis.tpwmthrs = tmc2160_write_hori_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }else{
        vert_regis.tpwmthrs = tmc2160_write_vert_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }
      break;   
      
    case TMC2160_CHOPCONF:        //0x00010083,24-27位配置细分，最后左移24位
      buff[0] = TMC2160_CHOPCONF +0x80;
      buff[1] = tmc2160_chopconf_data(data);
#ifdef PTZ_SUPER_LIGHT_WORM_STEP_TMC2160_AS5047D_24V
      buff[2] = 0xe1;//e1,,高4为被动快速衰减时间，可降低中程共振
      buff[3] = 0x03;//03
      buff[4] = 0x34;//34
#endif
      
#ifdef PTZ_MEDIUM_WORM_STEP_TMC2160_AS5047D_24V
      buff[2] = 0xf1;//e1,,高4为被动快速衰减时间，可降低中程共振
      buff[3] = 0x03;//03，00
      buff[4] = 0x04;//04，64，低4设置TOFF取3或4，高3设置hstrt，滞环过小振动噪音增加，过大斩波噪音增加
#endif
      if(choice == TMC2160_HORI)
      {
        hori_regis.chop_conf = tmc2160_write_hori_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }else{
        vert_regis.chop_conf = tmc2160_write_vert_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }
      break;
       
  case TMC2160_COOLCONF:      ///暂时不写...智能电流控制
      buff[0] = TMC2160_COOLCONF +0x80;
      buff[1] = 0x00;
      buff[2] = 0x00;
      buff[3] = 0xA8;
      buff[4] = 0x28;
      if(choice == TMC2160_HORI)
      {
        hori_regis.coolconf = tmc2160_write_hori_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }else{
        vert_regis.coolconf = tmc2160_write_vert_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }
      break;
             
    case  TMC2160_PWMCONF:         //0xc401321e，时钟35.1KHZ
      buff[0] = TMC2160_PWMCONF +0x80;
      buff[1] = 0xd2;//c4
      buff[2] = 0x0E;//09,0D
      buff[3] = 0x32;
      buff[4] = 0x1e;
#ifdef PTZ_MEDIUM_WORM_STEP_TMC2160_AS5047D_24V
      buff[3] = 0x32;
      buff[4] = 0x24;//0x6e
#endif
      if(choice == TMC2160_HORI)
      {
        tmc2160_write_hori_data(choice, buff[0],buff[1],0x09,buff[3],buff[4]);
        
        hori_regis.pwmconf = tmc2160_write_hori_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }else{
        tmc2160_write_vert_data(choice, buff[0],buff[1],0x09,buff[3],buff[4]);
        
        vert_regis.pwmconf = tmc2160_write_vert_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }
      break;  
      
    case  TMC2160_SHORT_CONF:
      buff[0] = TMC2160_SHORT_CONF +0x80;
      buff[1] = 0x00;
      buff[2] = 0x06;
      buff[3] = 0x0c;
      buff[4] = 0x0c;
      if(choice == TMC2160_HORI)
      {
        hori_regis.short_conf = tmc2160_write_hori_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }else{
        vert_regis.short_conf = tmc2160_write_vert_data(choice, buff[0],buff[1],buff[2],buff[3],buff[4]);
      }    
      break;
  }
  
  BSP_OS_SemPost(&ptz_tmc2160_mutex);
  return 0;
}

unsigned long int tmc2160_ihold_irun_data(unsigned char data)
{//电流设置共5位，第8-12位，因此要左移8位
  unsigned long int write_data = 0;
  write_data = write_data | data << 8;
  return write_data;
}

unsigned long int tmc2160_chopconf_data(unsigned char data)
{//驱动细分共4位，第24-27位
  //默认值写0x00010083,256细分
  unsigned char write_data = 0x10;
//  write_data = 0x00010083;
  switch (data)//此时data作为细分等级
  {
    case TMC2160_1_8_STEP:    //3，，8分频
      write_data = write_data | 0x05;// << 24;
      break;
      
    case TMC2160_1_16_STEP:   //4，，16分频
      write_data = write_data | 0x04;// << 24;
      break;
      
    case TMC2160_1_32_STEP:   //5，，32分频
      write_data = write_data | 0x03;// << 24;
      break;
      
    case TMC2160_1_64_STEP:   //6，，64分频
      write_data = write_data | 0x02;// << 24;
      break;
      
    case TMC2160_1_128_STEP:  //7，，128分频
      write_data = write_data | 0x01;// << 24;
      break;
  }
  return write_data;
}


///===== 计算效验码
unsigned int tmc2160_parameter_crc(TMC2160Parameter data)
{
  float crc = 0;
  unsigned int crc1 = 0;
  crc = crc + data.hori_step_1_8_en;
  crc = crc + data.hori_step_1_8_v_max;
  crc = crc + data.hori_step_1_8_v_min;
  
  crc = crc + data.hori_step_1_16_en;
  crc = crc + data.hori_step_1_16_v_max;
  crc = crc + data.hori_step_1_16_v_min;
  
  crc = crc + data.hori_step_1_32_en;
  crc = crc + data.hori_step_1_32_v_max;
  crc = crc + data.hori_step_1_32_v_min;

  crc = crc + data.hori_step_1_64_en;
  crc = crc + data.hori_step_1_64_v_max;
  crc = crc + data.hori_step_1_64_v_min;
  
  crc = crc + data.hori_step_1_128_en;
  crc = crc + data.hori_step_1_128_v_max;
  crc = crc + data.hori_step_1_128_v_min;
  
  crc = crc + data.hori_work_torque;
  crc = crc + data.hori_global_scaler;
  
  crc = crc + data.hori_acc_time_max;//电机从0加速到最大速度所用时间
  crc = crc + data.hori_dec_time_max;//电机从最大速度减速到0所用时间
  
  
  crc = crc + data.vert_step_1_8_en;
  crc = crc + data.vert_step_1_8_v_max;
  crc = crc + data.vert_step_1_8_v_min;
  
  crc = crc + data.vert_step_1_16_en;
  crc = crc + data.vert_step_1_16_v_max;
  crc = crc + data.vert_step_1_16_v_min;
  
  crc = crc + data.vert_step_1_32_en;
  crc = crc + data.vert_step_1_32_v_max;
  crc = crc + data.vert_step_1_32_v_min;

  crc = crc + data.vert_step_1_64_en;
  crc = crc + data.vert_step_1_64_v_max;
  crc = crc + data.vert_step_1_64_v_min;
  
  crc = crc + data.vert_step_1_128_en;
  crc = crc + data.vert_step_1_128_v_max;
  crc = crc + data.vert_step_1_128_v_min;
  
  crc = crc + data.vert_work_torque;
  crc = crc + data.vert_global_scaler;
  
  crc = crc + data.vert_acc_time_max;//电机从0加速到最大速度所用时间
  crc = crc + data.vert_dec_time_max;//电机从最大速度减速到0所用时间
  
  crc1 = (unsigned int)crc;
  return crc1;
}

///===== 运动参数存储
char tmc2160_parameter_save()
{
  TMC2160Parameter tmc2160_parameter_a;
  char i,j;
  //计算效验码
  tmc2160_parameter.crc = tmc2160_parameter_crc(tmc2160_parameter);
  
  //写入主存储区
  for(i = 0; i < TMC2160_PARAMETER_SAVE_FLASH_NUM; i++)
  {
    memset(&tmc2160_parameter_a, 0, sizeof(tmc2160_parameter_a));
    write_many_data(sizeof(tmc2160_parameter),(unsigned char *)(&tmc2160_parameter),TMC2160_PARAMETER_FLASH_ADD);
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
    Flash_Read((unsigned char *)(&tmc2160_parameter_a),TMC2160_PARAMETER_FLASH_ADD,sizeof(tmc2160_parameter_a));
    if(memcmp(&tmc2160_parameter, &tmc2160_parameter_a, sizeof(tmc2160_parameter)) == 0)//判断写入数据是否正确
    {
      break;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
  }
  //写入备份存储区
  for(j = 0; j < TMC2160_PARAMETER_SAVE_FLASH_NUM; j++)
  {
    memset(&tmc2160_parameter_a, 0, sizeof(tmc2160_parameter_a));
    write_many_data(sizeof(tmc2160_parameter),(unsigned char *)(&tmc2160_parameter),TMC2160_PARAMETER_BACKUP_FLASH_ADD);
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
    Flash_Read((unsigned char *)(&tmc2160_parameter_a),TMC2160_PARAMETER_BACKUP_FLASH_ADD,sizeof(tmc2160_parameter_a));
    if(memcmp(&tmc2160_parameter, &tmc2160_parameter_a, sizeof(tmc2160_parameter)) == 0)//判断写入数据是否正确
    {
      break;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
  }
  
  if(i < TMC2160_PARAMETER_SAVE_FLASH_NUM || j < TMC2160_PARAMETER_SAVE_FLASH_NUM)//判断写入数据是否成功
  {
    //写入成功
    return 1;
  }
  //写入失败
  return 0;
}

///===== 运动参数读取
char tmc2160_parameter_read()
{
  char i;
  //首先从主存储区读取数据
  for(i = 0; i < TMC2160_PARAMETER_READ_FLASH_NUM; i++)
  {
    memset(&tmc2160_parameter, 0, sizeof(tmc2160_parameter));
    Flash_Read((unsigned char *)(&tmc2160_parameter),TMC2160_PARAMETER_FLASH_ADD,sizeof(tmc2160_parameter));
    if(tmc2160_parameter.crc == tmc2160_parameter_crc(tmc2160_parameter) && tmc2160_parameter.crc > 0)
    {
      return 1;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 2u);
  }
  //如果主存储区读取的数据是错误的，则从备份存储区读取数据
  for(i = 0; i < TMC2160_PARAMETER_READ_FLASH_NUM; i++)
  {
    memset(&tmc2160_parameter, 0, sizeof(tmc2160_parameter));
    Flash_Read((unsigned char *)(&tmc2160_parameter),TMC2160_PARAMETER_BACKUP_FLASH_ADD,sizeof(tmc2160_parameter));
    if(tmc2160_parameter.crc == tmc2160_parameter_crc(tmc2160_parameter) && tmc2160_parameter.crc > 0)
    {
      return 1;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 2u);
  }
  return 0;
}

///===== 运动参数擦除
char tmc2160_parameter_erase()
{
  TMC2160Parameter tmc2160_parameter_a;
  TMC2160Parameter tmc2160_parameter_b;
  char i, j;
 
  //擦除主存储区
  for(i = 0; i < TMC2160_PARAMETER_SAVE_FLASH_NUM; i++)
  {
    memset(&tmc2160_parameter_a, 0, sizeof(tmc2160_parameter_a));
    write_many_data(sizeof(tmc2160_parameter_a),(unsigned char *)(&tmc2160_parameter_a),TMC2160_PARAMETER_FLASH_ADD);
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
    Flash_Read((unsigned char *)(&tmc2160_parameter_b),TMC2160_PARAMETER_FLASH_ADD,sizeof(tmc2160_parameter_b));
    if(memcmp(&tmc2160_parameter_a, &tmc2160_parameter_b, sizeof(tmc2160_parameter_a)) == 0)//判断擦除数据是否正确
    {
      break;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
  }
  //擦除备份存储区
  for(j = 0; j < TMC2160_PARAMETER_SAVE_FLASH_NUM; j++)
  {
    memset(&tmc2160_parameter_a, 0, sizeof(tmc2160_parameter_a));
    write_many_data(sizeof(tmc2160_parameter_a),(unsigned char *)(&tmc2160_parameter_a),TMC2160_PARAMETER_BACKUP_FLASH_ADD);
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
    Flash_Read((unsigned char *)(&tmc2160_parameter_b),TMC2160_PARAMETER_BACKUP_FLASH_ADD,sizeof(tmc2160_parameter_b));
    if(memcmp(&tmc2160_parameter_a, &tmc2160_parameter_b, sizeof(tmc2160_parameter_a)) == 0)//判断擦除数据是否正确
    {
      break;
    }
    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
  }
  
  if(i < TMC2160_PARAMETER_SAVE_FLASH_NUM || j < TMC2160_PARAMETER_SAVE_FLASH_NUM)//判断擦除数据是否成功
  {
    //擦除成功
    return 1;
  }
  //擦除失败
  return 0;
}

///===== 运动参数赋值
char tmc2160_data_reset()
{
  tmc2160_parameter.hori_step_1_8_en = 1;//是否有效，0无效，1有效
  tmc2160_parameter.hori_step_1_8_v_max = PTZ_HORI_1_8_STEP_V_MAX;//该细分程度下对应得最大转速
  tmc2160_parameter.hori_step_1_8_v_min = PTZ_HORI_1_8_STEP_V_MIN;//该细分程度下对应得最小转速
  
  tmc2160_parameter.hori_step_1_16_en = 1;//是否有效，0无效，1有效
  tmc2160_parameter.hori_step_1_16_v_max = PTZ_HORI_1_16_STEP_V_MAX;//该细分程度下对应得最大转速
  tmc2160_parameter.hori_step_1_16_v_min = PTZ_HORI_1_16_STEP_V_MIN;//该细分程度下对应得最小转速
  
  tmc2160_parameter.hori_step_1_32_en = 1;//是否有效，0无效，1有效
  tmc2160_parameter.hori_step_1_32_v_max = PTZ_HORI_1_32_STEP_V_MAX;//该细分程度下对应得最大转速
  tmc2160_parameter.hori_step_1_32_v_min = PTZ_HORI_1_32_STEP_V_MIN;//该细分程度下对应得最小转速
  
  tmc2160_parameter.hori_step_1_64_en = 1;//是否有效，0无效，1有效
  tmc2160_parameter.hori_step_1_64_v_max = PTZ_HORI_1_64_STEP_V_MAX;//该细分程度下对应得最大转速
  tmc2160_parameter.hori_step_1_64_v_min = PTZ_HORI_1_64_STEP_V_MIN;//该细分程度下对应得最小转速
  
  tmc2160_parameter.hori_step_1_128_en = 1;//是否有效，0无效，1有效
  tmc2160_parameter.hori_step_1_128_v_max = PTZ_HORI_1_128_STEP_V_MAX;//该细分程度下对应得最大转速
  tmc2160_parameter.hori_step_1_128_v_min = PTZ_HORI_1_128_STEP_V_MIN;//该细分程度下对应得最小转速
  
  tmc2160_parameter.hori_work_torque = TMC2160_HORI_TORQUE_VALUE;//工作时torque的值
  tmc2160_parameter.hori_global_scaler = CURR_GLOBAL_SCALER;//电流全局缩放
 
  tmc2160_parameter.hori_acc_time_max = PTZ_HORI_ACC_TIME_MAX;//电机从0加速到最大速度所用时间
  tmc2160_parameter.hori_dec_time_max = PTZ_HORI_DEC_TIME_MAX;//电机从最大速度减速到0所用时间
  
  
  
  tmc2160_parameter.vert_step_1_8_en = 0;//是否有效，0无效，1有效
  tmc2160_parameter.vert_step_1_8_v_max = 0;//该细分程度下对应得最大转速
  tmc2160_parameter.vert_step_1_8_v_min = 0;//该细分程度下对应得最小转速
  
  tmc2160_parameter.vert_step_1_16_en = 1;//是否有效，0无效，1有效
  tmc2160_parameter.vert_step_1_16_v_max = PTZ_VERT_1_16_STEP_V_MAX;//该细分程度下对应得最大转速
  tmc2160_parameter.vert_step_1_16_v_min = PTZ_VERT_1_16_STEP_V_MIN;//该细分程度下对应得最小转速
  
  tmc2160_parameter.vert_step_1_32_en = 1;//是否有效，0无效，1有效
  tmc2160_parameter.vert_step_1_32_v_max = PTZ_VERT_1_32_STEP_V_MAX;//该细分程度下对应得最大转速
  tmc2160_parameter.vert_step_1_32_v_min = PTZ_VERT_1_32_STEP_V_MIN;//该细分程度下对应得最小转速
  
  tmc2160_parameter.vert_step_1_64_en = 1;//是否有效，0无效，1有效
  tmc2160_parameter.vert_step_1_64_v_max = PTZ_VERT_1_64_STEP_V_MAX;//该细分程度下对应得最大转速
  tmc2160_parameter.vert_step_1_64_v_min = PTZ_VERT_1_64_STEP_V_MIN;//该细分程度下对应得最小转速
  
  tmc2160_parameter.vert_step_1_128_en = 1;//是否有效，0无效，1有效
  tmc2160_parameter.vert_step_1_128_v_max = PTZ_VERT_1_128_STEP_V_MAX;//该细分程度下对应得最大转速
  tmc2160_parameter.vert_step_1_128_v_min = PTZ_VERT_1_128_STEP_V_MIN;//该细分程度下对应得最小转速
  
  tmc2160_parameter.vert_work_torque = TMC2160_VERT_TORQUE_VALUE;//工作时torque的值
  tmc2160_parameter.vert_global_scaler = CURR_GLOBAL_SCALER;//电流全局缩放
 
  tmc2160_parameter.vert_acc_time_max = PTZ_VERT_ACC_TIME_MAX;//电机从0加速到最大速度所用时间
  tmc2160_parameter.vert_dec_time_max = PTZ_VERT_DEC_TIME_MAX;//电机从最大速度减速到0所用时间
  
  tmc2160_parameter.crc = tmc2160_parameter_crc(tmc2160_parameter);//效验码
  return 1;
  
}

///===== 驱动器复位
char tmc2160_reset()
{
  tmc2160_data_reset();
  
  //水平
  tmc2160_write_register(TMC2160_HORI, TMC2160_IHOLD_IRUN, TMC2160_HORI_TORQUE_VALUE);//电流默认值=20/32*2.3A
  tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, TMC2160_1_16_STEP);//细分默认16
  //垂直
  tmc2160_write_register(TMC2160_VERT, TMC2160_IHOLD_IRUN, TMC2160_VERT_TORQUE_VALUE);//电流默认值=20/32*2.3A
  tmc2160_write_register(TMC2160_VERT, TMC2160_CHOPCONF, TMC2160_1_16_STEP);//细分默认16
  if (tmc2160_parameter_save() == 1)
  {//复位成功
    return 1;
  }
  return 0;
}

///===== 参数初始化
char tmc2160_data_init()
{
  tmc2160_parameter_read();
  if (tmc2160_parameter_read() == 1)
  {//读取成功
    return 1;
  }else{//读取失败
    tmc2160_data_reset();
    tmc2160_parameter_save();
    return 0;
  }
}

///===== 数据上传
void tmc2160_parameter_upload(char dev)
{
  unsigned char data[7] = {0};
//*****************************发送水平参数**********************************//
  /*********************************1_8*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x09;
  data[4] = tmc2160_parameter.hori_step_1_8_en;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
 
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x0a;
  data[4] = ((unsigned short int)(tmc2160_parameter.hori_step_1_8_v_min * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.hori_step_1_8_v_min * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
 
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x0b;
  data[4] = ((unsigned short int)(tmc2160_parameter.hori_step_1_8_v_max * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.hori_step_1_8_v_max * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  /*********************************1_16*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x0c;
  data[4] = tmc2160_parameter.hori_step_1_16_en;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x0d;
  data[4] = ((unsigned short int)(tmc2160_parameter.hori_step_1_16_v_min * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.hori_step_1_16_v_min * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x0e;
  data[4] = ((unsigned short int)(tmc2160_parameter.hori_step_1_16_v_max * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.hori_step_1_16_v_max * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  
  /*********************************1_32*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x0f;
  data[4] = tmc2160_parameter.hori_step_1_32_en;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
 
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x10;
  data[4] = ((unsigned short int)(tmc2160_parameter.hori_step_1_32_v_min * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.hori_step_1_32_v_min * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x11;
  data[4] = ((unsigned short int)(tmc2160_parameter.hori_step_1_32_v_max * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.hori_step_1_32_v_max * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
 
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  
  /*********************************1_64*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x12;
  data[4] = tmc2160_parameter.hori_step_1_64_en;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x13;
  data[4] = ((unsigned short int)(tmc2160_parameter.hori_step_1_64_v_min * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.hori_step_1_64_v_min * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x14;
  data[4] = ((unsigned short int)(tmc2160_parameter.hori_step_1_64_v_max * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.hori_step_1_64_v_max * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  
  /*********************************1_128*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x15;
  data[4] = tmc2160_parameter.hori_step_1_128_en;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x16;
  data[4] = ((unsigned short int)(tmc2160_parameter.hori_step_1_128_v_min * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.hori_step_1_128_v_min * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x17;
  data[4] = ((unsigned short int)(tmc2160_parameter.hori_step_1_128_v_max * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.hori_step_1_128_v_max * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
 
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  /*********************************work_torque*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x20;
  data[4] = tmc2160_parameter.hori_work_torque;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  /********************************global_scaler***********************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x21;
  data[4] = tmc2160_parameter.hori_global_scaler;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  /*********************************acc_time*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x23;
  data[4] = tmc2160_parameter.hori_acc_time_max >> 8;
  data[5] = tmc2160_parameter.hori_acc_time_max & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  /*********************************dec_time*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x51;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x24;
  data[4] = tmc2160_parameter.hori_dec_time_max >> 8;
  data[5] = tmc2160_parameter.hori_dec_time_max & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
//************************************************************************************************************************/
  
  
  
//****************************发送垂直参数************************************//
  /*********************************1_8*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x09;
  data[4] = tmc2160_parameter.vert_step_1_8_en;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x0a;
  data[4] = ((unsigned short int)(tmc2160_parameter.vert_step_1_8_v_min * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.vert_step_1_8_v_min * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x0b;
  data[4] = ((unsigned short int)(tmc2160_parameter.vert_step_1_8_v_max * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.vert_step_1_8_v_max * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
   
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  /*********************************1_16*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x0c;
  data[4] = tmc2160_parameter.vert_step_1_16_en;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
   
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x0d;
  data[4] = ((unsigned short int)(tmc2160_parameter.vert_step_1_16_v_min * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.vert_step_1_16_v_min * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
   
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x0e;
  data[4] = ((unsigned short int)(tmc2160_parameter.vert_step_1_16_v_max * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.vert_step_1_16_v_max * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  
  /*********************************1_32*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x0f;
  data[4] = tmc2160_parameter.vert_step_1_32_en;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
   
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x10;
  data[4] = ((unsigned short int)(tmc2160_parameter.vert_step_1_32_v_min * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.vert_step_1_32_v_min * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x11;
  data[4] = ((unsigned short int)(tmc2160_parameter.vert_step_1_32_v_max * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.vert_step_1_32_v_max * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  
  /*********************************1_64*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x12;
  data[4] = tmc2160_parameter.vert_step_1_64_en;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x13;
  data[4] = ((unsigned short int)(tmc2160_parameter.vert_step_1_64_v_min * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.vert_step_1_64_v_min * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x14;
  data[4] = ((unsigned short int)(tmc2160_parameter.vert_step_1_64_v_max * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.vert_step_1_64_v_max * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  
  /**********************************1_128**************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x15;
  data[4] = tmc2160_parameter.vert_step_1_128_en;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x16;
  data[4] = ((unsigned short int)(tmc2160_parameter.vert_step_1_128_v_min * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.vert_step_1_128_v_min * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x17;
  data[4] = ((unsigned short int)(tmc2160_parameter.vert_step_1_128_v_max * 10)) >> 8;
  data[5] = ((unsigned short int)(tmc2160_parameter.vert_step_1_128_v_max * 10)) & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
   
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  /*********************************work_torque**********************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x20;
  data[4] = tmc2160_parameter.vert_work_torque;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  /********************************global_scaler*********************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x21;
  data[4] = tmc2160_parameter.vert_global_scaler;
  data[5] = 0;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  /*********************************acc_time*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x23;
  data[4] = tmc2160_parameter.vert_acc_time_max >> 8;
  data[5] = tmc2160_parameter.vert_acc_time_max & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  
  /*********************************dec_time*************************************/
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = 0x52;//0X51代表发送的是水平,0X52代表发送的是垂直
  data[3] = 0x24;
  data[4] = tmc2160_parameter.vert_dec_time_max >> 8;
  data[5] = tmc2160_parameter.vert_dec_time_max & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
   
//************************************************************************************************************************/
}

///===== tmc2160数据处理
void tmc2160_parameter_process(char dev, PTZ_DATA_PACK *pack)
{
  float speed;
  if(pack->command[0] == 0x51)//水平电机
  {
    speed = (float)((pack->data[0] << 8) | pack->data[1]) / 10.0;
    if(speed > PTZ_HORI_MOTOR_MAX_SPEED)
    {
      speed = PTZ_HORI_MOTOR_MAX_SPEED;
    }
    if(speed < 0)
    {
      speed = 0;
    }
    
    switch(pack->command[1]) 
    {
      case 0x09:
        if(pack->data[0] == 1)
        {
          tmc2160_parameter.hori_step_1_8_en = 1;
        }
        else
        {
          tmc2160_parameter.hori_step_1_8_en = 0;
        }
        break;
        
      case 0x0a:
        tmc2160_parameter.hori_step_1_8_v_min = speed;
        break;
        
      case 0x0b:
        tmc2160_parameter.hori_step_1_8_v_max = speed;
        break;
        
/*******************************************************************************************/
         
      case 0x0c:
        if(pack->data[0] == 1)
        {
          tmc2160_parameter.hori_step_1_16_en = 1;
        }
        else
        {
          tmc2160_parameter.hori_step_1_16_en = 0;
        }
        break;
        
      case 0x0d:
        tmc2160_parameter.hori_step_1_16_v_min = speed;
        break;
        
      case 0x0e:
        tmc2160_parameter.hori_step_1_16_v_max = speed;
        break;
       
/*******************************************************************************************/
         
      case 0x0f:
        if(pack->data[0] == 1)
        {
          tmc2160_parameter.hori_step_1_32_en = 1;
        }
        else
        {
          tmc2160_parameter.hori_step_1_32_en = 0;
        }
        break;
        
      case 0x10:
        tmc2160_parameter.hori_step_1_32_v_min = speed;
        break;
        
      case 0x11:
        tmc2160_parameter.hori_step_1_32_v_max = speed;
        break;
       
/*******************************************************************************************/
         
      case 0x12:
        if(pack->data[0] == 1)
        {
          tmc2160_parameter.hori_step_1_64_en = 1;
        }
        else
        {
          tmc2160_parameter.hori_step_1_64_en = 0;
        }
        break;
        
      case 0x13:
        tmc2160_parameter.hori_step_1_64_v_min = speed;
        break;
        
      case 0x14:
        tmc2160_parameter.hori_step_1_64_v_max = speed;
        break;
        
/*******************************************************************************************/
         
      case 0x15:
        if(pack->data[0] == 1)
        {
          tmc2160_parameter.hori_step_1_128_en = 1;
        }
        else
        {
          tmc2160_parameter.hori_step_1_128_en = 0;
        }
        break;
        
      case 0x16:
        tmc2160_parameter.hori_step_1_128_v_min = speed;
        break;
        
      case 0x17:
        tmc2160_parameter.hori_step_1_128_v_max = speed;
        break;
       
/*******************************************************************************************/
  
      
    case 0x20:
      tmc2160_parameter.hori_work_torque = pack->data[0];
      if(tmc2160_parameter.hori_work_torque > TMC2160_WORK_TORQUE_MAX)
      {
        tmc2160_parameter.hori_work_torque = TMC2160_WORK_TORQUE_MAX;
      }
      if(tmc2160_parameter.hori_work_torque < TMC2160_WORK_TORQUE_MIN)
      {
        tmc2160_parameter.hori_work_torque = TMC2160_WORK_TORQUE_MIN;
      }
      tmc2160_write_register(TMC2160_HORI, TMC2160_IHOLD_IRUN, tmc2160_parameter.hori_work_torque);
      break;
      
      case 0x21:
      tmc2160_parameter.hori_global_scaler = pack->data[0];      
      tmc2160_write_register(TMC2160_HORI, TMC2160_GLOBAL_SCALER, tmc2160_parameter.hori_global_scaler);
      break;
      
    case 0x23:
      tmc2160_parameter.hori_acc_time_max =  (pack->data[0] << 8) | pack->data[1];
      break;

    case 0x24:
      tmc2160_parameter.hori_dec_time_max =  (pack->data[0] << 8) | pack->data[1];
      break;
    }
        
  }
  
  if(pack->command[0] == 0x52)//垂直电机
  {
    speed = (float)((pack->data[0] << 8) | pack->data[1]) / 10.0;
    if(speed > PTZ_VERT_MOTOR_MAX_SPEED)
    {
      speed = PTZ_VERT_MOTOR_MAX_SPEED;
    }
    if(speed < 0)
    {
      speed = 0;
    }
    
    switch(pack->command[1]) 
    {
      case 0x09:
        if(pack->data[0] == 1)
        {
          tmc2160_parameter.vert_step_1_8_en = 1;
        }
        else
        {
          tmc2160_parameter.vert_step_1_8_en = 0;
        }
        break;
        
      case 0x0a:
        tmc2160_parameter.vert_step_1_8_v_min = speed;
        break;
        
      case 0x0b:
        tmc2160_parameter.vert_step_1_8_v_max = speed;
        break;
        
/*******************************************************************************************/
         
      case 0x0c:
        if(pack->data[0] == 1)
        {
          tmc2160_parameter.vert_step_1_16_en = 1;
        }
        else
        {
          tmc2160_parameter.vert_step_1_16_en = 0;
        }
        break;
        
      case 0x0d:
        tmc2160_parameter.vert_step_1_16_v_min = speed;
        break;
        
      case 0x0e:
        tmc2160_parameter.vert_step_1_16_v_max = speed;
        break;
       
/*******************************************************************************************/
         
      case 0x0f:
        if(pack->data[0] == 1)
        {
          tmc2160_parameter.vert_step_1_32_en = 1;
        }
        else
        {
          tmc2160_parameter.vert_step_1_32_en = 0;
        }
        break;
        
      case 0x10:
        tmc2160_parameter.vert_step_1_32_v_min = speed;
        break;
        
      case 0x11:
        tmc2160_parameter.vert_step_1_32_v_max = speed;
        break;
       
/*******************************************************************************************/
         
      case 0x12:
        if(pack->data[0] == 1)
        {
          tmc2160_parameter.vert_step_1_64_en = 1;
        }
        else
        {
          tmc2160_parameter.vert_step_1_64_en = 0;
        }
        break;
        
      case 0x13:
        tmc2160_parameter.vert_step_1_64_v_min = speed;
        break;
        
      case 0x14:
        tmc2160_parameter.vert_step_1_64_v_max = speed;
        break;
        
/*******************************************************************************************/
         
      case 0x15:
        if(pack->data[0] == 1)
        {
          tmc2160_parameter.vert_step_1_128_en = 1;
        }
        else
        {
          tmc2160_parameter.vert_step_1_128_en = 0;
        }
        break;
        
      case 0x16:
        tmc2160_parameter.vert_step_1_128_v_min = speed;
        break;
        
      case 0x17:
        tmc2160_parameter.vert_step_1_128_v_max = speed;
        break;
       
/*******************************************************************************************/
        
      case 0x20:
        tmc2160_parameter.vert_work_torque = pack->data[0];
        if(tmc2160_parameter.vert_work_torque > TMC2160_WORK_TORQUE_MAX)
        {
          tmc2160_parameter.vert_work_torque = TMC2160_WORK_TORQUE_MAX;
        }
        if(tmc2160_parameter.vert_work_torque < TMC2160_WORK_TORQUE_MIN)
        {
          tmc2160_parameter.vert_work_torque = TMC2160_WORK_TORQUE_MIN;
        }
        tmc2160_write_register(TMC2160_VERT, TMC2160_IHOLD_IRUN, tmc2160_parameter.vert_work_torque);
        break;
        
     case 0x21:
        tmc2160_parameter.vert_global_scaler = pack->data[0];      
        tmc2160_write_register(TMC2160_VERT, TMC2160_GLOBAL_SCALER, tmc2160_parameter.vert_global_scaler);
        break;
        
      case 0x23:
        tmc2160_parameter.vert_acc_time_max =  (pack->data[0] << 8) | pack->data[1];
        break;

      case 0x24:
        tmc2160_parameter.vert_dec_time_max =  (pack->data[0] << 8) | pack->data[1];
        break;

    }
        
  }

  if(pack->command[0] == 0x53)//数据操作
  {
    switch(pack->command[1]) 
    {
      case 0x00://读取并回传DRV8711参数
        //读取
        tmc2160_parameter_read();
        //回传
        tmc2160_parameter_upload(dev);
        break;
        
      case 0x01://保存DRV8711参数
        tmc2160_parameter_save();
        break;
        
      case 0x02://删除DRV8711在FLASH中的参数
        tmc2160_parameter_erase();
        break;
        
      case 0x03://复位DRV8711在FLASH中保存的参数
//        tmc2160_write_register(TMC2160_HORI, TMC2160_GSTAT, 0);
//        OSTimeDlyHMSM(0u, 0u, 0u, 1u);
//        tmc2160_write_register(TMC2160_VERT, TMC2160_GSTAT, 0);
//        OSTimeDlyHMSM(0u, 0u, 0u, 1u);
        tmc2160_reset();
        
        break;

    }
  }

}

///===== 驱动器状态读取
unsigned int tmc2160_state_read(char choice, char regis)
{
  unsigned long int status;
  if(choice == TMC2160_HORI)
  {
    status = tmc2160_write_hori_data(choice, regis,0xff,0xff,0xff,0xff);//choice——驱动器选择，x2/x3/x4/x5——写寄存器的数据
  }
  if(choice == TMC2160_VERT)
  {
    status = tmc2160_write_vert_data(choice, regis,0xff,0xff,0xff,0xff);//choice——驱动器选择，x2/x3/x4/x5——写寄存器的数据
  }
  return status;
}

void tmc2160_state_return(char dev)
{
  unsigned char data[7] = {0};
  unsigned long int status;
  //****************************发送水平状态******************//
  status = tmc2160_state_read(TMC2160_HORI,TMC2160_DRV_STATUS);
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = (status >> 24) & 0x00ff;//停止、相电源开路、短路、过温警告、过温触发、堵转状态,最高8位
  data[3] = (status >> 16) & 0x00ff;
  data[4] = (status >> 8) & 0x00ff;
  data[5] = status & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  //****************************读引脚状态******************//
  status = tmc2160_state_read(TMC2160_HORI,TMC2160_IOIN);
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = (status >> 24) & 0x00ff;//高8位为版本号，低8位为输入引脚状态
  data[3] = 0x00ff;
  data[4] = 0x00ff;
  data[5] = status & 0x00ff;//此低8位为引脚状态，0位脉冲信号、1位转向信号、4位驱动器使能信号。
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  OSTimeDlyHMSM(0u, 0u, 0u, 20u);
  //**************************发送垂直状态******************//
  status = tmc2160_state_read(TMC2160_VERT,TMC2160_DRV_STATUS);
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = (status >> 24) & 0x00ff;//停止、相电源开路、短路、过温警告、过温触发、堵转状态,最高8位
  data[3] = (status >> 16) & 0x00ff;
  data[4] = (status >> 8) & 0x00ff;
  data[5] = status & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  OSTimeDlyHMSM(0u, 0u, 0u, 10u);
  //****************************读引脚状态******************//
  status = tmc2160_state_read(TMC2160_VERT,TMC2160_IOIN);
  data[0] = 0xff;
  data[1] = g_ptz.address;
  data[2] = (status >> 24) & 0x00ff;//高8位为版本号，低8位为输入引脚状态
  data[3] = 0x00ff;
  data[4] = 0x00ff;
  data[5] = status & 0x00ff;
  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
  ptz_send_data(dev, data, sizeof(data));
  
  /**********************以下测试用，用于stealthchop参数自整定*********************/
  
//  OSTimeDlyHMSM(0u, 0u, 0u, 20u);
//  //****************************发送水平自整定值******************//
//  status = tmc2160_state_read(TMC2160_HORI,TMC2160_PWM_AUTO);
//  data[0] = 0xff;
//  data[1] = g_ptz.address;
//  data[2] = (status >> 24) & 0x00ff;
//  data[3] = (status >> 16) & 0x00ff;
//  data[4] = (status >> 8) & 0x00ff;
//  data[5] = status & 0x00ff;
//  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
//  ptz_send_data(dev, data, sizeof(data));
//  
//  OSTimeDlyHMSM(0u, 0u, 0u, 20u);
//  //****************************发送垂直自整定值******************//
//  status = tmc2160_state_read(TMC2160_VERT,TMC2160_PWM_AUTO);
//  data[0] = 0xff;
//  data[1] = g_ptz.address;
//  data[2] = (status >> 24) & 0x00ff;
//  data[3] = (status >> 16) & 0x00ff;
//  data[4] = (status >> 8) & 0x00ff;
//  data[5] = status & 0x00ff;
//  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
//  ptz_send_data(dev, data, sizeof(data));
//  
//  OSTimeDlyHMSM(0u, 0u, 0u, 20u);
//  //*****************************发送水平调幅结果******************//
//  status = tmc2160_state_read(TMC2160_HORI,TMC2160_PWM_SCALE);
//  data[0] = 0xff;
//  data[1] = g_ptz.address;
//  data[2] = (status >> 24) & 0x00ff;
//  data[3] = (status >> 16) & 0x00ff;
//  data[4] = (status >> 8) & 0x00ff;
//  data[5] = status & 0x00ff;
//  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
//  ptz_send_data(dev, data, sizeof(data));
//  
//  OSTimeDlyHMSM(0u, 0u, 0u, 20u);
//  //*****************************发送垂直调幅结果******************//
//  status = tmc2160_state_read(TMC2160_VERT,TMC2160_PWM_SCALE);
//  data[0] = 0xff;
//  data[1] = g_ptz.address;
//  data[2] = (status >> 24) & 0x00ff;
//  data[3] = (status >> 16) & 0x00ff;
//  data[4] = (status >> 8) & 0x00ff;
//  data[5] = status & 0x00ff;
//  data[6] = MotorCalPelcoDSUM(data,sizeof(data));
//  ptz_send_data(dev, data, sizeof(data));
}



#endif 