1011 lines
32 KiB
C
1011 lines
32 KiB
C
|
#include "gd32f4xx.h"
|
|||
|
#include "ptz_header_file.h"
|
|||
|
#include "math.h"
|
|||
|
#include "ptz_default_value.h"
|
|||
|
#include "ptz_type_select.h"
|
|||
|
#include "l6235d.h"
|
|||
|
#include "service_presetbitscan.h"
|
|||
|
|
|||
|
#ifdef PTZ_BLDC_MOTOR
|
|||
|
|
|||
|
static BSP_OS_SEM ptz_hori_stop_mutex;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4>
|
|||
|
static BSP_OS_SEM ptz_vert_stop_mutex;
|
|||
|
static char ptz_hori_stop_count;//ˮƽֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ
|
|||
|
static char ptz_vert_stop_count;//<2F><>ֱֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ
|
|||
|
|
|||
|
void ptz_sem_post_stop_mutex()//<2F>ͷ<EFBFBD><CDB7><EFBFBD>̨<EFBFBD><CCA8>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4>
|
|||
|
{
|
|||
|
BSP_OS_SemPost(&ptz_hori_stop_mutex);
|
|||
|
BSP_OS_SemPost(&ptz_vert_stop_mutex);
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void ptz_hori_start(char direction, float speed)
|
|||
|
{
|
|||
|
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
|
|||
|
ptz_hori_pid_clear_zero();
|
|||
|
//<2F>趨ת<E8B6A8><D7AA><EFBFBD>ٶ<EFBFBD>
|
|||
|
g_ptz.hori_speed_set = speed;
|
|||
|
//<2F>趨ת<E8B6A8><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.hori_direction_last = g_ptz.hori_direction_set;
|
|||
|
g_ptz.hori_direction_set = direction;
|
|||
|
g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.hori_start_stop_set = PTZ_HORI_START;
|
|||
|
g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
|
|||
|
|
|||
|
#ifdef PTZ_MOTOR_CONTROL_L6235D
|
|||
|
l6235d_hori_set_direction(direction);
|
|||
|
l6235d_hori_start_stop(PTZ_HORI_START);
|
|||
|
#endif
|
|||
|
|
|||
|
ptz_hori_stop_count = 0;
|
|||
|
#ifdef PTZ_ELECTRIC_STABLE_L6235D
|
|||
|
g_ptz.hori_electric_stable.stable_stop_switch = 0;
|
|||
|
#endif
|
|||
|
BSP_OS_SemPost(&ptz_hori_stop_mutex);
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void ptz_hori_stop(unsigned short int time)
|
|||
|
{
|
|||
|
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
|
|||
|
//ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
|
|||
|
g_ptz.hori_start_stop_set = PTZ_HORI_STOP;
|
|||
|
g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
|
|||
|
#ifdef PTZ_MOTOR_CONTROL_L6235D
|
|||
|
l6235d_hori_start_stop(PTZ_HORI_STOP);
|
|||
|
#endif
|
|||
|
|
|||
|
//PID<49><44>0
|
|||
|
ptz_hori_pid_clear_zero();
|
|||
|
//<2F>趨ת<E8B6A8><D7AA><EFBFBD>ٶ<EFBFBD>
|
|||
|
g_ptz.hori_speed_set = 0;
|
|||
|
g_ptz.hori_speed_actual = 0;
|
|||
|
|
|||
|
if(ptz_hori_stop_count <= 0)
|
|||
|
{
|
|||
|
OSTimeDlyHMSM(0u, 0u, 0u, time);
|
|||
|
ptz_hori_stop_count = 0;
|
|||
|
}
|
|||
|
ptz_hori_stop_count ++;
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD>
|
|||
|
#ifdef PTZ_ELECTRIC_STABLE_L6235D
|
|||
|
ptz_hori_electric_stable_init();
|
|||
|
#endif
|
|||
|
BSP_OS_SemPost(&ptz_hori_stop_mutex);
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void ptz_vert_start(char direction, float speed)
|
|||
|
{
|
|||
|
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
|
|||
|
ptz_vert_pid_clear_zero();
|
|||
|
//<2F>趨ת<E8B6A8><D7AA><EFBFBD>ٶ<EFBFBD>
|
|||
|
g_ptz.vert_speed_set = speed;
|
|||
|
//<2F>趨ת<E8B6A8><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.vert_direction_last = g_ptz.vert_direction_set;
|
|||
|
g_ptz.vert_direction_set = direction;
|
|||
|
g_ptz.vert_direction_actual = g_ptz.vert_direction_set;
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.vert_start_stop_set = PTZ_VERT_START;
|
|||
|
g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set;
|
|||
|
#ifdef PTZ_MOTOR_CONTROL_L6235D
|
|||
|
l6235d_vert_set_direction(direction);
|
|||
|
l6235d_vert_start_stop(PTZ_VERT_START);
|
|||
|
#endif
|
|||
|
|
|||
|
ptz_vert_stop_count = 0;
|
|||
|
#ifdef PTZ_ELECTRIC_STABLE_L6235D
|
|||
|
g_ptz.vert_electric_stable.stable_stop_switch = 0;
|
|||
|
#endif
|
|||
|
BSP_OS_SemPost(&ptz_vert_stop_mutex);
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void ptz_vert_stop(unsigned short int time)
|
|||
|
{
|
|||
|
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
|
|||
|
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
|
|||
|
{
|
|||
|
//ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
|
|||
|
g_ptz.vert_start_stop_set = PTZ_VERT_STOP;
|
|||
|
g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set;
|
|||
|
|
|||
|
#ifdef PTZ_MOTOR_CONTROL_L6235D
|
|||
|
l6235d_vert_start_stop(PTZ_VERT_STOP);
|
|||
|
#endif
|
|||
|
|
|||
|
//PID<49><44>0
|
|||
|
ptz_vert_pid_clear_zero();
|
|||
|
//<2F>趨ת<E8B6A8><D7AA><EFBFBD>ٶ<EFBFBD>
|
|||
|
g_ptz.vert_speed_set = 0;
|
|||
|
g_ptz.vert_speed_actual = 0;
|
|||
|
|
|||
|
if(ptz_vert_stop_count <= 0)
|
|||
|
{
|
|||
|
OSTimeDlyHMSM(0u, 0u, 0u, time);
|
|||
|
ptz_vert_stop_count = 0;
|
|||
|
}
|
|||
|
ptz_vert_stop_count ++;
|
|||
|
//<2F><>̨<EFBFBD><CCA8><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD>
|
|||
|
#ifdef PTZ_ELECTRIC_STABLE_L6235D
|
|||
|
ptz_vert_electric_stable_init();
|
|||
|
#endif
|
|||
|
}
|
|||
|
|
|||
|
BSP_OS_SemPost(&ptz_vert_stop_mutex);
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
|
|||
|
/// <20><>̨ˮƽת<C6BD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
static char ptz_hori_rotate_monitor_task()
|
|||
|
{
|
|||
|
static float BNP_D;//ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>뵱ǰλ<C7B0>õľ<C3B5><C4BE><EFBFBD>
|
|||
|
static float BFP_D;//ɲ<><C9B2><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD>뵱ǰλ<C7B0>õľ<C3B5><C4BE><EFBFBD>
|
|||
|
static unsigned int k;
|
|||
|
switch(g_ptz.hori_rotate_monitor_switch)
|
|||
|
{
|
|||
|
case 0://<2F>ر<EFBFBD>
|
|||
|
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_HORI_RIGHT_KEEP://<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
|
|||
|
//<2F>ر<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.hori_rotate_monitor_switch = 0;
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD>̨<EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
|||
|
{
|
|||
|
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
|
|||
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
}
|
|||
|
else//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
|
|||
|
{
|
|||
|
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
|
|||
|
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.hori_direction_set = PTZ_HORI_DIR_RIGHT;
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set);
|
|||
|
}
|
|||
|
}
|
|||
|
else//û<><C3BB>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set);
|
|||
|
}
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_HORI_LEFT_KEEP://<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
|
|||
|
//<2F>ر<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.hori_rotate_monitor_switch = 0;
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD>̨<EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
|||
|
{
|
|||
|
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
|
|||
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
}
|
|||
|
else//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
|
|||
|
{
|
|||
|
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
|
|||
|
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.hori_direction_set = PTZ_HORI_DIR_LEFT;
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set);
|
|||
|
}
|
|||
|
}
|
|||
|
else//û<><C3BB>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set);
|
|||
|
}
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_HORI_GO_TO_ANGLE_A://<2F>Ƕȶ<C7B6>λ
|
|||
|
case PTZ_HORI_MIN_DISTANCE:
|
|||
|
case PTZ_HORI_MAX_DISTANCE:
|
|||
|
case PTZ_HORI_RIGHT_ANGLE:
|
|||
|
case PTZ_HORI_LEFT_ANGLE:
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD>Ҫת<D2AA><D7AA>,<2C><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ0<CEAA><30><EFBFBD><EFBFBD>ͬһ<CDAC><D2BB>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA>
|
|||
|
if(g_ptz.hori_rotate_plan.rotate_switch == 1)
|
|||
|
{
|
|||
|
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǵ<EFBFBD><C7B4><EFBFBD>ͬһ<CDAC><D2BB>λ<EFBFBD><CEBB>
|
|||
|
//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
|||
|
{
|
|||
|
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
|
|||
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
|
|||
|
if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
}
|
|||
|
else//ת<><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
|
|||
|
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
|
|||
|
}
|
|||
|
}
|
|||
|
else//û<><C3BB>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
|
|||
|
}
|
|||
|
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
|
|||
|
{
|
|||
|
if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
|
|||
|
{
|
|||
|
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
else
|
|||
|
{
|
|||
|
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
}
|
|||
|
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
|
|||
|
{
|
|||
|
if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
|
|||
|
{
|
|||
|
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
else
|
|||
|
{
|
|||
|
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
}
|
|||
|
else//g_ptz.hori_rotate_plan.rotate_switch == 0;
|
|||
|
{
|
|||
|
g_ptz.hori_rotate_monitor_switch = 0;
|
|||
|
//<2F><><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>ֹͣ<CDA3><D6B9><EFBFBD>رռ<D8B1><D5BC><EFBFBD>
|
|||
|
g_ptz.hori_arrive_flag = 0;//ת<><D7AA><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB>
|
|||
|
ptz_location_return_return(LOCATION_HORI);//<2F><>λ<EFBFBD>ش<EFBFBD>
|
|||
|
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
|
|||
|
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
|||
|
{
|
|||
|
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
|||
|
}
|
|||
|
}
|
|||
|
break;
|
|||
|
|
|||
|
|
|||
|
case PTZ_HORI_RIGHT_CYCLE:
|
|||
|
case PTZ_HORI_LEFT_CYCLE:
|
|||
|
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
|||
|
{
|
|||
|
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
|
|||
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
|
|||
|
if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
}
|
|||
|
else//ת<><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
|
|||
|
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
|
|||
|
}
|
|||
|
}
|
|||
|
else//û<><C3BB>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
|
|||
|
}
|
|||
|
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_CYCLE;
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_HORI_CYCLE:
|
|||
|
BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle);
|
|||
|
if(BNP_D > 360.0 / 2.0)
|
|||
|
{
|
|||
|
BNP_D = 360.0 - BNP_D;
|
|||
|
}
|
|||
|
BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle);
|
|||
|
if(BFP_D > 360.0 / 2.0)
|
|||
|
{
|
|||
|
BFP_D = 360.0 - BFP_D;
|
|||
|
}
|
|||
|
/*<2A><>λ*/
|
|||
|
if(BNP_D + BFP_D > (g_ptz.hori_rotate_plan.stop_angle_range + 2.0))
|
|||
|
{
|
|||
|
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
|
|||
|
}
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_HORI_GO_TO_ANGLE_B:
|
|||
|
BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle);
|
|||
|
if(BNP_D > 360.0 / 2.0)
|
|||
|
{
|
|||
|
BNP_D = 360.0 - BNP_D;
|
|||
|
}
|
|||
|
BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle);
|
|||
|
if(BFP_D > 360.0 / 2.0)
|
|||
|
{
|
|||
|
BFP_D = 360.0 - BFP_D;
|
|||
|
}
|
|||
|
/*<2A><>λ*/
|
|||
|
if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01))
|
|||
|
{
|
|||
|
if(g_ptz.hori_repeat_locate_switch == 0)//<2F>ظ<EFBFBD><D8B8><EFBFBD>λ<EFBFBD>ر<EFBFBD>
|
|||
|
{
|
|||
|
g_ptz.hori_arrive_flag = 0;//ת<><D7AA>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB>
|
|||
|
ptz_location_return_return(LOCATION_HORI);//<2F><>λ<EFBFBD>ش<EFBFBD>
|
|||
|
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
|
|||
|
}
|
|||
|
g_ptz.hori_rotate_monitor_switch = 0;
|
|||
|
if(g_ptz.hori_repeat_locate_switch == 1)//<2F>ظ<EFBFBD><D8B8><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
|
|||
|
{
|
|||
|
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_A;
|
|||
|
}
|
|||
|
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
|||
|
}
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
|
|||
|
{
|
|||
|
if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
|
|||
|
{
|
|||
|
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
else
|
|||
|
{
|
|||
|
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
}
|
|||
|
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
|
|||
|
{
|
|||
|
if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
|
|||
|
{
|
|||
|
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
else
|
|||
|
{
|
|||
|
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
}
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_HORI_BRAKE://ֹͣת<D6B9><D7AA>
|
|||
|
g_ptz.hori_rotate_monitor_switch = 0;
|
|||
|
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_HORI_DEC_BRAKE_A://<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_DEC_BRAKE_B;
|
|||
|
k = 0;
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_HORI_DEC_BRAKE_B:
|
|||
|
k++;//K<><4B><EFBFBD><EFBFBD>1<EFBFBD><31>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1ms
|
|||
|
if(g_ptz.hori_speed_actual <= PTZ_HORI_MIN_SPEED * 1.5 ||
|
|||
|
k >= 1400)
|
|||
|
{
|
|||
|
k = 0;
|
|||
|
g_ptz.hori_rotate_monitor_switch = 0;
|
|||
|
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
|||
|
}
|
|||
|
|
|||
|
break;
|
|||
|
|
|||
|
//<2F>ظ<EFBFBD><D8B8><EFBFBD>λ·<CEBB><C2B7><EFBFBD>滮
|
|||
|
case PTZ_HORI_REPEAT_LOCATE_A:
|
|||
|
ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
|
|||
|
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_B;
|
|||
|
break;
|
|||
|
//<2F>ظ<EFBFBD><D8B8><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
case PTZ_HORI_REPEAT_LOCATE_B:
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD>Ҫת<D2AA><D7AA>,<2C><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ0<CEAA><30><EFBFBD><EFBFBD>ͬһ<CDAC><D2BB>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA>
|
|||
|
if(g_ptz.hori_rotate_plan.rotate_switch == 1)
|
|||
|
{
|
|||
|
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_C;
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǵ<EFBFBD><C7B4><EFBFBD>ͬһ<CDAC><D2BB>λ<EFBFBD><CEBB>
|
|||
|
//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
|||
|
{
|
|||
|
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
|
|||
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
|
|||
|
if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
}
|
|||
|
else//ת<><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
|
|||
|
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
|
|||
|
}
|
|||
|
}
|
|||
|
else//û<><C3BB>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
|
|||
|
}
|
|||
|
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
|
|||
|
{
|
|||
|
if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
|
|||
|
{
|
|||
|
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
else
|
|||
|
{
|
|||
|
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
}
|
|||
|
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
|
|||
|
{
|
|||
|
if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
|
|||
|
{
|
|||
|
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
else
|
|||
|
{
|
|||
|
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
}
|
|||
|
else//g_ptz.hori_rotate_plan.rotate_switch == 0;
|
|||
|
{
|
|||
|
g_ptz.hori_rotate_monitor_switch = 0;
|
|||
|
//<2F><><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>ֹͣ<CDA3><D6B9><EFBFBD>رռ<D8B1><D5BC><EFBFBD>
|
|||
|
g_ptz.hori_arrive_flag = 0;//ת<><D7AA>ָ<EFBFBD><D6B8>Ԥλ<D4A4><CEBB>
|
|||
|
ptz_location_return_return(LOCATION_HORI);//<2F><>λ<EFBFBD>ش<EFBFBD>
|
|||
|
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
|
|||
|
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
|||
|
{
|
|||
|
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
|||
|
}
|
|||
|
}
|
|||
|
break;
|
|||
|
//<2F>ظ<EFBFBD><D8B8><EFBFBD>λ<EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7>ﵽ<EFBFBD><EFB5BD>λλ<CEBB><CEBB>
|
|||
|
case PTZ_HORI_REPEAT_LOCATE_C:
|
|||
|
BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle);
|
|||
|
if(BNP_D > 360.0 / 2.0)
|
|||
|
{
|
|||
|
BNP_D = 360.0 - BNP_D;
|
|||
|
}
|
|||
|
BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle);
|
|||
|
if(BFP_D > 360.0 / 2.0)
|
|||
|
{
|
|||
|
BFP_D = 360.0 - BFP_D;
|
|||
|
}
|
|||
|
/*<2A><>λ*/
|
|||
|
if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01))
|
|||
|
{
|
|||
|
g_ptz.hori_arrive_flag = 0;//ת<><D7AA>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB>
|
|||
|
ptz_location_return_return(LOCATION_HORI);//<2F><>λ<EFBFBD>ش<EFBFBD>
|
|||
|
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
|
|||
|
g_ptz.hori_rotate_monitor_switch = 0;
|
|||
|
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
|||
|
}
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
|
|||
|
{
|
|||
|
if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
|
|||
|
{
|
|||
|
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
else
|
|||
|
{
|
|||
|
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
}
|
|||
|
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
|
|||
|
{
|
|||
|
if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
|
|||
|
{
|
|||
|
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
else
|
|||
|
{
|
|||
|
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
}
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_HORI_RIGHT_ANGLE_INC:
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD>Ҫת<D2AA><D7AA>,<2C><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ0<CEAA><30><EFBFBD><EFBFBD>ͬһ<CDAC><D2BB>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA>
|
|||
|
if(g_ptz.hori_rotate_plan.rotate_switch == 1)
|
|||
|
{
|
|||
|
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǵ<EFBFBD><C7B4><EFBFBD>ͬһ<CDAC><D2BB>λ<EFBFBD><CEBB>
|
|||
|
//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
|||
|
{
|
|||
|
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
|
|||
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
|
|||
|
if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
}
|
|||
|
else//ת<><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
|
|||
|
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
|
|||
|
}
|
|||
|
}
|
|||
|
else//û<><C3BB>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
|
|||
|
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
|||
|
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
|
|||
|
}
|
|||
|
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
|
|||
|
{
|
|||
|
if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
|
|||
|
{
|
|||
|
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
else
|
|||
|
{
|
|||
|
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
}
|
|||
|
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
|
|||
|
{
|
|||
|
if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
|
|||
|
{
|
|||
|
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
else
|
|||
|
{
|
|||
|
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_HORI_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
|||
|
}
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
}
|
|||
|
else//g_ptz.hori_rotate_plan.rotate_switch == 0;
|
|||
|
{
|
|||
|
g_ptz.hori_rotate_monitor_switch = 0;
|
|||
|
//<2F><><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>ֹͣ<CDA3><D6B9><EFBFBD>رռ<D8B1><D5BC><EFBFBD>
|
|||
|
g_ptz.hori_arrive_flag = 0;//ת<><D7AA><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB>
|
|||
|
ptz_location_return_return(LOCATION_HORI);//<2F><>λ<EFBFBD>ش<EFBFBD>
|
|||
|
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
|
|||
|
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
|||
|
{
|
|||
|
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
|||
|
}
|
|||
|
}
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_HORI_LEFT_ANGLE_INC:
|
|||
|
|
|||
|
break;
|
|||
|
|
|||
|
}
|
|||
|
return 1;
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
/// <20><>̨<EFBFBD><CCA8>ֱת<D6B1><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
static char ptz_vert_rotate_monitor_task()
|
|||
|
{
|
|||
|
static float BNP_D;//ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>뵱ǰλ<C7B0>õľ<C3B5><C4BE><EFBFBD>
|
|||
|
static float BFP_D;//ɲ<><C9B2><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD>뵱ǰλ<C7B0>õľ<C3B5><C4BE><EFBFBD>
|
|||
|
static char cmd_type;
|
|||
|
static unsigned int k;
|
|||
|
switch(g_ptz.vert_rotate_monitor_switch)
|
|||
|
{
|
|||
|
case 0://<2F>ر<EFBFBD>
|
|||
|
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_VERT_GO_TO_ANGLE_A://ת<><D7AA><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
case PTZ_VERT_ANGLE:
|
|||
|
case PTZ_VERT_UP_KEEP:
|
|||
|
case PTZ_VERT_DOWN_KEEP:
|
|||
|
cmd_type = g_ptz.vert_rotate_monitor_switch;
|
|||
|
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_B;
|
|||
|
if(g_ptz.vert_rotate_plan.rotate_switch == 1)
|
|||
|
{
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
|
|||
|
{
|
|||
|
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
|
|||
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
|
|||
|
if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction)
|
|||
|
{
|
|||
|
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
|
|||
|
}
|
|||
|
else//ת<><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
|
|||
|
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
|
|||
|
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
|
|||
|
ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
|
|||
|
}
|
|||
|
}
|
|||
|
else//û<><C3BB>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
|
|||
|
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
|
|||
|
ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
|
|||
|
}
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
|
|||
|
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_VERT_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
|
|||
|
}
|
|||
|
|
|||
|
}
|
|||
|
else//g_ptz.vert_rotate_plan.rotate_switch == 0
|
|||
|
{
|
|||
|
//ɲ<><C9B2><EFBFBD>ر<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
g_ptz.vert_arrive_flag = 0;//ת<><D7AA>ָ<EFBFBD><D6B8>Ԥλ<D4A4><CEBB>
|
|||
|
ptz_location_return_return(LOCATION_VERT);//<2F><>λ<EFBFBD>ش<EFBFBD>
|
|||
|
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
|
|||
|
g_ptz.vert_rotate_monitor_switch = 0;
|
|||
|
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
|
|||
|
{
|
|||
|
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
|||
|
}
|
|||
|
}
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_VERT_GO_TO_ANGLE_B://ת<><D7AA><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD>Ƕȶ<C7B6>λ
|
|||
|
BNP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle);
|
|||
|
BFP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_far_angle);
|
|||
|
/*<2A><>λ*/
|
|||
|
if(BNP_D + BFP_D <= (g_ptz.vert_rotate_plan.stop_angle_range + 0.01))
|
|||
|
{
|
|||
|
if(g_ptz.vert_repeat_locate_switch == 0 &&
|
|||
|
cmd_type != PTZ_VERT_UP_KEEP &&
|
|||
|
cmd_type != PTZ_VERT_DOWN_KEEP)
|
|||
|
{
|
|||
|
g_ptz.vert_arrive_flag = 0;//ת<><D7AA>ָ<EFBFBD><D6B8>Ԥλ<D4A4><CEBB>
|
|||
|
ptz_location_return_return(LOCATION_VERT);//<2F><>λ<EFBFBD>ش<EFBFBD>
|
|||
|
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
|
|||
|
}
|
|||
|
g_ptz.vert_rotate_monitor_switch = 0;
|
|||
|
if(g_ptz.vert_repeat_locate_switch == 1 &&
|
|||
|
cmd_type != PTZ_VERT_UP_KEEP &&
|
|||
|
cmd_type != PTZ_VERT_DOWN_KEEP)
|
|||
|
{
|
|||
|
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_A;
|
|||
|
}
|
|||
|
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
|||
|
}
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
|
|||
|
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_VERT_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
|
|||
|
}
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_VERT_BRAKE://ɲ<><C9B2>
|
|||
|
g_ptz.vert_rotate_monitor_switch = 0;
|
|||
|
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_VERT_DEC_BRAKE_A://<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
|
|||
|
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
|
|||
|
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DEC_BRAKE_B;
|
|||
|
k = 0;
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_VERT_DEC_BRAKE_B:
|
|||
|
k ++;//K<><4B><EFBFBD><EFBFBD>1,ʱ<><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1ms
|
|||
|
if(g_ptz.vert_speed_actual <= PTZ_VERT_MIN_SPEED * 1.5 ||
|
|||
|
k >= 1400)
|
|||
|
{
|
|||
|
k = 0;
|
|||
|
g_ptz.vert_rotate_monitor_switch = 0;
|
|||
|
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
|||
|
}
|
|||
|
break;
|
|||
|
|
|||
|
//<2F>ظ<EFBFBD><D8B8><EFBFBD>λ·<CEBB><C2B7><EFBFBD>滮
|
|||
|
case PTZ_VERT_REPEAT_LOCATE_A:
|
|||
|
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
|
|||
|
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_B;
|
|||
|
break;
|
|||
|
//<2F>ظ<EFBFBD><D8B8><EFBFBD>λת<CEBB><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
case PTZ_VERT_REPEAT_LOCATE_B:
|
|||
|
if(g_ptz.vert_rotate_plan.rotate_switch == 1)
|
|||
|
{
|
|||
|
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_C;
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
|
|||
|
{
|
|||
|
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
|
|||
|
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
|
|||
|
if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction)
|
|||
|
{
|
|||
|
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
|
|||
|
}
|
|||
|
else//ת<><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
|
|||
|
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
|
|||
|
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
|
|||
|
ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
|
|||
|
}
|
|||
|
}
|
|||
|
else//û<><C3BB>ת<EFBFBD><D7AA>
|
|||
|
{
|
|||
|
g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
|
|||
|
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
|
|||
|
ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
|
|||
|
}
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
|
|||
|
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_VERT_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
|
|||
|
}
|
|||
|
|
|||
|
}
|
|||
|
else//g_ptz.vert_rotate_plan.rotate_switch == 0
|
|||
|
{
|
|||
|
//ɲ<><C9B2><EFBFBD>ر<EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
g_ptz.vert_arrive_flag = 0;//ת<><D7AA>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB>
|
|||
|
ptz_location_return_return(LOCATION_VERT);//<2F><>λ<EFBFBD>ش<EFBFBD>
|
|||
|
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
|
|||
|
g_ptz.vert_rotate_monitor_switch = 0;
|
|||
|
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
|
|||
|
{
|
|||
|
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
|||
|
}
|
|||
|
}
|
|||
|
break;
|
|||
|
|
|||
|
case PTZ_VERT_REPEAT_LOCATE_C:
|
|||
|
BNP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle);
|
|||
|
BFP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_far_angle);
|
|||
|
/*<2A><>λ*/
|
|||
|
if(BNP_D + BFP_D <= (g_ptz.vert_rotate_plan.stop_angle_range + 0.01))
|
|||
|
{
|
|||
|
g_ptz.vert_arrive_flag = 0;//Ԥ<><D4A4>λɨ<CEBB>裬ת<E8A3AC><D7AA>ָ<EFBFBD><D6B8>Ԥ<EFBFBD><D4A4>λ
|
|||
|
ptz_location_return_return(LOCATION_VERT);//<2F><>λ<EFBFBD>ش<EFBFBD>
|
|||
|
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
|
|||
|
g_ptz.vert_rotate_monitor_switch = 0;
|
|||
|
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
|||
|
}
|
|||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
|
|||
|
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
|
|||
|
PTZ_VERT_MIN_SPEED_ANGLE)
|
|||
|
{
|
|||
|
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
|
|||
|
}
|
|||
|
break;
|
|||
|
}
|
|||
|
|
|||
|
//<2F><><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڼ<EFBFBD><DABC><EFBFBD>λ<EFBFBD><CEBB>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>Խ<EFBFBD><D4BD>
|
|||
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
if(g_ptz.vert_ps_sw1_state == PS_COVER)//<2F>¸<EFBFBD><C2B8><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
|
|||
|
{
|
|||
|
if(g_ptz.vert_start_stop_set == PTZ_VERT_START
|
|||
|
&& g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN)
|
|||
|
{
|
|||
|
g_ptz.vert_arrive_flag = 0;
|
|||
|
g_ptz.vert_rotate_monitor_switch = 0;
|
|||
|
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
|||
|
}
|
|||
|
}
|
|||
|
if(g_ptz.vert_ps_sw2_state == PS_COVER)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
|
|||
|
{
|
|||
|
if(g_ptz.vert_start_stop_set == PTZ_VERT_START
|
|||
|
&& g_ptz.vert_direction_set == PTZ_VERT_DIR_UP)
|
|||
|
{
|
|||
|
g_ptz.vert_arrive_flag = 0;
|
|||
|
g_ptz.vert_rotate_monitor_switch = 0;
|
|||
|
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
|||
|
}
|
|||
|
}
|
|||
|
#endif
|
|||
|
|
|||
|
return 1;
|
|||
|
}
|
|||
|
|
|||
|
static void ptz_hori_rotate_task()
|
|||
|
{
|
|||
|
while(1)
|
|||
|
{
|
|||
|
ptz_hori_rotate_monitor_task();
|
|||
|
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
static void ptz_vert_rotate_task()
|
|||
|
{
|
|||
|
while(1)
|
|||
|
{
|
|||
|
ptz_vert_rotate_monitor_task();
|
|||
|
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
static OS_STK task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE];
|
|||
|
static void creat_task_hori_rotate(void)
|
|||
|
{
|
|||
|
CPU_INT08U task_err;
|
|||
|
CPU_INT08U name_err;
|
|||
|
|
|||
|
task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_rotate_task,
|
|||
|
(void *) 0,
|
|||
|
(OS_STK *)&task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE - 1],
|
|||
|
(INT8U ) TASK_HORI_ROATE_MONITOR_PRIO,
|
|||
|
(INT16U ) TASK_HORI_ROATE_MONITOR_PRIO,
|
|||
|
(OS_STK *)&task_hori_rotate_stk[0],
|
|||
|
(INT32U ) TASK_HORI_ROATE_MONITOR_STK_SIZE,
|
|||
|
(void *) 0,
|
|||
|
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
|
|||
|
#if (OS_TASK_NAME_EN > 0)
|
|||
|
OSTaskNameSet(TASK_HORI_ROATE_MONITOR_PRIO, "ptz_hori_rotate_task", &name_err);
|
|||
|
#endif
|
|||
|
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
|
|||
|
static OS_STK task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE];
|
|||
|
static void creat_task_vert_rotate(void)
|
|||
|
{
|
|||
|
CPU_INT08U task_err;
|
|||
|
CPU_INT08U name_err;
|
|||
|
|
|||
|
task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_rotate_task,
|
|||
|
(void *) 0,
|
|||
|
(OS_STK *)&task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE - 1],
|
|||
|
(INT8U ) TASK_VERT_ROATE_MONITOR_PRIO,
|
|||
|
(INT16U ) TASK_VERT_ROATE_MONITOR_PRIO,
|
|||
|
(OS_STK *)&task_vert_rotate_stk[0],
|
|||
|
(INT32U ) TASK_VERT_ROATE_MONITOR_STK_SIZE,
|
|||
|
(void *) 0,
|
|||
|
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
|
|||
|
#if (OS_TASK_NAME_EN > 0)
|
|||
|
OSTaskNameSet(TASK_VERT_ROATE_MONITOR_PRIO, "ptz_vert_rotate_task", &name_err);
|
|||
|
#endif
|
|||
|
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void init_rotate_monitor_module(void)
|
|||
|
{
|
|||
|
BSP_OS_SemCreate(&ptz_hori_stop_mutex,1u,"ptz_hori_stop_mutex");
|
|||
|
BSP_OS_SemCreate(&ptz_vert_stop_mutex,1u,"ptz_vert_stop_mutex");
|
|||
|
|
|||
|
l6235d_init();
|
|||
|
creat_task_hori_rotate();
|
|||
|
creat_task_vert_rotate();
|
|||
|
|
|||
|
}
|
|||
|
|
|||
|
#endif
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|