1525 lines
48 KiB
C
1525 lines
48 KiB
C
#include "ptz_header_file.h"
|
||
#include "service_statusmonitor.h"
|
||
#include "agent_hyt.h"
|
||
#include "service_selfcheck.h"
|
||
|
||
|
||
extern void ptz_uart_dev_send(device_handle device, void *data, int len);
|
||
|
||
static char max_temperature_fault;
|
||
//温度、电压、电流故障检测
|
||
static char ptz_temp_volt_current_fault_detect_task()
|
||
{//只报故障，不做响应
|
||
static unsigned short int time_ms;
|
||
|
||
time_ms ++;
|
||
if(time_ms < 1000)
|
||
{
|
||
return 1;
|
||
}
|
||
else
|
||
{
|
||
time_ms = 0;
|
||
}
|
||
|
||
//一般温度故障，用于故障回传
|
||
if(g_ptz.temperature > PTZ_TEMP_DETECT_MAX || g_ptz.temperature < PTZ_TEMP_DETECT_MIN )
|
||
{
|
||
g_ptz.fault_detect.temperature_fault = FAULT;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.fault_detect.temperature_fault = NO_FAULT;
|
||
}
|
||
//极限温度故障，用于限制云台功能控制，超过温度阈值10℃有效，此时云台不能正常工作
|
||
if(g_ptz.temperature > (PTZ_TEMP_DETECT_MAX + 10.0) || g_ptz.temperature < (PTZ_TEMP_DETECT_MIN - 10.0) )
|
||
{
|
||
max_temperature_fault = FAULT;
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
}
|
||
else
|
||
{
|
||
max_temperature_fault = NO_FAULT;
|
||
}
|
||
|
||
if(g_ptz.Voltage >= PTZ_VOLT_DETECT_MAX )//|| g_ptz.Voltage <= PTZ_VOLT_DETECT_MIN //防止断电故障回传
|
||
{
|
||
g_ptz.fault_detect.voltage_fault = FAULT;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.fault_detect.voltage_fault = NO_FAULT;
|
||
}
|
||
|
||
if(g_ptz.electric_current >= PTZ_CURRENT_DETECT)
|
||
{
|
||
g_ptz.fault_detect.electric_current_fault = FAULT;//如果电流故障应该停止电机转动
|
||
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
}
|
||
else
|
||
{
|
||
g_ptz.fault_detect.electric_current_fault = NO_FAULT;
|
||
}
|
||
return 1;
|
||
}
|
||
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
static char ptz_vert_sw12_fault_detect_task()//水平光电开关故障监测
|
||
{//故障停止垂直电机，不回传
|
||
static unsigned char num;
|
||
if(g_ptz.vert_ps_sw1_state == PS_COVER && g_ptz.vert_ps_sw2_state == PS_COVER)//如果SW1和SW2同时被遮挡
|
||
{
|
||
num ++;
|
||
}
|
||
else
|
||
{
|
||
num = 0;
|
||
g_ptz.fault_detect.vert_sw1_fault = NO_FAULT;
|
||
g_ptz.fault_detect.vert_sw2_fault = NO_FAULT;
|
||
}
|
||
|
||
if(num >= 200)//防止num无限制增加
|
||
{
|
||
num = 100;
|
||
}
|
||
|
||
if(num >= 50)
|
||
{
|
||
g_ptz.fault_detect.vert_sw1_fault = FAULT;
|
||
g_ptz.fault_detect.vert_sw2_fault = FAULT;
|
||
|
||
}
|
||
|
||
if(g_ptz.fault_detect.vert_sw1_fault == FAULT || g_ptz.fault_detect.vert_sw2_fault == FAULT)
|
||
{
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
|
||
{
|
||
ptz_vert_stop(0);
|
||
}
|
||
}
|
||
return 0;
|
||
}
|
||
#endif
|
||
|
||
#ifdef PTZ_BLDC_MOTOR
|
||
//电机霍尔故障检测
|
||
static void ptz_motor_hall_fault_detect_task()
|
||
{
|
||
static unsigned char time_count;
|
||
|
||
|
||
static unsigned short int time_ms;
|
||
|
||
time_ms ++;
|
||
if(time_ms < 1000)
|
||
{
|
||
return;
|
||
}
|
||
else
|
||
{
|
||
time_ms = 0;
|
||
}
|
||
|
||
|
||
time_count ++;
|
||
if(time_count >= 5)
|
||
{
|
||
if(abs(g_ptz.hori_l6235d.l6235d_hall_h1_count - g_ptz.hori_l6235d.l6235d_hall_h2_count) >= PTZ_HALL_DETECT
|
||
||abs(g_ptz.hori_l6235d.l6235d_hall_h1_count - g_ptz.hori_l6235d.l6235d_hall_h3_count) >= PTZ_HALL_DETECT
|
||
||abs(g_ptz.hori_l6235d.l6235d_hall_h2_count - g_ptz.hori_l6235d.l6235d_hall_h3_count) >= PTZ_HALL_DETECT)
|
||
{
|
||
g_ptz.fault_detect.hori_hall_fault = FAULT;
|
||
ptz_hori_stop(0);
|
||
}
|
||
else
|
||
{
|
||
g_ptz.fault_detect.hori_hall_fault = NO_FAULT;
|
||
}
|
||
|
||
if(abs(g_ptz.vert_l6235d.l6235d_hall_h1_count - g_ptz.vert_l6235d.l6235d_hall_h2_count) >= PTZ_HALL_DETECT
|
||
||abs(g_ptz.vert_l6235d.l6235d_hall_h1_count - g_ptz.vert_l6235d.l6235d_hall_h3_count) >= PTZ_HALL_DETECT
|
||
||abs(g_ptz.vert_l6235d.l6235d_hall_h2_count - g_ptz.vert_l6235d.l6235d_hall_h3_count) >= PTZ_HALL_DETECT)
|
||
{
|
||
g_ptz.fault_detect.vert_hall_fault = FAULT;
|
||
ptz_vert_stop(0);
|
||
}
|
||
else
|
||
{
|
||
g_ptz.fault_detect.vert_hall_fault = NO_FAULT;
|
||
}
|
||
|
||
// g_ptz.hori_l6235d.l6235d_hall_h1_count = 0;
|
||
// g_ptz.hori_l6235d.l6235d_hall_h2_count = 0;
|
||
// g_ptz.hori_l6235d.l6235d_hall_h3_count = 0;
|
||
// //g_ptz.hori_l6235d.l6235d_hall_h123_count = 0;
|
||
//
|
||
// g_ptz.vert_l6235d.l6235d_hall_h1_count = 0;
|
||
// g_ptz.vert_l6235d.l6235d_hall_h2_count = 0;
|
||
// g_ptz.vert_l6235d.l6235d_hall_h3_count = 0;
|
||
// //g_ptz.vert_l6235d.l6235d_hall_h123_count = 0;
|
||
|
||
time_count = 0;
|
||
}
|
||
//防止数据超限溢出
|
||
if(g_ptz.hori_l6235d.l6235d_hall_h1_count >= UNSIGNED_INT_MAX_VALUE)
|
||
{
|
||
g_ptz.hori_l6235d.l6235d_hall_h1_count = 0;
|
||
}
|
||
if(g_ptz.hori_l6235d.l6235d_hall_h2_count >= UNSIGNED_INT_MAX_VALUE)
|
||
{
|
||
g_ptz.hori_l6235d.l6235d_hall_h2_count = 0;
|
||
}
|
||
if(g_ptz.hori_l6235d.l6235d_hall_h3_count >= UNSIGNED_INT_MAX_VALUE)
|
||
{
|
||
g_ptz.hori_l6235d.l6235d_hall_h3_count = 0;
|
||
}
|
||
|
||
|
||
if(g_ptz.vert_l6235d.l6235d_hall_h1_count >= UNSIGNED_INT_MAX_VALUE)
|
||
{
|
||
g_ptz.vert_l6235d.l6235d_hall_h1_count = 0;
|
||
}
|
||
if(g_ptz.vert_l6235d.l6235d_hall_h2_count >= UNSIGNED_INT_MAX_VALUE)
|
||
{
|
||
g_ptz.vert_l6235d.l6235d_hall_h2_count = 0;
|
||
}
|
||
if(g_ptz.vert_l6235d.l6235d_hall_h3_count >= UNSIGNED_INT_MAX_VALUE)
|
||
{
|
||
g_ptz.vert_l6235d.l6235d_hall_h3_count = 0;
|
||
}
|
||
|
||
}
|
||
|
||
|
||
//电机转动卡死故障检测
|
||
static char ptz_motor_rotate_fault_detect_task()
|
||
{
|
||
static char hori_rotate_fault_num;
|
||
static char vert_rotate_fault_num;
|
||
|
||
static char hori_rotate_fault_step;
|
||
static float hori_angle_a;
|
||
static float hori_angle_b;
|
||
|
||
static char vert_rotate_fault_step;
|
||
static float vert_angle_a;
|
||
static float vert_angle_b;
|
||
static unsigned short int time_ms;
|
||
|
||
time_ms ++;
|
||
//垂直
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
|
||
{
|
||
vert_rotate_fault_step = 0;//关闭判断
|
||
vert_rotate_fault_num = 0;
|
||
}
|
||
//水平
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
|
||
{
|
||
hori_rotate_fault_step = 0;//关闭判断
|
||
hori_rotate_fault_num = 0;
|
||
}
|
||
|
||
if(time_ms < 1000)
|
||
{
|
||
return 1;
|
||
}
|
||
else
|
||
{
|
||
time_ms = 0;
|
||
}
|
||
|
||
|
||
|
||
//垂直
|
||
switch(vert_rotate_fault_step)
|
||
{
|
||
case 0:
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
|
||
{
|
||
vert_rotate_fault_num = 0;
|
||
vert_angle_a = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
|
||
vert_rotate_fault_step = vert_rotate_fault_step + 1; //启动判断
|
||
}
|
||
break;
|
||
|
||
case 1://开始判断云台转动是否异常
|
||
vert_angle_b = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
|
||
{
|
||
//转动正常情况
|
||
if(vert_angle_b > vert_angle_a &&
|
||
vert_angle_b - vert_angle_a >= PTZ_VERT_MIN_SPEED * PTZ_VERT_BIG_GEAR_RATIO * 2)
|
||
{//只有有一次转动正常，说明转动就是正常的
|
||
vert_rotate_fault_num = 0;
|
||
//电机转动没有故障
|
||
g_ptz.fault_detect.vert_rotate_fault = NO_FAULT;
|
||
}
|
||
else//转动异常情况
|
||
{
|
||
if(g_ptz.vert_pid.PidUT_float >= PTZ_VERT_VOLTAGE_LEVEL)
|
||
{
|
||
vert_rotate_fault_num ++;
|
||
}
|
||
}
|
||
}
|
||
else//g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN
|
||
{
|
||
//转动正常情况
|
||
if(vert_angle_a > vert_angle_b &&
|
||
vert_angle_a - vert_angle_b >= PTZ_VERT_MIN_SPEED * PTZ_VERT_BIG_GEAR_RATIO * 2)
|
||
{//只有有一次转动正常，说明转动就是正常的
|
||
vert_rotate_fault_num = 0;
|
||
//电机转动没有故障
|
||
g_ptz.fault_detect.vert_rotate_fault = NO_FAULT;
|
||
}
|
||
else//转动异常情况
|
||
{
|
||
if(g_ptz.vert_pid.PidUT_float >= PTZ_VERT_VOLTAGE_LEVEL)
|
||
{
|
||
vert_rotate_fault_num ++;
|
||
}
|
||
}
|
||
}
|
||
|
||
if(vert_rotate_fault_num >= ROTATE_FAULT_MAX_NUM)//连续转动异常，则转动异常
|
||
{
|
||
vert_rotate_fault_num = 0;
|
||
//关闭所有云台自动扫描模式
|
||
g_preset_bit_scan.state = PRESET_BIT_SCAN_CLOSE_A;
|
||
g_area_scan.state = AREA_SCAN_CLOSE_A;
|
||
//云台直接刹车
|
||
ptz_hori_stop(0);
|
||
ptz_vert_stop(0);
|
||
|
||
//电机转动故障
|
||
g_ptz.fault_detect.vert_rotate_fault = FAULT;
|
||
}
|
||
|
||
vert_angle_a = vert_angle_b;
|
||
break;
|
||
}
|
||
|
||
/*************************************************************/
|
||
|
||
switch(hori_rotate_fault_step)
|
||
{
|
||
case 0:
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
||
{
|
||
hori_rotate_fault_num = 0;
|
||
hori_angle_a = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
|
||
hori_rotate_fault_step = hori_rotate_fault_step + 1; //启动判断
|
||
}
|
||
break;
|
||
|
||
case 1://开始判断云台转动是否异常
|
||
hori_angle_b = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
|
||
|
||
if(g_ptz.hori_self_check == PTZ_HORI_SELF_CHECK_END)
|
||
{//如果水平自检是完成了的
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
//转动正常情况
|
||
if((hori_angle_b > hori_angle_a && //转动距离要大于最小速度转动距离0.5倍,要小于最大转速转动距离的2倍
|
||
hori_angle_b - hori_angle_a > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 &&
|
||
hori_angle_b - hori_angle_a < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12)
|
||
||
|
||
(hori_angle_a > hori_angle_b &&
|
||
hori_angle_b + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_a > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 &&
|
||
hori_angle_b + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_a < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12) )
|
||
{//只有有一次转动正常，说明转动就是正常的
|
||
hori_rotate_fault_num = 0;
|
||
//电机转动没有故障
|
||
g_ptz.fault_detect.hori_rotate_fault = NO_FAULT;
|
||
}
|
||
else//转动异常情况
|
||
{
|
||
if(g_ptz.hori_pid.PidUT_float >= PTZ_HORI_VOLTAGE_LEVEL)
|
||
{
|
||
hori_rotate_fault_num ++;
|
||
}
|
||
}
|
||
}
|
||
else//g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT
|
||
{
|
||
//转动正常情况
|
||
if((hori_angle_b < hori_angle_a && //转动距离要大于最小速度转动距离0.5倍,要小于最大转速转动距离的2倍
|
||
hori_angle_a - hori_angle_b > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 &&
|
||
hori_angle_a - hori_angle_b < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12)
|
||
||
|
||
(hori_angle_a < hori_angle_b &&
|
||
hori_angle_a + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_b > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 &&
|
||
hori_angle_a + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_b < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12) )
|
||
{//只有有一次转动正常，说明转动就是正常的
|
||
hori_rotate_fault_num = 0;
|
||
//电机转动没有故障
|
||
g_ptz.fault_detect.hori_rotate_fault = NO_FAULT;
|
||
}
|
||
else//转动异常情况
|
||
{
|
||
if(g_ptz.hori_pid.PidUT_float >= PTZ_HORI_VOLTAGE_LEVEL)
|
||
{
|
||
hori_rotate_fault_num ++;
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
}
|
||
else
|
||
{//如果水平自检没完成
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
//转动正常情况
|
||
if(hori_angle_b > hori_angle_a &&
|
||
hori_angle_b - hori_angle_a >= PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2)
|
||
{//只有有一次转动正常，说明转动就是正常的
|
||
hori_rotate_fault_num = 0;
|
||
//电机转动没有故障
|
||
g_ptz.fault_detect.hori_rotate_fault = NO_FAULT;
|
||
}
|
||
else//转动异常情况
|
||
{
|
||
if(g_ptz.hori_pid.PidUT_float >= PTZ_HORI_VOLTAGE_LEVEL)
|
||
{
|
||
hori_rotate_fault_num ++;
|
||
}
|
||
}
|
||
}
|
||
else//g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT
|
||
{
|
||
//转动正常情况
|
||
if(hori_angle_a > hori_angle_b &&
|
||
hori_angle_a - hori_angle_b >= PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2)
|
||
{//只有有一次转动正常，说明转动就是正常的
|
||
hori_rotate_fault_num = 0;
|
||
//电机转动没有故障
|
||
g_ptz.fault_detect.hori_rotate_fault = NO_FAULT;
|
||
}
|
||
else//转动异常情况
|
||
{
|
||
if(g_ptz.hori_pid.PidUT_float >= PTZ_HORI_VOLTAGE_LEVEL)
|
||
{
|
||
hori_rotate_fault_num ++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
if(hori_rotate_fault_num >= ROTATE_FAULT_MAX_NUM)//连续转动异常，则转动异常
|
||
{
|
||
hori_rotate_fault_num = 0;
|
||
//关闭所有云台自动扫描模式
|
||
g_preset_bit_scan.state = PRESET_BIT_SCAN_CLOSE_A;
|
||
g_area_scan.state = AREA_SCAN_CLOSE_A;
|
||
//云台直接刹车
|
||
ptz_hori_stop(0);
|
||
ptz_vert_stop(0);
|
||
|
||
//电机转动故障
|
||
g_ptz.fault_detect.hori_rotate_fault = FAULT;
|
||
}
|
||
|
||
hori_angle_a = hori_angle_b;
|
||
break;
|
||
}
|
||
|
||
|
||
// if(g_ptz.fault_detect.hori_rotate_fault == FAULT ||
|
||
// g_ptz.fault_detect.vert_rotate_fault == FAULT)
|
||
// {
|
||
// g_ptz.fault_detect.fault = FAULT;
|
||
// }
|
||
// else
|
||
// {
|
||
// g_ptz.fault_detect.fault = NO_FAULT;
|
||
// }
|
||
return 1;
|
||
}
|
||
|
||
|
||
#endif
|
||
|
||
#ifdef PTZ_STEP_MOTOR
|
||
//电机转动卡死故障检测
|
||
static char ptz_motor_rotate_fault_detect_task()
|
||
{
|
||
static char hori_rotate_fault_num;
|
||
static char vert_rotate_fault_num;
|
||
|
||
static char hori_rotate_fault_step;
|
||
static float hori_angle_a;
|
||
static float hori_angle_b;
|
||
|
||
static char vert_rotate_fault_step;
|
||
static float vert_angle_a;
|
||
static float vert_angle_b;
|
||
|
||
static unsigned short int time_ms;
|
||
|
||
time_ms ++;
|
||
if(time_ms < 1000)
|
||
{
|
||
return 1;
|
||
}
|
||
else
|
||
{
|
||
time_ms = 0;
|
||
}
|
||
|
||
|
||
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
|
||
{
|
||
vert_rotate_fault_step = 0;//关闭判断
|
||
vert_rotate_fault_num = 0;
|
||
}
|
||
|
||
//垂直
|
||
switch(vert_rotate_fault_step)
|
||
{
|
||
case 0:
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
|
||
{
|
||
vert_rotate_fault_num = 0;
|
||
vert_angle_a = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
|
||
vert_rotate_fault_step = vert_rotate_fault_step + 1; //启动判断
|
||
}
|
||
break;
|
||
|
||
case 1://开始判断云台转动是否异常
|
||
vert_angle_b = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
|
||
{
|
||
//转动正常情况
|
||
if(vert_angle_b > vert_angle_a &&
|
||
vert_angle_b - vert_angle_a >= PTZ_VERT_MIN_SPEED * PTZ_VERT_BIG_GEAR_RATIO * 2)
|
||
{//只有有一次转动正常，说明转动就是正常的
|
||
vert_rotate_fault_num = 0;
|
||
//电机转动没有故障
|
||
g_ptz.fault_detect.vert_rotate_fault = NO_FAULT;
|
||
}
|
||
else//转动异常情况
|
||
{
|
||
vert_rotate_fault_num ++;
|
||
}
|
||
}
|
||
else//g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN
|
||
{
|
||
//转动正常情况
|
||
if(vert_angle_a > vert_angle_b &&
|
||
vert_angle_a - vert_angle_b >= PTZ_VERT_MIN_SPEED * PTZ_VERT_BIG_GEAR_RATIO * 2)
|
||
{//只有有一次转动正常，说明转动就是正常的
|
||
vert_rotate_fault_num = 0;
|
||
//电机转动没有故障
|
||
g_ptz.fault_detect.vert_rotate_fault = NO_FAULT;
|
||
}
|
||
else//转动异常情况
|
||
{
|
||
vert_rotate_fault_num ++;
|
||
}
|
||
}
|
||
|
||
if(vert_rotate_fault_num >= ROTATE_FAULT_MAX_NUM)//连续转动异常，则转动异常
|
||
{
|
||
vert_rotate_fault_num = 0;
|
||
//关闭所有云台自动扫描模式
|
||
g_preset_bit_scan.state = PRESET_BIT_SCAN_CLOSE_A;
|
||
g_area_scan.state = AREA_SCAN_CLOSE_A;
|
||
//云台直接刹车
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
|
||
//电机转动故障
|
||
g_ptz.fault_detect.vert_rotate_fault = FAULT;
|
||
}
|
||
|
||
vert_angle_a = vert_angle_b;
|
||
break;
|
||
}
|
||
|
||
/*************************************************************/
|
||
|
||
//水平
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
|
||
{
|
||
hori_rotate_fault_step = 0;//关闭判断
|
||
hori_rotate_fault_num = 0;
|
||
}
|
||
|
||
|
||
switch(hori_rotate_fault_step)
|
||
{
|
||
case 0:
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
||
{
|
||
hori_rotate_fault_num = 0;
|
||
hori_angle_a = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
|
||
hori_rotate_fault_step = hori_rotate_fault_step + 1; //启动判断
|
||
}
|
||
break;
|
||
|
||
case 1://开始判断云台转动是否异常
|
||
hori_angle_b = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
|
||
|
||
if(g_ptz.hori_self_check == PTZ_HORI_SELF_CHECK_END)
|
||
{//如果水平自检是完成了的
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
//转动正常情况
|
||
if((hori_angle_b > hori_angle_a && //转动距离要大于最小速度转动距离0.5倍,要小于最大转速转动距离的2倍
|
||
hori_angle_b - hori_angle_a > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 &&
|
||
hori_angle_b - hori_angle_a < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12)
|
||
||
|
||
(hori_angle_a > hori_angle_b &&
|
||
hori_angle_b + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_a > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 &&
|
||
hori_angle_b + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_a < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12) )
|
||
{//只有有一次转动正常，说明转动就是正常的
|
||
hori_rotate_fault_num = 0;
|
||
//电机转动没有故障
|
||
g_ptz.fault_detect.hori_rotate_fault = NO_FAULT;
|
||
}
|
||
else//转动异常情况
|
||
{
|
||
hori_rotate_fault_num ++;
|
||
}
|
||
}
|
||
else//g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT
|
||
{
|
||
//转动正常情况
|
||
if((hori_angle_b < hori_angle_a && //转动距离要大于最小速度转动距离0.5倍,要小于最大转速转动距离的2倍
|
||
hori_angle_a - hori_angle_b > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 &&
|
||
hori_angle_a - hori_angle_b < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12)
|
||
||
|
||
(hori_angle_a < hori_angle_b &&
|
||
hori_angle_a + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_b > PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2 &&
|
||
hori_angle_a + g_ptz.hori_as5047d.as5047d_ptz_angle_max - hori_angle_b < PTZ_HORI_MAX_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 12) )
|
||
{//只有有一次转动正常，说明转动就是正常的
|
||
hori_rotate_fault_num = 0;
|
||
//电机转动没有故障
|
||
g_ptz.fault_detect.hori_rotate_fault = NO_FAULT;
|
||
}
|
||
else//转动异常情况
|
||
{
|
||
hori_rotate_fault_num ++;
|
||
}
|
||
|
||
}
|
||
|
||
}
|
||
else
|
||
{//如果水平自检没完成
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
//转动正常情况
|
||
if(hori_angle_b > hori_angle_a &&
|
||
hori_angle_b - hori_angle_a >= PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2)
|
||
{//只有有一次转动正常，说明转动就是正常的
|
||
hori_rotate_fault_num = 0;
|
||
//电机转动没有故障
|
||
g_ptz.fault_detect.hori_rotate_fault = NO_FAULT;
|
||
}
|
||
else//转动异常情况
|
||
{
|
||
hori_rotate_fault_num ++;
|
||
}
|
||
}
|
||
else//g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT
|
||
{
|
||
//转动正常情况
|
||
if(hori_angle_a > hori_angle_b &&
|
||
hori_angle_a - hori_angle_b >= PTZ_HORI_MIN_SPEED * PTZ_HORI_BIG_GEAR_RATIO * 2)
|
||
{//只有有一次转动正常，说明转动就是正常的
|
||
hori_rotate_fault_num = 0;
|
||
//电机转动没有故障
|
||
g_ptz.fault_detect.hori_rotate_fault = NO_FAULT;
|
||
}
|
||
else//转动异常情况
|
||
{
|
||
hori_rotate_fault_num ++;
|
||
}
|
||
}
|
||
}
|
||
|
||
if(hori_rotate_fault_num >= ROTATE_FAULT_MAX_NUM)//连续转动异常，则转动异常
|
||
{
|
||
hori_rotate_fault_num = 0;
|
||
//关闭所有云台自动扫描模式
|
||
g_preset_bit_scan.state = PRESET_BIT_SCAN_CLOSE_A;
|
||
g_area_scan.state = AREA_SCAN_CLOSE_A;
|
||
//云台直接刹车
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
|
||
//电机转动故障
|
||
g_ptz.fault_detect.hori_rotate_fault = FAULT;
|
||
}
|
||
|
||
hori_angle_a = hori_angle_b;
|
||
break;
|
||
}
|
||
|
||
|
||
|
||
// if(g_ptz.fault_detect.hori_rotate_fault == FAULT ||
|
||
// g_ptz.fault_detect.vert_rotate_fault == FAULT)
|
||
// {
|
||
// g_ptz.fault_detect.fault = FAULT;
|
||
// }
|
||
// else
|
||
// {
|
||
// g_ptz.fault_detect.fault = NO_FAULT;
|
||
// }
|
||
return 1;
|
||
}
|
||
|
||
|
||
#endif
|
||
|
||
|
||
//故障代码自动回传
|
||
static char ptz_fault_return_task()
|
||
{
|
||
|
||
static unsigned short int time_ms;
|
||
|
||
time_ms ++;
|
||
if(time_ms < 1000)
|
||
{
|
||
return 1;
|
||
}
|
||
else
|
||
{
|
||
time_ms = 0;
|
||
}
|
||
//水平转动故障回传
|
||
if(g_ptz.fault_detect.hori_rotate_fault == FAULT &&
|
||
g_ptz.cmd_save.cmd_data_udp_flag == 1)
|
||
{
|
||
ptz_send_fault_state(PTZ_UDP,HORI_ROT);
|
||
}
|
||
//垂直转动故障回传
|
||
if(g_ptz.fault_detect.vert_rotate_fault == FAULT &&
|
||
g_ptz.cmd_save.cmd_data_udp_flag == 1)
|
||
{
|
||
ptz_send_fault_state(PTZ_UDP,VERT_ROT);
|
||
}
|
||
//温度故障回传
|
||
if(g_ptz.fault_detect.temperature_fault == FAULT &&
|
||
g_ptz.cmd_save.cmd_data_udp_flag == 1)
|
||
{
|
||
ptz_send_fault_state(PTZ_UDP,WORK_TEMP);
|
||
}
|
||
//电压故障回传
|
||
if(g_ptz.fault_detect.voltage_fault == FAULT &&
|
||
g_ptz.cmd_save.cmd_data_udp_flag == 1)
|
||
{
|
||
ptz_send_fault_state(PTZ_UDP,WORK_VOLT);
|
||
}
|
||
//工作电流故障回传
|
||
if(g_ptz.fault_detect.electric_current_fault == FAULT
|
||
&& g_ptz.cmd_save.cmd_data_udp_flag == 1)
|
||
{
|
||
ptz_send_fault_state(PTZ_UDP,WORK_ELECTRIC_CURRENT);
|
||
}
|
||
//水平hall故障回传
|
||
if(g_ptz.fault_detect.hori_hall_fault == FAULT &&
|
||
g_ptz.cmd_save.cmd_data_udp_flag == 1)
|
||
{
|
||
ptz_send_fault_state(PTZ_UDP,HORI_HALL);
|
||
}
|
||
//垂直hall故障回传
|
||
if(g_ptz.fault_detect.vert_hall_fault == FAULT &&
|
||
g_ptz.cmd_save.cmd_data_udp_flag == 1)
|
||
{
|
||
ptz_send_fault_state(PTZ_UDP,VERT_HALL);
|
||
}
|
||
return 1;
|
||
}
|
||
|
||
|
||
|
||
static char ptz_fault_detect_task()
|
||
{
|
||
while(1)
|
||
{
|
||
//电机转动卡死故障
|
||
ptz_motor_rotate_fault_detect_task();
|
||
//温度电压电流故障
|
||
ptz_temp_volt_current_fault_detect_task();
|
||
//电机hall故障
|
||
#ifdef PTZ_BLDC_MOTOR
|
||
ptz_motor_hall_fault_detect_task();
|
||
#endif
|
||
//故障回传
|
||
ptz_fault_return_task();
|
||
//光电开关故障，，光电开关不能故障回传，因为断电时会检测到故障
|
||
ptz_vert_sw12_fault_detect_task();
|
||
|
||
|
||
if(g_ptz.fault_detect.hori_rotate_fault == FAULT ||
|
||
g_ptz.fault_detect.vert_rotate_fault == FAULT ||
|
||
max_temperature_fault == FAULT)//温度超过阈值10℃触发总故障
|
||
{//总故障
|
||
g_ptz.fault_detect.fault = FAULT;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.fault_detect.fault = NO_FAULT;
|
||
}
|
||
|
||
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
||
}
|
||
}
|
||
|
||
|
||
//#define TASK_FAULT_DETECT_PRIO 19u
|
||
//#define TASK_FAULT_DETECT_STK_SIZE 100u
|
||
|
||
static OS_STK task_fault_detect_stk[TASK_FAULT_DETECT_STK_SIZE];
|
||
static void creat_task_fault_detect(void)
|
||
{
|
||
CPU_INT08U task_err;
|
||
CPU_INT08U name_err;
|
||
|
||
task_err = OSTaskCreateExt((void (*)(void *)) ptz_fault_detect_task,
|
||
(void *) 0,
|
||
(OS_STK *)&task_fault_detect_stk[TASK_FAULT_DETECT_STK_SIZE - 1],
|
||
(INT8U ) TASK_FAULT_DETECT_PRIO,
|
||
(INT16U ) TASK_FAULT_DETECT_PRIO,
|
||
(OS_STK *)&task_fault_detect_stk[0],
|
||
(INT32U ) TASK_FAULT_DETECT_STK_SIZE,
|
||
(void *) 0,
|
||
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
|
||
#if (OS_TASK_NAME_EN > 0)
|
||
OSTaskNameSet(TASK_FAULT_DETECT_PRIO, "ptz_fault_detect_task", &name_err);
|
||
#endif
|
||
|
||
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
|
||
pdebug(DEBUG_LEVEL_INFO,"create ptz_fault_detect_task success...\n\r");
|
||
} else {
|
||
pdebug(DEBUG_LEVEL_FATAL,"create ptz_fault_detect_task failed...\n\r");
|
||
}
|
||
}
|
||
|
||
void init_status_monitor_module()
|
||
{
|
||
creat_task_fault_detect();
|
||
}
|
||
|
||
|
||
|
||
void ptz_send_fault_state(char dev, unsigned char FaultState)
|
||
{
|
||
unsigned char buff[7];
|
||
unsigned short int buf;
|
||
if(g_ptz.cmd_save.cmd_data_flag == 1)//云台接收过指令，有故障主动回传的目标才能主动回传
|
||
{
|
||
//水平转动状态
|
||
if(FaultState == HORI_ROT || FaultState == ALL_FAULT_STATE)
|
||
{
|
||
memset(buff, 0, sizeof(buff));
|
||
buff[0] = 0xff;
|
||
buff[1] = g_ptz.address;
|
||
buff[2] = HORI_ROT;
|
||
buff[3] = g_ptz.fault_detect.hori_rotate_fault;
|
||
|
||
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
|
||
{//转动方向
|
||
buff[4] = PTZ_RIGHT;
|
||
}
|
||
else
|
||
{
|
||
buff[4] = PTZ_LEFT;
|
||
}
|
||
|
||
buff[5] = g_ptz.hori_start_stop_set;//电机是处于转动状态还是刹车状态
|
||
buff[6] = MotorCalPelcoDSUM(buff, sizeof(buff));
|
||
|
||
switch(dev)
|
||
{
|
||
case PTZ_UDP:
|
||
send_udp_data_aim(buff, sizeof(buff),
|
||
(struct sockaddr*)&g_ptz.cmd_save.cmd_from, g_ptz.cmd_save.cmd_fromlen);
|
||
break;
|
||
|
||
case PTZ_UART_422:
|
||
ptz_uart_dev_send(uart_422_handle, buff, sizeof(buff));
|
||
break;
|
||
|
||
case PTZ_UART_485:
|
||
ptz_uart_dev_send(uart_485_handle, buff, sizeof(buff));
|
||
break;
|
||
}
|
||
|
||
}
|
||
#ifdef PTZ_BLDC_MOTOR
|
||
//水平霍尔状态
|
||
if(FaultState == HORI_HALL || FaultState == ALL_FAULT_STATE)
|
||
{
|
||
memset(buff, 0, sizeof(buff));
|
||
buff[0] = 0xff;
|
||
buff[1] = g_ptz.address;
|
||
buff[2] = HORI_HALL;
|
||
buff[3] = g_ptz.fault_detect.hori_hall_fault;
|
||
buff[6] = MotorCalPelcoDSUM(buff, sizeof(buff));
|
||
|
||
switch(dev)
|
||
{
|
||
case PTZ_UDP:
|
||
send_udp_data_aim(buff, sizeof(buff),
|
||
(struct sockaddr*)&g_ptz.cmd_save.cmd_from, g_ptz.cmd_save.cmd_fromlen);
|
||
break;
|
||
|
||
case PTZ_UART_422:
|
||
ptz_uart_dev_send(uart_422_handle, buff, sizeof(buff));
|
||
break;
|
||
|
||
case PTZ_UART_485:
|
||
ptz_uart_dev_send(uart_485_handle, buff, sizeof(buff));
|
||
break;
|
||
}
|
||
|
||
}
|
||
#endif
|
||
//垂直转动状态
|
||
if(FaultState == VERT_ROT || FaultState == ALL_FAULT_STATE)
|
||
{
|
||
memset(buff, 0, sizeof(buff));
|
||
buff[0] = 0xff;
|
||
buff[1] = g_ptz.address;
|
||
buff[2] = VERT_ROT;
|
||
buff[3] = g_ptz.fault_detect.vert_rotate_fault;
|
||
|
||
if(g_ptz.vert_direction_set == PTZ_VERT_DIR_UP)
|
||
{//转动方向
|
||
buff[4] = PTZ_UP;
|
||
}
|
||
else
|
||
{
|
||
buff[4] = PTZ_DOWN;
|
||
}
|
||
|
||
buff[5] = g_ptz.vert_start_stop_set;//电机是处于转动状态还是刹车状态
|
||
buff[6] = MotorCalPelcoDSUM(buff, sizeof(buff));
|
||
|
||
switch(dev)
|
||
{
|
||
case PTZ_UDP:
|
||
send_udp_data_aim(buff, sizeof(buff),
|
||
(struct sockaddr*)&g_ptz.cmd_save.cmd_from, g_ptz.cmd_save.cmd_fromlen);
|
||
break;
|
||
|
||
case PTZ_UART_422:
|
||
ptz_uart_dev_send(uart_422_handle, buff, sizeof(buff));
|
||
break;
|
||
|
||
case PTZ_UART_485:
|
||
ptz_uart_dev_send(uart_485_handle, buff, sizeof(buff));
|
||
break;
|
||
}
|
||
|
||
}
|
||
|
||
#ifdef PTZ_BLDC_MOTOR
|
||
//垂直霍尔状态
|
||
if(FaultState == VERT_HALL || FaultState == ALL_FAULT_STATE)
|
||
{
|
||
memset(buff, 0, sizeof(buff));
|
||
buff[0] = 0xff;
|
||
buff[1] = g_ptz.address;
|
||
buff[2] = VERT_HALL;
|
||
buff[3] = g_ptz.fault_detect.vert_hall_fault;
|
||
buff[6] = MotorCalPelcoDSUM(buff, sizeof(buff));
|
||
|
||
switch(dev)
|
||
{
|
||
case PTZ_UDP:
|
||
send_udp_data_aim(buff, sizeof(buff),
|
||
(struct sockaddr*)&g_ptz.cmd_save.cmd_from, g_ptz.cmd_save.cmd_fromlen);
|
||
break;
|
||
|
||
case PTZ_UART_422:
|
||
ptz_uart_dev_send(uart_422_handle, buff, sizeof(buff));
|
||
break;
|
||
|
||
case PTZ_UART_485:
|
||
ptz_uart_dev_send(uart_485_handle, buff, sizeof(buff));
|
||
break;
|
||
}
|
||
|
||
}
|
||
#endif
|
||
//当前工作温度WorkTemp
|
||
if(FaultState == WORK_TEMP || FaultState == ALL_FAULT_STATE)
|
||
{
|
||
memset(buff, 0, sizeof(buff));
|
||
buf = (unsigned short int)(g_ptz.temperature * 100 + 0.5);
|
||
buff[0] = 0xff;
|
||
buff[1] = g_ptz.address;
|
||
buff[2] = WORK_TEMP;
|
||
buff[3] = g_ptz.fault_detect.temperature_fault;
|
||
buff[4] = (unsigned char)(buf >> 8);
|
||
buff[5] = (unsigned char)(buf & 0x00ff);
|
||
buff[6] = MotorCalPelcoDSUM(buff, sizeof(buff));
|
||
|
||
switch(dev)
|
||
{
|
||
case PTZ_UDP:
|
||
send_udp_data_aim(buff, sizeof(buff),
|
||
(struct sockaddr*)&g_ptz.cmd_save.cmd_from, g_ptz.cmd_save.cmd_fromlen);
|
||
break;
|
||
|
||
case PTZ_UART_422:
|
||
ptz_uart_dev_send(uart_422_handle, buff, sizeof(buff));
|
||
break;
|
||
|
||
case PTZ_UART_485:
|
||
ptz_uart_dev_send(uart_485_handle, buff, sizeof(buff));
|
||
break;
|
||
}
|
||
}
|
||
|
||
//当前工作电压
|
||
if(FaultState == WORK_VOLT || FaultState == ALL_FAULT_STATE)
|
||
{
|
||
memset(buff, 0, sizeof(buff));
|
||
buf = (unsigned short int)(g_ptz.Voltage * 100 + 0.5);
|
||
buff[0] = 0xff;
|
||
buff[1] = g_ptz.address;
|
||
buff[2] = WORK_VOLT;
|
||
buff[3] = g_ptz.fault_detect.voltage_fault;
|
||
buff[4] = (unsigned char)(buf >> 8);
|
||
buff[5] = (unsigned char)(buf & 0x00ff);
|
||
buff[6] = MotorCalPelcoDSUM(buff, sizeof(buff));
|
||
|
||
switch(dev)
|
||
{
|
||
case PTZ_UDP:
|
||
send_udp_data_aim(buff, sizeof(buff),
|
||
(struct sockaddr*)&g_ptz.cmd_save.cmd_from, g_ptz.cmd_save.cmd_fromlen);
|
||
break;
|
||
|
||
case PTZ_UART_422:
|
||
ptz_uart_dev_send(uart_422_handle, buff, sizeof(buff));
|
||
break;
|
||
|
||
case PTZ_UART_485:
|
||
ptz_uart_dev_send(uart_485_handle, buff, sizeof(buff));
|
||
break;
|
||
}
|
||
}
|
||
|
||
//当前工作电流
|
||
if(FaultState == WORK_ELECTRIC_CURRENT || FaultState == ALL_FAULT_STATE)
|
||
{
|
||
memset(buff, 0, sizeof(buff));
|
||
buf = (unsigned short int)(g_ptz.electric_current * 100 + 0.5);
|
||
buff[0] = 0xff;
|
||
buff[1] = g_ptz.address;
|
||
buff[2] = WORK_ELECTRIC_CURRENT;
|
||
buff[3] = g_ptz.fault_detect.electric_current_fault;
|
||
buff[4] = (unsigned char)(buf >> 8);
|
||
buff[5] = (unsigned char)(buf & 0x00ff);
|
||
buff[6] = MotorCalPelcoDSUM(buff, sizeof(buff));
|
||
|
||
switch(dev)
|
||
{
|
||
case PTZ_UDP:
|
||
send_udp_data_aim(buff, sizeof(buff),
|
||
(struct sockaddr*)&g_ptz.cmd_save.cmd_from, g_ptz.cmd_save.cmd_fromlen);
|
||
break;
|
||
|
||
case PTZ_UART_422:
|
||
ptz_uart_dev_send(uart_422_handle, buff, sizeof(buff));
|
||
break;
|
||
|
||
case PTZ_UART_485:
|
||
ptz_uart_dev_send(uart_485_handle, buff, sizeof(buff));
|
||
break;
|
||
}
|
||
|
||
}
|
||
|
||
//当前辅助开关状态
|
||
if(FaultState == CAMERA_POWER || FaultState == ALL_FAULT_STATE)
|
||
{
|
||
memset(buff, 0, sizeof(buff));
|
||
buff[0] = 0xff;
|
||
buff[1] = g_ptz.address;
|
||
buff[2] = CAMERA_POWER;
|
||
buff[3] = g_ptz.power.aux_switch_3;//可见光相机
|
||
buff[4] = g_ptz.power.aux_switch_4;//红外相机
|
||
buff[6] = MotorCalPelcoDSUM(buff, sizeof(buff));
|
||
|
||
switch(dev)
|
||
{
|
||
case PTZ_UDP:
|
||
send_udp_data_aim(buff, sizeof(buff),
|
||
(struct sockaddr*)&g_ptz.cmd_save.cmd_from, g_ptz.cmd_save.cmd_fromlen);
|
||
break;
|
||
|
||
case PTZ_UART_422:
|
||
ptz_uart_dev_send(uart_422_handle, buff, sizeof(buff));
|
||
break;
|
||
|
||
case PTZ_UART_485:
|
||
ptz_uart_dev_send(uart_485_handle, buff, sizeof(buff));
|
||
break;
|
||
}
|
||
|
||
}
|
||
|
||
//垂直光电开关状态
|
||
if(FaultState == VERT_PHOTOELECT_SWITCH || FaultState == ALL_FAULT_STATE)
|
||
{
|
||
memset(buff, 0, sizeof(buff));
|
||
buff[0] = 0xff;
|
||
buff[1] = g_ptz.address;
|
||
buff[2] = VERT_PHOTOELECT_SWITCH;
|
||
buff[4] = g_ptz.fault_detect.vert_sw1_fault;//光电开关1
|
||
buff[5] = g_ptz.fault_detect.vert_sw2_fault;//光电开关2
|
||
buff[6] = MotorCalPelcoDSUM(buff, sizeof(buff));
|
||
|
||
switch(dev)
|
||
{
|
||
case PTZ_UDP:
|
||
send_udp_data_aim(buff, sizeof(buff),
|
||
(struct sockaddr*)&g_ptz.cmd_save.cmd_from, g_ptz.cmd_save.cmd_fromlen);
|
||
break;
|
||
|
||
case PTZ_UART_422:
|
||
ptz_uart_dev_send(uart_422_handle, buff, sizeof(buff));
|
||
break;
|
||
|
||
case PTZ_UART_485:
|
||
ptz_uart_dev_send(uart_485_handle, buff, sizeof(buff));
|
||
break;
|
||
}
|
||
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
|
||
|
||
///指令是否能够执行判断函数
|
||
PtzCmdType ptz_cmd_execute_judge()
|
||
{
|
||
PtzCmdType cmd;
|
||
//云台处于继电器调试状态
|
||
if(g_ptz.ptz_motor_relay_state == PTZ_RELAY_ON)
|
||
{
|
||
cmd.manual_control = CMD_ALLOW;//手动控制，包括上下左右等方向控制
|
||
cmd.manual_stop = CMD_ALLOW;//手动停止指令
|
||
|
||
cmd.preset_bit_set = CMD_NOT_ALLOW;//预置位设置
|
||
cmd.preset_bit_del = CMD_NOT_ALLOW;//预置位删除
|
||
cmd.preset_bit_call = CMD_NOT_ALLOW;//预置位调用
|
||
|
||
cmd.preset_bit_scan_start = CMD_NOT_ALLOW;//预置位扫描启动
|
||
cmd.preset_bit_scan_pause = CMD_NOT_ALLOW;//预置位扫描暂停
|
||
cmd.preset_bit_scan_recovery = CMD_NOT_ALLOW;//预置位扫描恢复
|
||
cmd.preset_bit_scan_close = CMD_NOT_ALLOW;//预置位扫描关闭
|
||
|
||
cmd.area_set = CMD_NOT_ALLOW;//区域数据设置
|
||
cmd.area_scan_start = CMD_NOT_ALLOW;//区域扫描启动
|
||
cmd.area_scan_pause = CMD_NOT_ALLOW;//区域扫描暂停
|
||
cmd.area_scan_recovery = CMD_NOT_ALLOW;//区域扫描恢复
|
||
cmd.area_scan_close = CMD_NOT_ALLOW;//区域扫描关闭
|
||
|
||
cmd.set_zero = CMD_NOT_ALLOW;
|
||
return cmd;
|
||
}
|
||
//云台还处于自检状态
|
||
if(g_ptz.hori_self_check != PTZ_HORI_SELF_CHECK_END ||
|
||
g_ptz.vert_self_check != PTZ_VERT_SELF_CHECK_END)
|
||
{
|
||
cmd.manual_control = CMD_NOT_ALLOW;//手动控制，包括上下左右等方向控制
|
||
cmd.manual_stop = CMD_NOT_ALLOW;//手动停止指令
|
||
|
||
cmd.preset_bit_set = CMD_NOT_ALLOW;//预置位设置
|
||
cmd.preset_bit_del = CMD_NOT_ALLOW;//预置位删除
|
||
cmd.preset_bit_call = CMD_NOT_ALLOW;//预置位调用
|
||
|
||
cmd.preset_bit_scan_start = CMD_NOT_ALLOW;//预置位扫描启动
|
||
cmd.preset_bit_scan_pause = CMD_NOT_ALLOW;//预置位扫描暂停
|
||
cmd.preset_bit_scan_recovery = CMD_NOT_ALLOW;//预置位扫描恢复
|
||
cmd.preset_bit_scan_close = CMD_NOT_ALLOW;//预置位扫描关闭
|
||
|
||
cmd.area_set = CMD_NOT_ALLOW;//区域数据设置
|
||
cmd.area_scan_start = CMD_NOT_ALLOW;//区域扫描启动
|
||
cmd.area_scan_pause = CMD_NOT_ALLOW;//区域扫描暂停
|
||
cmd.area_scan_recovery = CMD_NOT_ALLOW;//区域扫描恢复
|
||
cmd.area_scan_close = CMD_NOT_ALLOW;//区域扫描关闭
|
||
|
||
cmd.set_zero = CMD_NOT_ALLOW;
|
||
return cmd;
|
||
}
|
||
//云台处于故障状态
|
||
if(g_ptz.fault_detect.fault == FAULT)
|
||
{
|
||
cmd.manual_control = CMD_NOT_ALLOW;//手动控制，包括上下左右等方向控制
|
||
cmd.manual_stop = CMD_ALLOW;//手动停止指令
|
||
|
||
cmd.preset_bit_set = CMD_NOT_ALLOW;//预置位设置
|
||
cmd.preset_bit_del = CMD_NOT_ALLOW;//预置位删除
|
||
cmd.preset_bit_call = CMD_NOT_ALLOW;//预置位调用
|
||
|
||
cmd.preset_bit_scan_start = CMD_NOT_ALLOW;//预置位扫描启动
|
||
cmd.preset_bit_scan_pause = CMD_NOT_ALLOW;//预置位扫描暂停
|
||
cmd.preset_bit_scan_recovery = CMD_NOT_ALLOW;//预置位扫描恢复
|
||
cmd.preset_bit_scan_close = CMD_NOT_ALLOW;//预置位扫描关闭
|
||
|
||
cmd.area_set = CMD_NOT_ALLOW;//区域数据设置
|
||
cmd.area_scan_start = CMD_NOT_ALLOW;//区域扫描启动
|
||
cmd.area_scan_pause = CMD_NOT_ALLOW;//区域扫描暂停
|
||
cmd.area_scan_recovery = CMD_NOT_ALLOW;//区域扫描恢复
|
||
cmd.area_scan_close = CMD_NOT_ALLOW;//区域扫描关闭
|
||
|
||
cmd.set_zero = CMD_NOT_ALLOW;
|
||
return cmd;
|
||
}
|
||
//云台处于预置位扫描状态
|
||
if(g_preset_bit_scan.state >= PRESET_BIT_SCAN_START &&
|
||
g_preset_bit_scan.state <= PRESET_BIT_SCAN_START + 4)
|
||
{
|
||
cmd.manual_control = CMD_NOT_ALLOW;//手动控制，包括上下左右等方向控制
|
||
cmd.manual_stop = CMD_NOT_ALLOW;//手动停止指令
|
||
|
||
cmd.preset_bit_set = CMD_NOT_ALLOW;//预置位设置
|
||
cmd.preset_bit_del = CMD_NOT_ALLOW;//预置位删除
|
||
cmd.preset_bit_call = CMD_NOT_ALLOW;//预置位调用
|
||
|
||
cmd.preset_bit_scan_start = CMD_ALLOW;//预置位扫描启动
|
||
cmd.preset_bit_scan_pause = CMD_ALLOW;//预置位扫描暂停
|
||
cmd.preset_bit_scan_recovery = CMD_NOT_ALLOW;//预置位扫描恢复
|
||
cmd.preset_bit_scan_close = CMD_ALLOW;//预置位扫描关闭
|
||
|
||
cmd.area_set = CMD_ALLOW;//区域数据设置
|
||
cmd.area_scan_start = CMD_NOT_ALLOW;//区域扫描启动
|
||
cmd.area_scan_pause = CMD_NOT_ALLOW;//区域扫描暂停
|
||
cmd.area_scan_recovery = CMD_NOT_ALLOW;//区域扫描恢复
|
||
cmd.area_scan_close = CMD_NOT_ALLOW;//区域扫描关闭
|
||
|
||
cmd.set_zero = CMD_NOT_ALLOW;
|
||
return cmd;
|
||
}
|
||
//云台处于预置位扫描暂停状态
|
||
if(g_preset_bit_scan.state == PRESET_BIT_SCAN_PAUSE_A ||
|
||
g_preset_bit_scan.state == PRESET_BIT_SCAN_PAUSE_B)
|
||
{
|
||
// if(g_preset_bit_scan.state == PRESET_BIT_SCAN_PAUSE_A)
|
||
// {
|
||
// cmd.manual_control = CMD_NOT_ALLOW;//手动控制，包括上下左右等方向控制
|
||
// cmd.manual_stop = CMD_NOT_ALLOW;//手动停止指令
|
||
//
|
||
// cmd.preset_bit_set = CMD_NOT_ALLOW;//预置位设置
|
||
// cmd.preset_bit_del = CMD_NOT_ALLOW;//预置位删除
|
||
// cmd.preset_bit_call = CMD_NOT_ALLOW;//预置位调用
|
||
//
|
||
// cmd.preset_bit_scan_start = CMD_NOT_ALLOW;//预置位扫描启动
|
||
// cmd.preset_bit_scan_pause = CMD_NOT_ALLOW;//预置位扫描暂停
|
||
// cmd.preset_bit_scan_recovery = CMD_NOT_ALLOW;//预置位扫描恢复
|
||
// cmd.preset_bit_scan_close = CMD_NOT_ALLOW;//预置位扫描关闭
|
||
//
|
||
// cmd.area_set = CMD_ALLOW;//区域数据设置
|
||
// cmd.area_scan_start = CMD_NOT_ALLOW;//区域扫描启动
|
||
// cmd.area_scan_pause = CMD_NOT_ALLOW;//区域扫描暂停
|
||
// cmd.area_scan_recovery = CMD_NOT_ALLOW;//区域扫描恢复
|
||
// cmd.area_scan_close = CMD_NOT_ALLOW;//区域扫描关闭
|
||
//
|
||
// cmd.set_zero = CMD_NOT_ALLOW;
|
||
// }
|
||
//if(g_preset_bit_scan.state == PRESET_BIT_SCAN_PAUSE_B)
|
||
cmd.manual_control = CMD_ALLOW;//手动控制，包括上下左右等方向控制
|
||
cmd.manual_stop = CMD_ALLOW;//手动停止指令
|
||
|
||
cmd.preset_bit_set = CMD_ALLOW;//预置位设置
|
||
cmd.preset_bit_del = CMD_ALLOW;//预置位删除
|
||
cmd.preset_bit_call = CMD_ALLOW;//预置位调用
|
||
|
||
cmd.preset_bit_scan_start = CMD_ALLOW;//预置位扫描启动
|
||
cmd.preset_bit_scan_pause = CMD_NOT_ALLOW;//预置位扫描暂停
|
||
cmd.preset_bit_scan_recovery = CMD_ALLOW;//预置位扫描恢复
|
||
cmd.preset_bit_scan_close = CMD_ALLOW;//预置位扫描关闭
|
||
|
||
cmd.area_set = CMD_ALLOW;//区域数据设置
|
||
cmd.area_scan_start = CMD_NOT_ALLOW;//区域扫描启动
|
||
cmd.area_scan_pause = CMD_NOT_ALLOW;//区域扫描暂停
|
||
cmd.area_scan_recovery = CMD_NOT_ALLOW;//区域扫描恢复
|
||
cmd.area_scan_close = CMD_NOT_ALLOW;//区域扫描关闭
|
||
|
||
cmd.set_zero = CMD_NOT_ALLOW;
|
||
return cmd;
|
||
}
|
||
|
||
if(g_preset_bit_scan.state == PRESET_BIT_SCAN_RECOVERY)//预置位扫描恢复状态中
|
||
{
|
||
cmd.manual_control = CMD_NOT_ALLOW;//手动控制，包括上下左右等方向控制
|
||
cmd.manual_stop = CMD_NOT_ALLOW;//手动停止指令
|
||
|
||
cmd.preset_bit_set = CMD_NOT_ALLOW;//预置位设置
|
||
cmd.preset_bit_del = CMD_NOT_ALLOW;//预置位删除
|
||
cmd.preset_bit_call = CMD_NOT_ALLOW;//预置位调用
|
||
|
||
cmd.preset_bit_scan_start = CMD_ALLOW;//预置位扫描启动
|
||
cmd.preset_bit_scan_pause = CMD_NOT_ALLOW;//预置位扫描暂停
|
||
cmd.preset_bit_scan_recovery = CMD_NOT_ALLOW;//预置位扫描恢复
|
||
cmd.preset_bit_scan_close = CMD_ALLOW;//预置位扫描关闭
|
||
|
||
cmd.area_set = CMD_ALLOW;//区域数据设置
|
||
cmd.area_scan_start = CMD_NOT_ALLOW;//区域扫描启动
|
||
cmd.area_scan_pause = CMD_NOT_ALLOW;//区域扫描暂停
|
||
cmd.area_scan_recovery = CMD_NOT_ALLOW;//区域扫描恢复
|
||
cmd.area_scan_close = CMD_NOT_ALLOW;//区域扫描关闭
|
||
|
||
cmd.set_zero = CMD_NOT_ALLOW;
|
||
return cmd;
|
||
}
|
||
|
||
if(g_area_scan.state >= AREA_SCAN_START && //云台处于区域扫描状态
|
||
g_area_scan.state < AREA_SCAN_PAUSE_A)
|
||
{
|
||
cmd.manual_control = CMD_NOT_ALLOW;//手动控制，包括上下左右等方向控制
|
||
cmd.manual_stop = CMD_NOT_ALLOW;//手动停止指令
|
||
|
||
cmd.preset_bit_set = CMD_ALLOW;//预置位设置
|
||
cmd.preset_bit_del = CMD_ALLOW;//预置位删除
|
||
cmd.preset_bit_call = CMD_NOT_ALLOW;//预置位调用
|
||
|
||
cmd.preset_bit_scan_start = CMD_NOT_ALLOW;//预置位扫描启动
|
||
cmd.preset_bit_scan_pause = CMD_NOT_ALLOW;//预置位扫描暂停
|
||
cmd.preset_bit_scan_recovery = CMD_NOT_ALLOW;//预置位扫描恢复
|
||
cmd.preset_bit_scan_close = CMD_NOT_ALLOW;//预置位扫描关闭
|
||
|
||
cmd.area_set = CMD_NOT_ALLOW;//区域数据设置
|
||
cmd.area_scan_start = CMD_NOT_ALLOW;//区域扫描启动
|
||
cmd.area_scan_pause = CMD_ALLOW;//区域扫描暂停
|
||
cmd.area_scan_recovery = CMD_NOT_ALLOW;//区域扫描恢复
|
||
cmd.area_scan_close = CMD_ALLOW;//区域扫描关闭
|
||
|
||
cmd.set_zero = CMD_NOT_ALLOW;
|
||
return cmd;
|
||
}
|
||
|
||
if(g_area_scan.state >= AREA_SCAN_PAUSE_A && //云台处于区域扫描暂停状态
|
||
g_area_scan.state <= AREA_SCAN_PAUSE_B)
|
||
{
|
||
if(g_area_scan.state == AREA_SCAN_PAUSE_A)
|
||
{
|
||
cmd.manual_control = CMD_NOT_ALLOW;//手动控制，包括上下左右等方向控制
|
||
cmd.manual_stop = CMD_NOT_ALLOW;//手动停止指令
|
||
|
||
cmd.preset_bit_set = CMD_ALLOW;//预置位设置
|
||
cmd.preset_bit_del = CMD_ALLOW;//预置位删除
|
||
cmd.preset_bit_call = CMD_NOT_ALLOW;//预置位调用
|
||
|
||
cmd.preset_bit_scan_start = CMD_NOT_ALLOW;//预置位扫描启动
|
||
cmd.preset_bit_scan_pause = CMD_NOT_ALLOW;//预置位扫描暂停
|
||
cmd.preset_bit_scan_recovery = CMD_NOT_ALLOW;//预置位扫描恢复
|
||
cmd.preset_bit_scan_close = CMD_NOT_ALLOW;//预置位扫描关闭
|
||
|
||
cmd.area_set = CMD_NOT_ALLOW;//区域数据设置
|
||
cmd.area_scan_start = CMD_NOT_ALLOW;//区域扫描启动
|
||
cmd.area_scan_pause = CMD_NOT_ALLOW;//区域扫描暂停
|
||
cmd.area_scan_recovery = CMD_NOT_ALLOW;//区域扫描恢复
|
||
cmd.area_scan_close = CMD_NOT_ALLOW;//区域扫描关闭
|
||
|
||
cmd.set_zero = CMD_NOT_ALLOW;
|
||
}
|
||
if(g_area_scan.state == AREA_SCAN_PAUSE_B)
|
||
{
|
||
cmd.manual_control = CMD_ALLOW;//手动控制，包括上下左右等方向控制
|
||
cmd.manual_stop = CMD_ALLOW;//手动停止指令
|
||
|
||
cmd.preset_bit_set = CMD_ALLOW;//预置位设置
|
||
cmd.preset_bit_del = CMD_ALLOW;//预置位删除
|
||
cmd.preset_bit_call = CMD_ALLOW;//预置位调用
|
||
|
||
cmd.preset_bit_scan_start = CMD_NOT_ALLOW;//预置位扫描启动
|
||
cmd.preset_bit_scan_pause = CMD_NOT_ALLOW;//预置位扫描暂停
|
||
cmd.preset_bit_scan_recovery = CMD_NOT_ALLOW;//预置位扫描恢复
|
||
cmd.preset_bit_scan_close = CMD_NOT_ALLOW;//预置位扫描关闭
|
||
|
||
cmd.area_set = CMD_NOT_ALLOW;//区域数据设置
|
||
cmd.area_scan_start = CMD_ALLOW;//区域扫描启动
|
||
cmd.area_scan_pause = CMD_NOT_ALLOW;//区域扫描暂停
|
||
cmd.area_scan_recovery = CMD_ALLOW;//区域扫描恢复
|
||
cmd.area_scan_close = CMD_ALLOW;//区域扫描关闭
|
||
|
||
cmd.set_zero = CMD_NOT_ALLOW;
|
||
}
|
||
return cmd;
|
||
}
|
||
|
||
if(g_area_scan.state >= AREA_SCAN_RECOVERY &&//云台处于预置位扫描恢复阶段
|
||
g_area_scan.state <= AREA_SCAN_RECOVERY + 1)
|
||
{
|
||
cmd.manual_control = CMD_NOT_ALLOW;//手动控制，包括上下左右等方向控制
|
||
cmd.manual_stop = CMD_NOT_ALLOW;//手动停止指令
|
||
|
||
cmd.preset_bit_set = CMD_ALLOW;//预置位设置
|
||
cmd.preset_bit_del = CMD_ALLOW;//预置位删除
|
||
cmd.preset_bit_call = CMD_NOT_ALLOW;//预置位调用
|
||
|
||
cmd.preset_bit_scan_start = CMD_NOT_ALLOW;//预置位扫描启动
|
||
cmd.preset_bit_scan_pause = CMD_NOT_ALLOW;//预置位扫描暂停
|
||
cmd.preset_bit_scan_recovery = CMD_NOT_ALLOW;//预置位扫描恢复
|
||
cmd.preset_bit_scan_close = CMD_NOT_ALLOW;//预置位扫描关闭
|
||
|
||
cmd.area_set = CMD_NOT_ALLOW;//区域数据设置
|
||
cmd.area_scan_start = CMD_ALLOW;//区域扫描启动
|
||
cmd.area_scan_pause = CMD_NOT_ALLOW;//区域扫描暂停
|
||
cmd.area_scan_recovery = CMD_NOT_ALLOW;//区域扫描恢复
|
||
cmd.area_scan_close = CMD_ALLOW;//区域扫描关闭
|
||
|
||
cmd.set_zero = CMD_NOT_ALLOW;
|
||
|
||
return cmd;
|
||
}
|
||
//其他正常状态
|
||
cmd.manual_control = CMD_ALLOW;//手动控制，包括上下左右等方向控制
|
||
cmd.manual_stop = CMD_ALLOW;//手动停止指令
|
||
|
||
cmd.preset_bit_set = CMD_ALLOW;//预置位设置
|
||
cmd.preset_bit_del = CMD_ALLOW;//预置位删除
|
||
cmd.preset_bit_call = CMD_ALLOW;//预置位调用
|
||
|
||
cmd.preset_bit_scan_start = CMD_ALLOW;//预置位扫描启动
|
||
cmd.preset_bit_scan_pause = CMD_NOT_ALLOW;//预置位扫描暂停
|
||
cmd.preset_bit_scan_recovery = CMD_NOT_ALLOW;//预置位扫描恢复
|
||
cmd.preset_bit_scan_close = CMD_NOT_ALLOW;//预置位扫描关闭
|
||
|
||
cmd.area_set = CMD_ALLOW;//区域数据设置
|
||
cmd.area_scan_start = CMD_ALLOW;//区域扫描启动
|
||
cmd.area_scan_pause = CMD_NOT_ALLOW;//区域扫描暂停
|
||
cmd.area_scan_recovery = CMD_NOT_ALLOW;//区域扫描恢复
|
||
cmd.area_scan_close = CMD_NOT_ALLOW;//区域扫描关闭
|
||
|
||
cmd.set_zero = CMD_ALLOW;
|
||
return cmd;
|
||
}
|
||
|
||
|
||
|
||
//云台工作模式判断
|
||
PtzWorkMode ptz_work_mode_judge()
|
||
{
|
||
PtzWorkMode mode;
|
||
//云台还处于自检状态
|
||
if(g_ptz.hori_self_check != PTZ_HORI_SELF_CHECK_END ||
|
||
g_ptz.vert_self_check != PTZ_VERT_SELF_CHECK_END)
|
||
{
|
||
mode.mode = SELF_CHECKING;
|
||
mode.data_a = g_ptz.hori_self_check;
|
||
mode.data_b = g_ptz.vert_self_check;
|
||
return mode;
|
||
}
|
||
//云台处于故障状态
|
||
if(g_ptz.fault_detect.fault == FAULT)
|
||
{
|
||
if(g_ptz.fault_detect.hori_rotate_fault == FAULT)
|
||
{//只是水平电机无法转动
|
||
mode.mode = MOTOR_HORI_CANNOT_ROT;
|
||
}
|
||
if(g_ptz.fault_detect.vert_rotate_fault == FAULT)
|
||
{//只是水平电机无法转动
|
||
mode.mode = MOTOR_VERT_CANNOT_ROT;
|
||
}
|
||
if(g_ptz.fault_detect.vert_rotate_fault == FAULT && //两个电机都无法转动
|
||
g_ptz.fault_detect.hori_rotate_fault == FAULT)
|
||
{
|
||
mode.mode = MOTOR_HOVE_CANNOT_ROT;
|
||
}
|
||
mode.data_a = g_ptz.hori_start_stop_set;
|
||
mode.data_b = g_ptz.vert_start_stop_set;
|
||
return mode;
|
||
}
|
||
//云台处于预置位扫描状态
|
||
if(g_preset_bit_scan.state >= PRESET_BIT_SCAN_START &&
|
||
g_preset_bit_scan.state <= PRESET_BIT_SCAN_START + 4)
|
||
{
|
||
mode.mode = PP_SCANING;
|
||
mode.data_a = g_preset_bit_scan.state;
|
||
mode.data_b = g_preset_bit_scan.actual_num;
|
||
return mode;
|
||
}
|
||
//云台处于预置位扫描暂停状态
|
||
if(g_preset_bit_scan.state == PRESET_BIT_SCAN_PAUSE_A ||
|
||
g_preset_bit_scan.state == PRESET_BIT_SCAN_PAUSE_B)
|
||
{
|
||
mode.mode = PP_PAUSING;
|
||
mode.data_a = g_preset_bit_scan.state;
|
||
mode.data_b = g_preset_bit_scan.actual_num;
|
||
return mode;
|
||
}
|
||
|
||
if(g_preset_bit_scan.state == PRESET_BIT_SCAN_RECOVERY)//预置位扫描恢复状态中
|
||
{
|
||
mode.mode = PP_RECOING;
|
||
mode.data_a = g_preset_bit_scan.state;
|
||
mode.data_b = g_preset_bit_scan.actual_num;
|
||
return mode;
|
||
}
|
||
|
||
if(g_area_scan.state >= AREA_SCAN_START && //云台处于区域扫描状态
|
||
g_area_scan.state < AREA_SCAN_PAUSE_A)
|
||
{
|
||
mode.mode = AREA_SCANING;
|
||
mode.data_a = g_area_scan.state;
|
||
mode.data_b = g_area_scan.actual_num;
|
||
return mode;
|
||
}
|
||
|
||
if(g_area_scan.state >= AREA_SCAN_PAUSE_A && //云台处于区域扫描暂停状态
|
||
g_area_scan.state <= AREA_SCAN_PAUSE_B)
|
||
{
|
||
mode.mode = AREA_PAUSING;
|
||
mode.data_a = g_area_scan.state;
|
||
mode.data_b = g_area_scan.actual_num;
|
||
return mode;
|
||
}
|
||
|
||
if(g_area_scan.state >= AREA_SCAN_RECOVERY &&//云台处于预置位扫描恢复阶段
|
||
g_area_scan.state <= AREA_SCAN_RECOVERY + 1)
|
||
{
|
||
mode.mode = AREA_RECOING;
|
||
mode.data_a = g_area_scan.state;
|
||
mode.data_b = g_area_scan.actual_num;
|
||
return mode;
|
||
}
|
||
//其他正常状态
|
||
mode.mode = OTHER_MODE;
|
||
mode.data_a = g_ptz.hori_start_stop_set;
|
||
mode.data_b = g_ptz.vert_start_stop_set;
|
||
return mode;
|
||
}
|
||
|
||
|
||
//发送云台当前的工作模式
|
||
void ptz_send_work_mode(char dev)
|
||
{
|
||
unsigned char work_mode[7] = {0xff,0x00,0x00,0x00,0x00,0x00,0x00};
|
||
PtzWorkMode ptz_mode;
|
||
ptz_mode = ptz_work_mode_judge();
|
||
work_mode[1] = g_ptz.address;
|
||
work_mode[2] = 0xe0;
|
||
work_mode[3] = ptz_mode.mode;
|
||
work_mode[4] = ptz_mode.data_a;
|
||
work_mode[5] = ptz_mode.data_b;
|
||
work_mode[6] = MotorCalPelcoDSUM(work_mode,sizeof(work_mode));
|
||
|
||
ptz_send_data(dev,work_mode, sizeof(work_mode));
|
||
|
||
}
|
||
|
||
|
||
|