1522 lines
58 KiB
C
1522 lines
58 KiB
C
///Copyright (c) 2022, 四川汇源光通信有限公司
|
||
///All rights reserved.
|
||
///@file service_selfcheck.C
|
||
///@brief 云台开机自检程序，包括水平全范围自检、简易自检，垂直全范围、简易
|
||
/// 自检
|
||
///
|
||
///@details
|
||
///@note
|
||
///@author ypc
|
||
///@date 2019/05/23
|
||
///
|
||
///@version v1.0 2019/05/23 初始版本
|
||
/// Hyt2.1.0 2022/06/02 更换GD32单片，文件移植
|
||
|
||
#include "service_selfcheck.h"
|
||
#include "bsp_os.h"
|
||
#include "ptz_global_variable.h"
|
||
#include "angle_poweroffsave.h"
|
||
#include "comm_cfginfo.h"
|
||
#include "tmc2160.h"
|
||
#include "service_error_count.h"
|
||
|
||
#ifdef PTZ_BLDC_MOTOR
|
||
/// 云台全范围自检
|
||
static unsigned char ptz_hori_complete_self_check_task()
|
||
{
|
||
switch(g_ptz.hori_self_check)
|
||
{
|
||
//首先让云台水平向右转动
|
||
case PTZ_HORI_SELF_CHECK_COMPLETE_STEP:
|
||
if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
|
||
{
|
||
ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1;
|
||
}
|
||
else//没有被挡住
|
||
{
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_ps_sw3_left_rise = 0;
|
||
g_ptz.hori_ps_sw3_right_rise = 0;
|
||
g_ptz.hori_ps_sw3_left_fall = 0;
|
||
ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
case PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1:
|
||
if(g_ptz.hori_ps_sw3_state == PS_NO_COVER)
|
||
{
|
||
OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_ps_sw3_left_rise = 0;
|
||
g_ptz.hori_ps_sw3_right_rise = 0;
|
||
g_ptz.hori_ps_sw3_left_fall = 0;
|
||
ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
//等待第一次水平光电开关下降沿中断
|
||
case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2):
|
||
if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
|
||
{
|
||
g_ptz.hori_as5047d.as5047d_data_reset = 1;
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_self_check++;
|
||
|
||
OSTimeDlyHMSM(0u, 0u, 5u, 0u);
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
}
|
||
break;
|
||
|
||
//等待第二次水平光电开关下降沿中断
|
||
case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 3):
|
||
if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
|
||
{
|
||
g_ptz.hori_as5047d.as5047d_cycle_num = g_ptz.hori_as5047d.as5047d_cycle_count;
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_max = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_K = 360.0 / g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
||
|
||
g_ptz.hori_as5047d.as5047d_remain_angle = g_ptz.hori_as5047d.as5047d_ptz_angle_max -
|
||
g_ptz.hori_as5047d.as5047d_cycle_num * 360.0;
|
||
g_ptz.hori_angleP.angle_allow_max = 360.0;
|
||
g_ptz.hori_angleP.angle_allow_min = 0.0;
|
||
g_ptz.hori_as5047d.as5047d_state = 1;
|
||
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_ps_sw3_left_rise = 0;
|
||
g_ptz.hori_ps_sw3_right_rise = 0;
|
||
g_ptz.hori_ps_sw3_left_fall = 0;
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
g_ptz.hori_angle_state = 1;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE;
|
||
}
|
||
break;
|
||
}
|
||
return g_ptz.hori_self_check;
|
||
}
|
||
|
||
/// 云台水平简化自检
|
||
static unsigned char ptz_hori_simplify_self_check_task()
|
||
{
|
||
switch(g_ptz.hori_self_check)
|
||
{
|
||
case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP:
|
||
if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
|
||
{
|
||
ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1;
|
||
}
|
||
else//没有挡住
|
||
{
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_ps_sw3_left_rise = 0;
|
||
g_ptz.hori_ps_sw3_right_rise = 0;
|
||
g_ptz.hori_ps_sw3_left_fall = 0;
|
||
ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1://向左转动，直到光电开关脱离挡片遮挡
|
||
if(g_ptz.hori_ps_sw3_state == PS_NO_COVER)
|
||
{
|
||
OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_ps_sw3_left_rise = 0;
|
||
g_ptz.hori_ps_sw3_right_rise = 0;
|
||
g_ptz.hori_ps_sw3_left_fall = 0;
|
||
ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2:
|
||
//等待水平光电开关右转下降沿
|
||
if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
|
||
{
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = 0;
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
|
||
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
|
||
g_ptz.hori_as5047d.as5047d_state = 1;
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_ps_sw3_left_rise = 0;
|
||
g_ptz.hori_ps_sw3_right_rise = 0;
|
||
g_ptz.hori_ps_sw3_left_fall = 0;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3;
|
||
}
|
||
break;
|
||
|
||
case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3://转动到0位
|
||
OSTimeDlyHMSM(0u, 0u, 1u, 0u);//延时转动
|
||
g_ptz.hori_angle_control = 0.0;
|
||
g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
|
||
ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_A;
|
||
g_ptz.hori_angle_state = 1;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
break;
|
||
}
|
||
return g_ptz.hori_self_check;
|
||
}
|
||
|
||
|
||
/// 云台垂直全范围自检
|
||
static unsigned char ptz_vert_complete_self_check_task()
|
||
{
|
||
switch(g_ptz.vert_self_check)
|
||
{
|
||
///首先读取光电开关的状态，初步判断垂直状态
|
||
case PTZ_VERT_SELF_CHECK_COMPLETE_STEP:
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置
|
||
{
|
||
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1;
|
||
}
|
||
if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置
|
||
{
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2;
|
||
}
|
||
if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER)
|
||
//云台处于中间位置
|
||
{
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1):
|
||
if(g_ptz.vert_ps_sw1_state == PS_NO_COVER)
|
||
{
|
||
OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
g_ptz.vert_self_check ++;
|
||
}
|
||
break;
|
||
//开启向下转动，查找下俯极限位置
|
||
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2):
|
||
ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check ++;
|
||
break;
|
||
|
||
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
|
||
//判断是否到达下俯极限位置,然后云台开始向上转动
|
||
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3):
|
||
if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
|
||
g_ptz.vert_ps_sw1_state == PS_COVER)
|
||
{
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_as5047d.as5047d_data_reset = 1;
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10;
|
||
}
|
||
break;
|
||
#endif
|
||
|
||
#ifdef PTZ_SW1_UP_RISE_UPDATE
|
||
//判断是否到达下俯极限位置,然后云台开始向上转动
|
||
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3):
|
||
if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
|
||
g_ptz.vert_ps_sw1_state == PS_COVER)
|
||
{
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 300u);
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check++;
|
||
}
|
||
break;
|
||
//等待上仰上升沿
|
||
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 4):
|
||
if(g_ptz.vert_ps_sw1_up_rise >= 1 ||
|
||
g_ptz.vert_ps_sw1_state == PS_NO_COVER)
|
||
{
|
||
g_ptz.vert_as5047d.as5047d_data_reset = 1;
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10;
|
||
}
|
||
break;
|
||
#endif
|
||
|
||
//等待SW2下降沿,计算垂直转动范围
|
||
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10):
|
||
if(g_ptz.vert_ps_sw2_up_fall >= 1 ||
|
||
g_ptz.vert_ps_sw2_state == PS_COVER)
|
||
{
|
||
g_ptz.vert_as5047d.as5047d_cycle_num = g_ptz.vert_as5047d.as5047d_cycle_count;
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_max = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
|
||
|
||
g_ptz.vert_angleP.angle_range =
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_max / PTZ_VERT_BIG_GEAR_RATIO;
|
||
|
||
g_ptz.vert_angleP.angle_allow_min = g_ptz.vert_angleP.angle_range / 2.0 * (-1);
|
||
g_ptz.vert_angleP.angle_allow_max = g_ptz.vert_angleP.angle_range / 2.0;
|
||
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_K = 1 / PTZ_VERT_BIG_GEAR_RATIO;
|
||
|
||
|
||
g_ptz.vert_as5047d.as5047d_remain_angle = g_ptz.vert_as5047d.as5047d_ptz_angle_max -
|
||
g_ptz.vert_as5047d.as5047d_cycle_num * 360.0;
|
||
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
g_ptz.vert_angle_state = 1;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE;
|
||
}
|
||
break;
|
||
}
|
||
return g_ptz.vert_self_check;
|
||
}
|
||
|
||
/// 云台垂直简化自检
|
||
static unsigned char ptz_vert_simplify_self_check_task()
|
||
{
|
||
switch(g_ptz.vert_self_check)
|
||
{
|
||
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP:
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置
|
||
{
|
||
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1;
|
||
}
|
||
if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置
|
||
{
|
||
ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
|
||
}
|
||
if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER)
|
||
//云台处于中间位置
|
||
{
|
||
ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1:
|
||
if(g_ptz.vert_ps_sw1_state == PS_NO_COVER)
|
||
{
|
||
OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
|
||
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2:
|
||
if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
|
||
g_ptz.vert_ps_sw1_state == PS_COVER)
|
||
{//等待SW1光电开关被遮挡，同时更新当前位置
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = 0;
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
|
||
g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10;
|
||
}
|
||
break;
|
||
#endif
|
||
|
||
#ifdef PTZ_SW1_UP_RISE_UPDATE
|
||
//判断是否到达下俯极限位置,然后云台开始向上转动
|
||
case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2):
|
||
if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
|
||
g_ptz.vert_ps_sw1_state == PS_COVER)
|
||
{
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 300u);
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check++;
|
||
}
|
||
break;
|
||
|
||
//等待上仰上升沿
|
||
case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 3):
|
||
if(g_ptz.vert_ps_sw1_up_rise >= 1 ||
|
||
g_ptz.vert_ps_sw1_state == PS_NO_COVER)
|
||
{
|
||
//等待SW1光电开关被遮挡，同时更新当前位置
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = 0;
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
|
||
g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10;
|
||
}
|
||
break;
|
||
#endif
|
||
|
||
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10:
|
||
OSTimeDlyHMSM(0u, 0u, 1u, 0u);
|
||
g_ptz.vert_angle_control = 0.0;
|
||
g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED;
|
||
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_A;
|
||
g_ptz.vert_angle_state = 1;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
break;
|
||
}
|
||
return g_ptz.vert_self_check;
|
||
}
|
||
|
||
#endif
|
||
|
||
#ifdef PTZ_STEP_MOTOR
|
||
/// 云台全范围自检
|
||
static unsigned char ptz_hori_complete_self_check_task()
|
||
{
|
||
TMC2160Control data;
|
||
switch(g_ptz.hori_self_check)
|
||
{
|
||
//首先让云台水平向右转动
|
||
case PTZ_HORI_SELF_CHECK_COMPLETE_STEP:
|
||
data = ptz_hori_choice_microstep(PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_tmc2160 = data;
|
||
if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
|
||
{
|
||
ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1;
|
||
}
|
||
else//没有被挡住
|
||
{
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_ps_sw3_left_rise = 0;
|
||
g_ptz.hori_ps_sw3_right_rise = 0;
|
||
g_ptz.hori_ps_sw3_left_fall = 0;
|
||
ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
case PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 1:
|
||
if(g_ptz.hori_ps_sw3_state == PS_NO_COVER)
|
||
{
|
||
OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_ps_sw3_left_rise = 0;
|
||
g_ptz.hori_ps_sw3_right_rise = 0;
|
||
g_ptz.hori_ps_sw3_left_fall = 0;
|
||
ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
//等待第一次水平光电开关下降沿中断
|
||
case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 2):
|
||
if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
|
||
{
|
||
g_ptz.hori_as5047d.as5047d_data_reset = 1;
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_self_check++;
|
||
|
||
OSTimeDlyHMSM(0u, 0u, 5u, 0u);
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
}
|
||
break;
|
||
|
||
//等待第二次水平光电开关下降沿中断
|
||
case (PTZ_HORI_SELF_CHECK_COMPLETE_STEP + 3):
|
||
if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
|
||
{
|
||
g_ptz.hori_as5047d.as5047d_cycle_num = g_ptz.hori_as5047d.as5047d_cycle_count;
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_max = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_K = 360.0 / g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
||
|
||
g_ptz.hori_as5047d.as5047d_remain_angle = g_ptz.hori_as5047d.as5047d_ptz_angle_max -
|
||
g_ptz.hori_as5047d.as5047d_cycle_num * 360.0;
|
||
g_ptz.hori_angleP.angle_allow_max = 360.0;
|
||
g_ptz.hori_angleP.angle_allow_min = 0.0;
|
||
g_ptz.hori_as5047d.as5047d_state = 1;
|
||
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_ps_sw3_left_rise = 0;
|
||
g_ptz.hori_ps_sw3_right_rise = 0;
|
||
g_ptz.hori_ps_sw3_left_fall = 0;
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
g_ptz.hori_angle_state = 1;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE;
|
||
}
|
||
break;
|
||
}
|
||
return g_ptz.hori_self_check;
|
||
}
|
||
|
||
/// 云台水平简化自检
|
||
static unsigned char ptz_hori_simplify_self_check_task()
|
||
{
|
||
TMC2160Control data;
|
||
switch(g_ptz.hori_self_check)
|
||
{
|
||
case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP:
|
||
data = ptz_hori_choice_microstep(PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_tmc2160 = data;
|
||
if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
|
||
{
|
||
ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1;
|
||
}
|
||
else//没有挡住
|
||
{
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_ps_sw3_left_rise = 0;
|
||
g_ptz.hori_ps_sw3_right_rise = 0;
|
||
g_ptz.hori_ps_sw3_left_fall = 0;
|
||
ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1://向左转动，直到光电开关脱离挡片遮挡
|
||
if(g_ptz.hori_ps_sw3_state == PS_NO_COVER)
|
||
{
|
||
OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_ps_sw3_left_rise = 0;
|
||
g_ptz.hori_ps_sw3_right_rise = 0;
|
||
g_ptz.hori_ps_sw3_left_fall = 0;
|
||
ptz_hori_start(PTZ_HORI_DIR_RIGHT, PTZ_HORI_SELF_CHECK_SPEED);
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 2:
|
||
//等待水平光电开关右转下降沿
|
||
if(g_ptz.hori_ps_sw3_right_fall >= PS_FALL_ON)
|
||
{
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = 0;
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = 0;
|
||
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
|
||
g_ptz.hori_as5047d.as5047d_state = 1;
|
||
g_ptz.hori_ps_sw3_right_fall = 0;
|
||
g_ptz.hori_ps_sw3_left_rise = 0;
|
||
g_ptz.hori_ps_sw3_right_rise = 0;
|
||
g_ptz.hori_ps_sw3_left_fall = 0;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3;
|
||
}
|
||
break;
|
||
|
||
case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 3://转动到0位
|
||
OSTimeDlyHMSM(0u, 0u, 1u, 0u);//延时转动
|
||
g_ptz.hori_angle_control = 0.0;
|
||
g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
|
||
ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_A;
|
||
g_ptz.hori_angle_state = 1;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
break;
|
||
}
|
||
return g_ptz.hori_self_check;
|
||
}
|
||
|
||
|
||
/// 云台垂直全范围自检
|
||
static unsigned char ptz_vert_complete_self_check_task()
|
||
{
|
||
TMC2160Control data;
|
||
switch(g_ptz.vert_self_check)
|
||
{
|
||
///首先读取光电开关的状态，初步判断垂直状态
|
||
case PTZ_VERT_SELF_CHECK_COMPLETE_STEP:
|
||
data = ptz_vert_choice_microstep(PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_tmc2160 = data;
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置
|
||
{
|
||
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1;
|
||
}
|
||
if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置
|
||
{
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2;
|
||
}
|
||
if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER)
|
||
//云台处于中间位置
|
||
{
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 1):
|
||
if(g_ptz.vert_ps_sw1_state == PS_NO_COVER)
|
||
{
|
||
OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
g_ptz.vert_self_check ++;
|
||
}
|
||
break;
|
||
//开启向下转动，查找下俯极限位置
|
||
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 2):
|
||
ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check ++;
|
||
break;
|
||
|
||
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
|
||
//判断是否到达下俯极限位置,然后云台开始向上转动
|
||
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3):
|
||
if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
|
||
g_ptz.vert_ps_sw1_state == PS_COVER)
|
||
{
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_as5047d.as5047d_data_reset = 1;
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10;
|
||
}
|
||
break;
|
||
#endif
|
||
|
||
#ifdef PTZ_SW1_UP_RISE_UPDATE
|
||
//判断是否到达下俯极限位置,然后云台开始向上转动
|
||
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 3):
|
||
if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
|
||
g_ptz.vert_ps_sw1_state == PS_COVER)
|
||
{
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 300u);
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check++;
|
||
}
|
||
break;
|
||
//等待上仰上升沿
|
||
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 4):
|
||
if(g_ptz.vert_ps_sw1_up_rise >= 1 ||
|
||
g_ptz.vert_ps_sw1_state == PS_NO_COVER)
|
||
{
|
||
g_ptz.vert_as5047d.as5047d_data_reset = 1;
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10;
|
||
}
|
||
break;
|
||
#endif
|
||
|
||
//等待SW2下降沿,计算垂直转动范围
|
||
case (PTZ_VERT_SELF_CHECK_COMPLETE_STEP + 10):
|
||
if(g_ptz.vert_ps_sw2_up_fall >= 1 ||
|
||
g_ptz.vert_ps_sw2_state == PS_COVER)
|
||
{
|
||
g_ptz.vert_as5047d.as5047d_cycle_num = g_ptz.vert_as5047d.as5047d_cycle_count;
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_max = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
|
||
|
||
g_ptz.vert_angleP.angle_range =
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_max / PTZ_VERT_BIG_GEAR_RATIO;
|
||
|
||
g_ptz.vert_angleP.angle_allow_min = g_ptz.vert_angleP.angle_range / 2.0 * (-1);
|
||
g_ptz.vert_angleP.angle_allow_max = g_ptz.vert_angleP.angle_range / 2.0;
|
||
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_K = 1 / PTZ_VERT_BIG_GEAR_RATIO;
|
||
|
||
|
||
g_ptz.vert_as5047d.as5047d_remain_angle = g_ptz.vert_as5047d.as5047d_ptz_angle_max -
|
||
g_ptz.vert_as5047d.as5047d_cycle_num * 360.0;
|
||
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
g_ptz.vert_angle_state = 1;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE;
|
||
}
|
||
break;
|
||
}
|
||
return g_ptz.vert_self_check;
|
||
}
|
||
|
||
/// 云台垂直简化自检
|
||
static unsigned char ptz_vert_simplify_self_check_task()
|
||
{
|
||
TMC2160Control data;
|
||
switch(g_ptz.vert_self_check)
|
||
{
|
||
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP:
|
||
data = ptz_vert_choice_microstep(PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_tmc2160 = data;
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
if(g_ptz.vert_ps_sw1_state == PS_COVER)//云台处于下俯极限位置
|
||
{
|
||
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1;
|
||
}
|
||
if(g_ptz.vert_ps_sw2_state == PS_COVER)//云台处于上仰极限位置
|
||
{
|
||
ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
|
||
}
|
||
if(g_ptz.vert_ps_sw1_state == PS_NO_COVER && g_ptz.vert_ps_sw2_state == PS_NO_COVER)
|
||
//云台处于中间位置
|
||
{
|
||
ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 1:
|
||
if(g_ptz.vert_ps_sw1_state == PS_NO_COVER)
|
||
{
|
||
OSTimeDlyHMSM(0u, 0u, 3u, 0u);//延时转动
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
ptz_vert_start(PTZ_VERT_DIR_DOWN, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2;
|
||
}
|
||
break;
|
||
|
||
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
|
||
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2:
|
||
if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
|
||
g_ptz.vert_ps_sw1_state == PS_COVER)
|
||
{//等待SW1光电开关被遮挡，同时更新当前位置
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = 0;
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
|
||
g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10;
|
||
}
|
||
break;
|
||
#endif
|
||
|
||
#ifdef PTZ_SW1_UP_RISE_UPDATE
|
||
//判断是否到达下俯极限位置,然后云台开始向上转动
|
||
case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 2):
|
||
if(g_ptz.vert_ps_sw1_down_fall >= 1 ||
|
||
g_ptz.vert_ps_sw1_state == PS_COVER)
|
||
{
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 300u);
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
|
||
g_ptz.vert_self_check++;
|
||
}
|
||
break;
|
||
//等待上仰上升沿
|
||
case (PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 3):
|
||
if(g_ptz.vert_ps_sw1_up_rise >= 1 ||
|
||
g_ptz.vert_ps_sw1_state == PS_NO_COVER)
|
||
{
|
||
//等待SW1光电开关被遮挡，同时更新当前位置
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = 0;
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
|
||
g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
g_ptz.vert_ps_sw1_down_fall = 0;
|
||
g_ptz.vert_ps_sw1_up_rise = 0;
|
||
g_ptz.vert_ps_sw2_up_fall = 0;
|
||
g_ptz.vert_ps_sw2_down_rise = 0;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10;
|
||
}
|
||
break;
|
||
#endif
|
||
|
||
case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP + 10:
|
||
OSTimeDlyHMSM(0u, 0u, 1u, 0u);
|
||
data = ptz_vert_choice_microstep(PTZ_VERT_BEST_SPEED);
|
||
g_ptz.vert_tmc2160 = data;
|
||
g_ptz.vert_angle_control = 0.0;
|
||
g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED;
|
||
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_A;
|
||
g_ptz.vert_angle_state = 1;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
break;
|
||
}
|
||
return g_ptz.vert_self_check;
|
||
}
|
||
|
||
#endif
|
||
|
||
|
||
|
||
static void ptz_hori_self_check_task()
|
||
{
|
||
while(1)
|
||
{
|
||
if(error_conut_state == 0)
|
||
{//误差计算结束后开始自检
|
||
ptz_hori_complete_self_check_task();
|
||
ptz_hori_simplify_self_check_task();
|
||
}
|
||
//保存自检参数
|
||
if(g_ptz.vert_self_check == PTZ_VERT_SELF_CHECK_DATA_SAVE &&
|
||
g_ptz.hori_self_check == PTZ_HORI_SELF_CHECK_DATA_SAVE)
|
||
{
|
||
ptz_self_check_data_save();//保存数据
|
||
//转到垂直0位
|
||
g_ptz.vert_angle_control = 0.0;
|
||
g_ptz.vert_speed_control = PTZ_VERT_BEST_SPEED;
|
||
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_A;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
//转到水平0位
|
||
g_ptz.hori_angle_control = 0.0;
|
||
g_ptz.hori_speed_control = PTZ_HORI_BEST_SPEED;
|
||
ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_A;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
}
|
||
|
||
if(g_ptz.hori_self_check == PTZ_HORI_SELF_CHECK_END &&
|
||
g_ptz.vert_self_check == PTZ_VERT_SELF_CHECK_END)
|
||
{
|
||
|
||
#ifdef PTZ_CMD_SAVE_BEFORE
|
||
ptz_cmd_before_start();//查看是否让云台自动进行工作
|
||
#endif
|
||
OSTaskDel(OS_PRIO_SELF);//关闭该任务
|
||
}
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
||
}
|
||
}
|
||
|
||
static void ptz_vert_self_check_task()
|
||
{
|
||
while(1)
|
||
{
|
||
if(error_conut_state == 0)
|
||
{//误差计算结束后开始自检
|
||
ptz_vert_complete_self_check_task();
|
||
ptz_vert_simplify_self_check_task();
|
||
}
|
||
if(g_ptz.vert_self_check == PTZ_VERT_SELF_CHECK_END)
|
||
{
|
||
if(g_ptz.offset_angle.vert_offset_switch == 2)
|
||
{
|
||
g_ptz.vert_angleP.angle_allow_min = g_ptz.vert_angleP.angle_range / 2.0 * (-1);
|
||
g_ptz.vert_angleP.angle_allow_max = g_ptz.vert_angleP.angle_range / 2.0;
|
||
g_ptz.vert_angleP.angle_allow_min =
|
||
g_ptz.vert_angleP.angle_allow_min - g_ptz.offset_angle.vert_zero_offset_angle;
|
||
g_ptz.vert_angleP.angle_allow_max =
|
||
g_ptz.vert_angleP.angle_allow_max - g_ptz.offset_angle.vert_zero_offset_angle;
|
||
}
|
||
OSTaskDel(OS_PRIO_SELF);//关闭该任务
|
||
}
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
||
}
|
||
}
|
||
//水平自检任务
|
||
static OS_STK task_hori_self_check_stk[TASK_HORI_SELF_CHECK_STK_SIZE];
|
||
static void creat_task_hori_self_check(void)
|
||
{
|
||
CPU_INT08U task_err;
|
||
CPU_INT08U name_err;
|
||
|
||
task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_self_check_task,
|
||
(void *) 0,
|
||
(OS_STK *)&task_hori_self_check_stk[TASK_HORI_SELF_CHECK_STK_SIZE - 1],
|
||
(INT8U ) TASK_HORI_SELF_CHECK_PRIO,
|
||
(INT16U ) TASK_HORI_SELF_CHECK_PRIO,
|
||
(OS_STK *)&task_hori_self_check_stk[0],
|
||
(INT32U ) TASK_HORI_SELF_CHECK_STK_SIZE,
|
||
(void *) 0,
|
||
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
|
||
#if (OS_TASK_NAME_EN > 0)
|
||
OSTaskNameSet(TASK_HORI_SELF_CHECK_PRIO, "ptz_hori_self_check_task", &name_err);
|
||
#endif
|
||
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
|
||
pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_self_check_task success...\n\r");
|
||
} else {
|
||
pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_self_check_task failed...\n\r");
|
||
}
|
||
}
|
||
|
||
//垂直自检任务
|
||
static OS_STK task_vert_self_check_stk[TASK_VERT_SELF_CHECK_STK_SIZE];
|
||
static void creat_task_vert_self_check(void)
|
||
{
|
||
CPU_INT08U task_err;
|
||
CPU_INT08U name_err;
|
||
|
||
task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_self_check_task,
|
||
(void *) 0,
|
||
(OS_STK *)&task_vert_self_check_stk[TASK_VERT_SELF_CHECK_STK_SIZE - 1],
|
||
(INT8U ) TASK_VERT_SELF_CHECK_PRIO,
|
||
(INT16U ) TASK_VERT_SELF_CHECK_PRIO,
|
||
(OS_STK *)&task_vert_self_check_stk[0],
|
||
(INT32U ) TASK_VERT_SELF_CHECK_STK_SIZE,
|
||
(void *) 0,
|
||
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
|
||
#if (OS_TASK_NAME_EN > 0)
|
||
OSTaskNameSet(TASK_VERT_SELF_CHECK_PRIO, "ptz_vert_self_check_task", &name_err);
|
||
#endif
|
||
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
|
||
pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_self_check_task success...\n\r");
|
||
} else {
|
||
pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_self_check_task failed...\n\r");
|
||
}
|
||
}
|
||
|
||
/*
|
||
@brief 自检参数初始化和自检方式选择
|
||
@param
|
||
@return
|
||
@note 20180903 v1.0
|
||
20220602 v2.0 lqc
|
||
*/
|
||
/*
|
||
static void ptz_self_check_data_init()
|
||
{
|
||
if(ptz_self_check_data_read() == 1)//自检参数读取成功，进行简易自检
|
||
{
|
||
#ifdef PTZ_NO_SELF_CHECK
|
||
if(ptz_power_off_data_read() == 1)
|
||
{
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
g_ptz.hori_angle_state = 1;
|
||
g_ptz.vert_angle_state = 1;
|
||
g_ptz.no_self_check_state = 1;//读取成功
|
||
return;
|
||
}
|
||
g_ptz.no_self_check_state = 0;//读取失败
|
||
#endif
|
||
|
||
g_ptz.hori_angle_state = 0;
|
||
g_ptz.vert_angle_state = 0;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
|
||
|
||
|
||
#ifdef PTZ_VERT_SIMPLIFY_CHECK_OFF//不要垂直简易自检
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
#endif
|
||
|
||
#ifdef PTZ_HORI_SIMPLIFY_CHECK_OFF//不要水平简易自检
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.hori_as5047d.as5047d_state = 1;
|
||
#endif
|
||
}
|
||
else//自检参数失败进行全范围自检
|
||
{
|
||
g_ptz.hori_angle_state = 0;
|
||
g_ptz.vert_angle_state = 0;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP;
|
||
|
||
|
||
#ifdef PTZ_VERT_COMPLETE_CHECK_OFF//不要垂直全范围自检
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE;
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
#endif
|
||
|
||
#ifdef PTZ_HORI_COMPLETE_CHECK_OFF//不要水平全范围自检
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE;
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.hori_as5047d.as5047d_state = 1;
|
||
#endif
|
||
}
|
||
}
|
||
*/
|
||
|
||
/************通过判断配置信息来初始化自检方式***********/
|
||
|
||
#ifdef ONE
|
||
//自检参数初始化和自检方式选择
|
||
static void ptz_self_check_data_init()
|
||
{
|
||
if(strstr((const char*)system_info.complete_self_check, "enable"))
|
||
{//全自检使能
|
||
if(ptz_self_check_data_read() == 1)//自检参数读取成功，进行简易自检
|
||
{//自检参数读取成功，进入简易自检
|
||
|
||
if(strstr((const char*)system_info.simplify_self_check, "both_enable"))
|
||
{//双轴简易自检
|
||
g_ptz.no_self_check_state = 0;//读取失败
|
||
g_ptz.hori_angle_state = 0;
|
||
g_ptz.vert_angle_state = 0;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
|
||
g_ptz.hori_as5047d.as5047d_state = 0;
|
||
g_ptz.vert_as5047d.as5047d_state = 0;
|
||
return;
|
||
}
|
||
if(strstr((const char*)system_info.simplify_self_check, "hori_enable"))
|
||
{//水平简易自检、垂直不自检
|
||
if(ptz_power_off_data_read() == 1)
|
||
{//掉电数据读取成功
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
g_ptz.hori_angle_state = 0;
|
||
g_ptz.vert_angle_state = 1;
|
||
g_ptz.no_self_check_state = 1;//读取成功
|
||
g_ptz.hori_as5047d.as5047d_state = 0;
|
||
|
||
}else{//掉电数据读取失败，垂直赋默认值，水平简易自检
|
||
g_ptz.no_self_check_state = 0;//读取失败
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
|
||
g_ptz.hori_as5047d.as5047d_state = 0;
|
||
}
|
||
return;
|
||
}
|
||
if(strstr((const char*)system_info.simplify_self_check, "vert_enable"))
|
||
{//垂直简易自检、水平不自检
|
||
if(ptz_power_off_data_read() == 1)
|
||
{//掉电数据读取成功
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
|
||
g_ptz.hori_angle_state = 1;
|
||
g_ptz.vert_angle_state = 0;
|
||
g_ptz.no_self_check_state = 1;//读取成功
|
||
g_ptz.vert_as5047d.as5047d_state = 0;
|
||
|
||
}else{//掉电数据读取失败，垂直赋默认值，水平简易自检
|
||
g_ptz.no_self_check_state = 0;//读取失败
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
|
||
g_ptz.vert_as5047d.as5047d_state = 0;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.hori_as5047d.as5047d_state = 1;
|
||
}
|
||
return;
|
||
}
|
||
if(strstr((const char*)system_info.simplify_self_check, "both_disable"))
|
||
{//双轴不简易自检
|
||
if(ptz_power_off_data_read() == 1)
|
||
{//掉电数据读取成功，结束自检
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
g_ptz.hori_angle_state = 1;
|
||
g_ptz.vert_angle_state = 1;
|
||
g_ptz.no_self_check_state = 1;//读取成功
|
||
|
||
}else{//掉电数据读取失败，赋默认值，结束自检
|
||
g_ptz.no_self_check_state = 0;//读取失败
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.hori_as5047d.as5047d_state = 1;
|
||
}
|
||
return;
|
||
}
|
||
|
||
}else{
|
||
//自检参数失败进行全范围自检
|
||
g_ptz.hori_angle_state = 0;
|
||
g_ptz.vert_angle_state = 0;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP;
|
||
return;
|
||
}
|
||
}else{//任何情况下都不自检
|
||
if((ptz_self_check_data_read() == 1) && (ptz_power_off_data_read() == 1))
|
||
{//自检参数读取成功、且掉电数据读取成功
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
g_ptz.hori_angle_state = 1;
|
||
g_ptz.vert_angle_state = 1;
|
||
g_ptz.no_self_check_state = 1;//读取成功
|
||
return;
|
||
}else{
|
||
g_ptz.no_self_check_state = 0;//读取失败
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE;
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE;
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.hori_as5047d.as5047d_state = 1;
|
||
return;
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
#endif
|
||
|
||
//自检参数初始化和自检方式选择
|
||
static void ptz_self_check_data_init()
|
||
{
|
||
if(strstr((const char*)system_info.complete_self_check, "enable"))
|
||
{//全自检使能
|
||
if(ptz_self_check_data_read() == 1)//自检参数读取成功，进行简易自检
|
||
{//自检参数读取成功，进入简易自检
|
||
error_conut_state = 0;//不进行误差计算
|
||
if(strstr((const char*)system_info.simplify_self_check, "both_enabled"))
|
||
{//双轴简易自检
|
||
g_ptz.no_self_check_state = 0;//读取失败
|
||
g_ptz.hori_angle_state = 0;
|
||
g_ptz.vert_angle_state = 0;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
|
||
g_ptz.hori_as5047d.as5047d_state = 0;//编码器状态为0时，角度计算不可用，该值保存于自检参数以及掉电保护数据中，自检完成后置1，
|
||
g_ptz.vert_as5047d.as5047d_state = 0;
|
||
return;
|
||
}
|
||
if(strstr((const char*)system_info.simplify_self_check, "hori_enabled"))
|
||
{//水平简易自检、垂直不自检
|
||
if(ptz_power_off_data_read() == 1)
|
||
{//掉电数据读取成功
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
g_ptz.hori_angle_state = 0;
|
||
g_ptz.vert_angle_state = 1;
|
||
g_ptz.no_self_check_state = 1;//读取成功
|
||
g_ptz.hori_as5047d.as5047d_state = 0;
|
||
|
||
}else{//掉电数据读取失败，垂直赋默认值，水平简易自检
|
||
g_ptz.no_self_check_state = 0;//读取失败
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP;
|
||
g_ptz.hori_as5047d.as5047d_state = 0;
|
||
g_ptz.hori_angle_state = 0;//掉电参数读取失败，角度状态先置故障
|
||
g_ptz.vert_angle_state = 0;
|
||
}
|
||
return;
|
||
}
|
||
if(strstr((const char*)system_info.simplify_self_check, "vert_enabled"))
|
||
{//垂直简易自检、水平不自检
|
||
if(ptz_power_off_data_read() == 1)
|
||
{//掉电数据读取成功
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
|
||
g_ptz.hori_angle_state = 1;
|
||
g_ptz.vert_angle_state = 0;
|
||
g_ptz.no_self_check_state = 1;//读取成功
|
||
g_ptz.vert_as5047d.as5047d_state = 0;
|
||
|
||
}else{//掉电数据读取失败，水平赋默认值，垂直简易自检
|
||
g_ptz.no_self_check_state = 0;//读取失败
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP;
|
||
g_ptz.vert_as5047d.as5047d_state = 0;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.hori_as5047d.as5047d_state = 1;
|
||
g_ptz.hori_angle_state = 0;//掉电参数读取失败，角度状态先置故障
|
||
g_ptz.vert_angle_state = 0;
|
||
}
|
||
return;
|
||
}
|
||
if(strstr((const char*)system_info.simplify_self_check, "both_disabled"))
|
||
{//双轴不简易自检
|
||
if(ptz_power_off_data_read() == 1)
|
||
{//掉电数据读取成功，结束自检
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
g_ptz.hori_angle_state = 1;
|
||
g_ptz.vert_angle_state = 1;
|
||
g_ptz.no_self_check_state = 1;//读取成功
|
||
|
||
}else{//掉电数据读取失败，赋默认值，结束自检
|
||
g_ptz.no_self_check_state = 0;//读取失败
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.hori_as5047d.as5047d_state = 1;
|
||
g_ptz.hori_angle_state = 0;//掉电参数读取失败，角度状态先置故障
|
||
g_ptz.vert_angle_state = 0;
|
||
}
|
||
return;
|
||
}
|
||
|
||
}else{
|
||
error_conut_state = 1;//开始进行误差计算
|
||
//自检参数失败进行全范围自检
|
||
g_ptz.hori_angle_state = 0;
|
||
g_ptz.vert_angle_state = 0;
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_COMPLETE_STEP;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_COMPLETE_STEP;
|
||
return;
|
||
}
|
||
}else{//任何情况下都不自检
|
||
error_conut_state = 0;//不进行误差计算
|
||
if(ptz_self_check_data_read() == 1)
|
||
{//自检参数读取成功
|
||
if(ptz_power_off_data_read() == 1)
|
||
{//掉电数据读取成功
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
g_ptz.hori_angle_state = 1;
|
||
g_ptz.vert_angle_state = 1;
|
||
g_ptz.no_self_check_state = 1;//读取成功
|
||
return;
|
||
}else{//掉电数据读取失败
|
||
g_ptz.no_self_check_state = 0;//读取失败
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_END;
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_END;
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num / 2;
|
||
g_ptz.hori_as5047d.as5047d_state = 1;
|
||
|
||
g_ptz.hori_angle_state = 0;//角度状态先置故障
|
||
g_ptz.vert_angle_state = 0;
|
||
return;
|
||
}
|
||
|
||
}else{//自检参数读取失败
|
||
g_ptz.no_self_check_state = 0;//读取失败
|
||
g_ptz.vert_self_check = PTZ_VERT_SELF_CHECK_DATA_SAVE;
|
||
/* 自检参数读取失败，重新赋值经验值进行保存 */
|
||
//中型蜗轮蜗杆步进电机云台
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_max = 9600.0;
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_K = 1.0/80;
|
||
g_ptz.vert_as5047d.as5047d_remain_angle = 230.0;
|
||
g_ptz.vert_as5047d.as5047d_cycle_num = 26;
|
||
|
||
g_ptz.vert_angleP.angle_min = -60.0;//云台角度最小值
|
||
g_ptz.vert_angleP.angle_max = 60.0;//云台角度最大值
|
||
g_ptz.vert_angleP.angle_allow_min = -60.0;//云台可用角度最小值
|
||
g_ptz.vert_angleP.angle_allow_max = 60.0;//云台可用角度最大值
|
||
g_ptz.vert_angleP.angle_range = 120.0;//云台角度范围
|
||
|
||
g_ptz.vert_as5047d.as5047d_state = 1;
|
||
|
||
g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_DATA_SAVE;
|
||
/* 自检参数读取失败，重新赋值经验值进行保存 */
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_max = 28800.0;
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_K = 1.0/80;
|
||
g_ptz.hori_as5047d.as5047d_remain_angle = 0.0;
|
||
g_ptz.hori_as5047d.as5047d_cycle_num = 80;
|
||
|
||
g_ptz.hori_angleP.angle_min = 0.0;//云台角度最小值
|
||
g_ptz.hori_angleP.angle_max = 360.0;//云台角度最大值
|
||
g_ptz.hori_angleP.angle_allow_min = 0.0;//云台可用角度最小值
|
||
g_ptz.hori_angleP.angle_allow_max = 360.0;//云台可用角度最大值
|
||
g_ptz.hori_angleP.angle_range = 360.0;//云台角度范围
|
||
|
||
g_ptz.hori_as5047d.as5047d_state = 1;
|
||
|
||
g_ptz.hori_angle_state = 0;//角度状态先置故障
|
||
g_ptz.vert_angle_state = 0;
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
//自检任务初始化
|
||
void init_self_check_module(void)
|
||
{
|
||
// g_ptz.hori_self_check = 255;
|
||
// g_ptz.vert_self_check = 255;
|
||
|
||
ptz_self_check_data_init();
|
||
// init_error_count_task();//误差计算初始化
|
||
creat_task_hori_self_check();
|
||
creat_task_vert_self_check();
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
//计算效验码
|
||
unsigned int ptz_self_check_data_crc(PtzSelfCheckData data)
|
||
{
|
||
float crc = 0;
|
||
unsigned int crc1 = 0;
|
||
crc = crc + data.hori_as5047d.as5047d_ptz_angle_max;
|
||
crc = crc + data.hori_as5047d.as5047d_ptz_angle_K;
|
||
crc = crc + data.hori_as5047d.as5047d_remain_angle;
|
||
crc = crc + data.hori_as5047d.as5047d_cycle_num;
|
||
|
||
crc = crc + data.vert_as5047d.as5047d_ptz_angle_max;
|
||
crc = crc + data.vert_as5047d.as5047d_ptz_angle_K;
|
||
crc = crc + data.vert_as5047d.as5047d_remain_angle;
|
||
crc = crc + data.vert_as5047d.as5047d_cycle_num;
|
||
|
||
crc = crc + data.hori_angleP.angle_min;//云台角度最小值
|
||
crc = crc + data.hori_angleP.angle_max;//云台角度最大值
|
||
crc = crc + data.hori_angleP.angle_allow_min;//云台可用角度最小值
|
||
crc = crc + data.hori_angleP.angle_allow_max;//云台可用角度最大值
|
||
crc = crc + data.hori_angleP.angle_range;//云台角度范围
|
||
|
||
crc = crc + data.vert_angleP.angle_min;//云台角度最小值
|
||
crc = crc + data.vert_angleP.angle_max;//云台角度最大值
|
||
crc = crc + data.vert_angleP.angle_allow_min;//云台可用角度最小值
|
||
crc = crc + data.vert_angleP.angle_allow_max;//云台可用角度最大值
|
||
crc = crc + data.vert_angleP.angle_range;//云台角度范围
|
||
|
||
crc = crc + data.hori_angle_error;//水平结构误差
|
||
crc1 = (unsigned int)crc;
|
||
return crc1;
|
||
}
|
||
|
||
static PtzSelfCheckData self_check_data;
|
||
static PtzSelfCheckData self_check_data_a;
|
||
//保存自检参数
|
||
char ptz_self_check_data_save()
|
||
{
|
||
char i,j;
|
||
//复制数据
|
||
memcpy(&self_check_data.hori_as5047d, &g_ptz.hori_as5047d, sizeof(g_ptz.hori_as5047d));
|
||
memcpy(&self_check_data.hori_angleP, &g_ptz.hori_angleP, sizeof(g_ptz.hori_angleP));
|
||
memcpy(&self_check_data.vert_as5047d, &g_ptz.vert_as5047d, sizeof(g_ptz.vert_as5047d));
|
||
memcpy(&self_check_data.vert_angleP, &g_ptz.vert_angleP, sizeof(g_ptz.vert_angleP));
|
||
|
||
self_check_data.hori_angle_error = g_ptz.hori_angle_error;//存储水平结构误差
|
||
|
||
//计算效验码
|
||
self_check_data.crc = ptz_self_check_data_crc(self_check_data);
|
||
|
||
//写入主存储区
|
||
for(i = 0; i < SELF_CHECK_DATA_SAVE_FLASH_NUM; i++)
|
||
{
|
||
memset(&self_check_data_a, 0, sizeof(self_check_data_a));
|
||
write_many_data(sizeof(self_check_data),(unsigned char *)(&self_check_data),SELF_CHECK_DATA_FLASH_ADD);
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
Flash_Read((unsigned char *)(&self_check_data_a),SELF_CHECK_DATA_FLASH_ADD,sizeof(self_check_data_a));
|
||
if(memcmp(&self_check_data, &self_check_data_a, sizeof(self_check_data)) == 0)//判断写入数据是否正确
|
||
{
|
||
break;
|
||
}
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
}
|
||
//写入备份存储区
|
||
for(j = 0; j < SELF_CHECK_DATA_SAVE_FLASH_NUM; j++)
|
||
{
|
||
memset(&self_check_data_a, 0, sizeof(self_check_data_a));
|
||
write_many_data(sizeof(self_check_data),(unsigned char *)(&self_check_data),SELF_CHECK_DATA_BACKUP_FLASH_ADD);
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
Flash_Read((unsigned char *)(&self_check_data_a),SELF_CHECK_DATA_BACKUP_FLASH_ADD,sizeof(self_check_data_a));
|
||
if(memcmp(&self_check_data, &self_check_data_a, sizeof(self_check_data)) == 0)//判断写入数据是否正确
|
||
{
|
||
break;
|
||
}
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
}
|
||
|
||
if(i < SELF_CHECK_DATA_SAVE_FLASH_NUM || j < SELF_CHECK_DATA_SAVE_FLASH_NUM)//判断写入数据是否成功
|
||
{
|
||
//写入成功
|
||
return 1;
|
||
}
|
||
//写入失败
|
||
return 0;
|
||
}
|
||
|
||
//自检参数读取
|
||
char ptz_self_check_data_read()
|
||
{
|
||
char i,j;
|
||
//首先从主存储区读取数据
|
||
for(i = 0; i < SELF_CHECK_DATA_READ_FLASH_NUM; i++)
|
||
{
|
||
memset(&self_check_data, 0, sizeof(self_check_data));
|
||
Flash_Read((unsigned char *)(&self_check_data),SELF_CHECK_DATA_FLASH_ADD,sizeof(self_check_data));
|
||
if(self_check_data.crc == ptz_self_check_data_crc(self_check_data)
|
||
&& self_check_data.crc > 0)
|
||
{
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_max = self_check_data.hori_as5047d.as5047d_ptz_angle_max;
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_K = self_check_data.hori_as5047d.as5047d_ptz_angle_K;
|
||
g_ptz.hori_as5047d.as5047d_remain_angle = self_check_data.hori_as5047d.as5047d_remain_angle;
|
||
g_ptz.hori_as5047d.as5047d_cycle_num = self_check_data.hori_as5047d.as5047d_cycle_num;
|
||
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_max = self_check_data.vert_as5047d.as5047d_ptz_angle_max;
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_K = self_check_data.vert_as5047d.as5047d_ptz_angle_K;
|
||
g_ptz.vert_as5047d.as5047d_remain_angle = self_check_data.vert_as5047d.as5047d_remain_angle;
|
||
g_ptz.vert_as5047d.as5047d_cycle_num = self_check_data.vert_as5047d.as5047d_cycle_num;
|
||
|
||
g_ptz.hori_angleP.angle_min = self_check_data.hori_angleP.angle_min;//云台角度最小值
|
||
g_ptz.hori_angleP.angle_max = self_check_data.hori_angleP.angle_max;//云台角度最大值
|
||
g_ptz.hori_angleP.angle_allow_min = self_check_data.hori_angleP.angle_allow_min;//云台可用角度最小值
|
||
g_ptz.hori_angleP.angle_allow_max = self_check_data.hori_angleP.angle_allow_max;//云台可用角度最大值
|
||
g_ptz.hori_angleP.angle_range = self_check_data.hori_angleP.angle_range;//云台角度范围
|
||
|
||
g_ptz.vert_angleP.angle_min = self_check_data.vert_angleP.angle_min;//云台角度最小值
|
||
g_ptz.vert_angleP.angle_max = self_check_data.vert_angleP.angle_max;//云台角度最大值
|
||
g_ptz.vert_angleP.angle_allow_min = self_check_data.vert_angleP.angle_allow_min;//云台可用角度最小值
|
||
g_ptz.vert_angleP.angle_allow_max = self_check_data.vert_angleP.angle_allow_max;//云台可用角度最大值
|
||
g_ptz.vert_angleP.angle_range = self_check_data.vert_angleP.angle_range;//云台角度范围
|
||
|
||
g_ptz.hori_angle_error = self_check_data.hori_angle_error;//赋值水平结构误差
|
||
return 1;
|
||
}
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
|
||
}
|
||
//如果主存储区读取的数据是错误的，则从备份存储区读取数据
|
||
for(j = 0; j < SELF_CHECK_DATA_READ_FLASH_NUM; j++)
|
||
{
|
||
memset(&self_check_data, 0, sizeof(self_check_data));
|
||
Flash_Read((unsigned char *)(&self_check_data),SELF_CHECK_DATA_BACKUP_FLASH_ADD,sizeof(self_check_data));
|
||
if(self_check_data.crc == ptz_self_check_data_crc(self_check_data)
|
||
&& self_check_data.crc > 0)
|
||
{
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_max = self_check_data.hori_as5047d.as5047d_ptz_angle_max;
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_K = self_check_data.hori_as5047d.as5047d_ptz_angle_K;
|
||
g_ptz.hori_as5047d.as5047d_remain_angle = self_check_data.hori_as5047d.as5047d_remain_angle;
|
||
g_ptz.hori_as5047d.as5047d_cycle_num = self_check_data.hori_as5047d.as5047d_cycle_num;
|
||
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_max = self_check_data.vert_as5047d.as5047d_ptz_angle_max;
|
||
g_ptz.vert_as5047d.as5047d_ptz_angle_K = self_check_data.vert_as5047d.as5047d_ptz_angle_K;
|
||
g_ptz.vert_as5047d.as5047d_remain_angle = self_check_data.vert_as5047d.as5047d_remain_angle;
|
||
g_ptz.vert_as5047d.as5047d_cycle_num = self_check_data.vert_as5047d.as5047d_cycle_num;
|
||
|
||
g_ptz.hori_angleP.angle_min = self_check_data.hori_angleP.angle_min;//云台角度最小值
|
||
g_ptz.hori_angleP.angle_max = self_check_data.hori_angleP.angle_max;//云台角度最大值
|
||
g_ptz.hori_angleP.angle_allow_min = self_check_data.hori_angleP.angle_allow_min;//云台可用角度最小值
|
||
g_ptz.hori_angleP.angle_allow_max = self_check_data.hori_angleP.angle_allow_max;//云台可用角度最大值
|
||
g_ptz.hori_angleP.angle_range = self_check_data.hori_angleP.angle_range;//云台角度范围
|
||
|
||
g_ptz.vert_angleP.angle_min = self_check_data.vert_angleP.angle_min;//云台角度最小值
|
||
g_ptz.vert_angleP.angle_max = self_check_data.vert_angleP.angle_max;//云台角度最大值
|
||
g_ptz.vert_angleP.angle_allow_min = self_check_data.vert_angleP.angle_allow_min;//云台可用角度最小值
|
||
g_ptz.vert_angleP.angle_allow_max = self_check_data.vert_angleP.angle_allow_max;//云台可用角度最大值
|
||
g_ptz.vert_angleP.angle_range = self_check_data.vert_angleP.angle_range;//云台角度范围
|
||
|
||
g_ptz.hori_angle_error = self_check_data.hori_angle_error;//赋值水平结构误差
|
||
return 1;
|
||
}
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
|
||
}
|
||
return 0;
|
||
}
|
||
|
||
//擦除自检数据
|
||
char ptz_self_check_data_erase()
|
||
{
|
||
char i, j;
|
||
memset(&self_check_data_a, 0, sizeof(self_check_data_a));
|
||
memset(&self_check_data, 0, sizeof(self_check_data));
|
||
|
||
#ifdef PTZ_NO_SELF_CHECK
|
||
ptz_power_off_data_erase(PTZ_MB85RC64_ADD_A);
|
||
ptz_power_off_data_erase(PTZ_MB85RC64_ADD_B);
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
#endif
|
||
|
||
//擦除主存储区
|
||
for(i = 0; i < SELF_CHECK_DATA_SAVE_FLASH_NUM; i++)
|
||
{
|
||
memset(&self_check_data_a, 0, sizeof(self_check_data_a));
|
||
write_many_data(sizeof(self_check_data),(unsigned char *)(&self_check_data),SELF_CHECK_DATA_FLASH_ADD);
|
||
asm("nop");
|
||
asm("nop");
|
||
Flash_Read((unsigned char *)(&self_check_data_a),SELF_CHECK_DATA_FLASH_ADD,sizeof(self_check_data_a));
|
||
if(memcmp(&self_check_data, &self_check_data_a, sizeof(self_check_data)) == 0)//判断擦除数据是否正确
|
||
{
|
||
break;
|
||
}
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
}
|
||
//擦除备份存储区
|
||
for(j = 0; j < SELF_CHECK_DATA_SAVE_FLASH_NUM; j++)
|
||
{
|
||
memset(&self_check_data_a, 0, sizeof(self_check_data_a));
|
||
write_many_data(sizeof(self_check_data),(unsigned char *)(&self_check_data),SELF_CHECK_DATA_BACKUP_FLASH_ADD);
|
||
asm("nop");
|
||
asm("nop");
|
||
Flash_Read((unsigned char *)(&self_check_data_a),SELF_CHECK_DATA_BACKUP_FLASH_ADD,sizeof(self_check_data_a));
|
||
if(memcmp(&self_check_data, &self_check_data_a, sizeof(self_check_data)) == 0)//判断擦除数据是否正确
|
||
{
|
||
break;
|
||
}
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
}
|
||
|
||
if(i < SELF_CHECK_DATA_SAVE_FLASH_NUM || j < SELF_CHECK_DATA_SAVE_FLASH_NUM)//判断擦除数据是否成功
|
||
{
|
||
//擦除成功
|
||
return 1;
|
||
}
|
||
//擦除失败
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|