1800 lines
58 KiB
C
1800 lines
58 KiB
C
#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 h_tmc2160_power_on()
|
||
{
|
||
//电源打开
|
||
TMC2160_H_POWER_ON;
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
|
||
//====初始化驱动器参数
|
||
tmc2160_data_init();
|
||
|
||
TMC2160_HORI_CSN_DISABLE;
|
||
TMC2160_HORI_DISABLE;
|
||
|
||
//====上电清除全局状态标志
|
||
tmc2160_write_register(TMC2160_HORI, TMC2160_GSTAT, 0);
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
|
||
//====寄存器通用配置
|
||
//水平
|
||
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
|
||
|
||
}
|
||
|
||
///驱动器重新上电后自动复位，全部置零
|
||
void v_tmc2160_power_on()
|
||
{
|
||
//电源打开
|
||
TMC2160_V_POWER_ON;
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
|
||
//====初始化驱动器参数
|
||
tmc2160_data_init();
|
||
|
||
TMC2160_VERT_CSN_DISABLE;
|
||
TMC2160_VERT_DISABLE;
|
||
|
||
//====上电清除全局状态标志
|
||
tmc2160_write_register(TMC2160_VERT, TMC2160_GSTAT, 0);
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
|
||
//====寄存器通用配置
|
||
//垂直
|
||
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
|
||
|
||
}
|
||
|
||
|
||
///===== 初始化驱动器 =====
|
||
///驱动器重新上电后自动复位，全部置零
|
||
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;
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 100u);
|
||
|
||
//====初始化驱动器参数
|
||
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] = 0x0c;//3c;//0x24;//0x24;//28,08
|
||
buff[3] = 0x04;//04
|
||
#endif
|
||
|
||
#ifdef PTZ_MEDIUM_WORM_STEP_TMC2160_AS5047D_24V
|
||
buff[2] = 0x38;//0x24;//38,
|
||
buff[3] = 0x02;//0x04;//02
|
||
#endif
|
||
buff[4] = 0x00;//0x10;//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] = 0x06;//0x04;//电流降延时，电机停止后延时断电，可防止电机因惯性多转
|
||
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] = 0x00;
|
||
buff[4] = 0xf4;
|
||
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;//C3
|
||
#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] = 0x0d;//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;//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 |