3834 lines
130 KiB
C
3834 lines
130 KiB
C
|
#include "ptz_header_file.h"
|
|||
|
|
#include "get_angle.h"
|
|||
|
|
#include "rotate_plan.h"
|
|||
|
|
#include "rotate_bldc.h"
|
|||
|
|
#include "rotate_step.h"
|
|||
|
|
#include "agent_hyt.h"
|
|||
|
|
#include "ptz_struct.h"
|
|||
|
|
#include "math.h"
|
|||
|
|
#include "service_selfcheck.h"
|
|||
|
|
#include "ptz_type_select.h"
|
|||
|
|
#include "ptz_global_variable.h"
|
|||
|
|
#include "speed_to_step.h"
|
|||
|
|
|
|||
|
|
///<2F><>̨<EFBFBD>ǶȻظ<C8BB>
|
|||
|
|
void ptz_send_angle(char dev,char angle_choice)
|
|||
|
|
{
|
|||
|
|
unsigned short int uint16_angle;
|
|||
|
|
unsigned char data[7];
|
|||
|
|
if(angle_choice == PTZ_HORI)
|
|||
|
|
{
|
|||
|
|
uint16_angle = (unsigned short int)(g_ptz.hori_angle_actual * 100 + 0.5);
|
|||
|
|
data[0] = 0xff;
|
|||
|
|
data[1] = g_ptz.address;
|
|||
|
|
data[3] = 0x59;
|
|||
|
|
data[4] = (u_int16_t)(uint16_angle >> 8);
|
|||
|
|
data[5] = (u_int16_t)(uint16_angle & 0x00ff);
|
|||
|
|
data[6] = MotorCalPelcoDSUM(data,sizeof(data));
|
|||
|
|
}
|
|||
|
|
else//angle_choice == PTZ_VERT
|
|||
|
|
{
|
|||
|
|
uint16_angle = (unsigned short int)(g_ptz.vert_angle_actual * 100 + 0.5);
|
|||
|
|
data[0] = 0xff;
|
|||
|
|
data[1] = g_ptz.address;
|
|||
|
|
data[3] = 0x5b;
|
|||
|
|
data[4] = (u_int16_t)(uint16_angle >> 8);
|
|||
|
|
data[5] = (u_int16_t)(uint16_angle & 0x00ff);
|
|||
|
|
data[6] = MotorCalPelcoDSUM(data,sizeof(data));
|
|||
|
|
}
|
|||
|
|
ptz_send_data(dev, data, sizeof(data));
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F>Ƕ<EFBFBD>״̬<D7B4><CCAC>ѯ<EFBFBD><D1AF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
void ptz_send_sw_angle_state(char dev)
|
|||
|
|
{
|
|||
|
|
unsigned char data[7];
|
|||
|
|
data[0] = 0xff;
|
|||
|
|
data[1] = g_ptz.address;
|
|||
|
|
data[2] = 0x9d;
|
|||
|
|
data[3] = 0;
|
|||
|
|
data[4] = g_ptz.hori_angle_state;
|
|||
|
|
data[5] = g_ptz.vert_angle_state;
|
|||
|
|
data[6] = MotorCalPelcoDSUM(data,sizeof(data));
|
|||
|
|
|
|||
|
|
|
|||
|
|
ptz_send_data(dev, data, sizeof(data));
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//ʱ<><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
unsigned int time_ticks(unsigned int time_now, unsigned time_end)
|
|||
|
|
{
|
|||
|
|
unsigned int time;
|
|||
|
|
if(time_now >= time_end)
|
|||
|
|
{
|
|||
|
|
time = time_now - time_end;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
time = (time_now - 0) + (4294967295 - time_end);
|
|||
|
|
}
|
|||
|
|
return time;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
#ifdef PTZ_BLDC_MOTOR
|
|||
|
|
static void ptz_hori_ps_delay_ms()
|
|||
|
|
{
|
|||
|
|
unsigned int hori_ps_time = 0;
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
if(g_ptz.hori_speed_actual > 0)
|
|||
|
|
{
|
|||
|
|
hori_ps_time = (unsigned int)((PTZ_HORI_MAX_SPEED / g_ptz.hori_speed_actual) * PTZ_HORI_PS_TIEM + 0.5);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
hori_ps_time = (unsigned int)((PTZ_HORI_MAX_SPEED / PTZ_HORI_MIN_SPEED) * PTZ_HORI_PS_TIEM + 0.5);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(hori_ps_time < PTZ_HORI_PS_TIEM)
|
|||
|
|
{
|
|||
|
|
hori_ps_time = PTZ_HORI_PS_TIEM;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
OSTimeDlyHMSM(0u, 0u, 0u, hori_ps_time);
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void ptz_vert_ps_delay_ms()
|
|||
|
|
{
|
|||
|
|
unsigned int vert_ps_time = 0;
|
|||
|
|
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
if(g_ptz.vert_speed_actual > 0)
|
|||
|
|
{
|
|||
|
|
vert_ps_time = (unsigned int)((PTZ_VERT_MAX_SPEED / g_ptz.vert_speed_actual) * PTZ_VERT_PS_TIEM);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
vert_ps_time = (unsigned int)((PTZ_VERT_MAX_SPEED / PTZ_VERT_MIN_SPEED) * PTZ_VERT_PS_TIEM);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(vert_ps_time < PTZ_VERT_PS_TIEM)
|
|||
|
|
{
|
|||
|
|
vert_ps_time = PTZ_VERT_PS_TIEM;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
OSTimeDlyHMSM(0u, 0u, 0u, vert_ps_time);
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#ifdef PTZ_STEP_MOTOR
|
|||
|
|
|
|||
|
|
static void ptz_hori_ps_delay_ms()
|
|||
|
|
{
|
|||
|
|
// unsigned int hori_ps_time = 0;
|
|||
|
|
//#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
// if(g_ptz.hori_speed_actual > 0)
|
|||
|
|
// {
|
|||
|
|
// hori_ps_time = (unsigned int)((PTZ_HORI_MAX_SPEED / g_ptz.hori_speed_actual) * PTZ_HORI_PS_TIEM + 0.5);
|
|||
|
|
// }
|
|||
|
|
// else
|
|||
|
|
// {
|
|||
|
|
// hori_ps_time = (unsigned int)((PTZ_HORI_MAX_SPEED / PTZ_HORI_MIN_SPEED) * PTZ_HORI_PS_TIEM + 0.5);
|
|||
|
|
// }
|
|||
|
|
//
|
|||
|
|
// if(hori_ps_time < PTZ_HORI_PS_TIEM)
|
|||
|
|
// {
|
|||
|
|
// hori_ps_time = PTZ_HORI_PS_TIEM;
|
|||
|
|
// }
|
|||
|
|
//
|
|||
|
|
// OSTimeDlyHMSM(0u, 0u, 0u, hori_ps_time);
|
|||
|
|
//#endif
|
|||
|
|
//
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static void ptz_vert_ps_delay_ms()
|
|||
|
|
{
|
|||
|
|
// unsigned int vert_ps_time = 0;
|
|||
|
|
//
|
|||
|
|
//#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
// if(g_ptz.vert_speed_actual > 0)
|
|||
|
|
// {
|
|||
|
|
// vert_ps_time = (unsigned int)((PTZ_VERT_MAX_SPEED / g_ptz.vert_speed_actual) * PTZ_VERT_PS_TIEM + 0.5);
|
|||
|
|
// }
|
|||
|
|
// else
|
|||
|
|
// {
|
|||
|
|
// vert_ps_time = (unsigned int)((PTZ_VERT_MAX_SPEED / PTZ_VERT_MIN_SPEED) * PTZ_VERT_PS_TIEM + 0.5);
|
|||
|
|
// }
|
|||
|
|
//
|
|||
|
|
// if(vert_ps_time < PTZ_VERT_PS_TIEM)
|
|||
|
|
// {
|
|||
|
|
// vert_ps_time = PTZ_VERT_PS_TIEM;
|
|||
|
|
// }
|
|||
|
|
//
|
|||
|
|
// OSTimeDlyHMSM(0u, 0u, 0u, vert_ps_time);
|
|||
|
|
//#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨ˮƽ<CBAE>Ƕ<EFBFBD>
|
|||
|
|
#ifdef PTZ_HORI_ANGLE_AS5047D_ANGLE_ASY
|
|||
|
|
static char ptz_hori_get_angle()
|
|||
|
|
{
|
|||
|
|
float right_diff_x1 = 0;
|
|||
|
|
float right_diff_x2 = 0;
|
|||
|
|
|
|||
|
|
float hori_angle[PTZ_HORI_ANGLE_READ_NUM] = {0};
|
|||
|
|
char i = 0, j = 0, k = 0;
|
|||
|
|
float tem = 0;
|
|||
|
|
float tem1 = 0;
|
|||
|
|
float tem2 = 0;
|
|||
|
|
float left_diff_x1 = 0;
|
|||
|
|
float left_diff_x2 = 0;
|
|||
|
|
unsigned int tem3 = 0;
|
|||
|
|
float tem4 = 0;
|
|||
|
|
static float hori_cycle_angle_right;
|
|||
|
|
static float hori_cycle_angle_left;
|
|||
|
|
char flag;
|
|||
|
|
#ifdef PTZ_HORI_GET_ANGLE_MANY
|
|||
|
|
for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
hori_angle[i] = as5047d_hori_get_angle_a();
|
|||
|
|
|
|||
|
|
|
|||
|
|
// test_angle[test_angle_count] = hori_angle[i];
|
|||
|
|
// test_angle_count ++;
|
|||
|
|
// if(test_angle_count >= 500)
|
|||
|
|
// {
|
|||
|
|
// test_angle_count = 0;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
|
|||
|
|
if(hori_angle[i] < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
hori_angle[i] > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(hori_angle[i]) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
i --;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ڴű<DAB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD>ֹ<EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>ӽ<EFBFBD>360<36>ȣ<EFBFBD><C8A3>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>0.2<EFBFBD><EFBFBD>0.1<EFBFBD><EFBFBD>359.9<EFBFBD><EFBFBD>359.8<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݾ<EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
flag = 0;
|
|||
|
|
for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
if(fabs(hori_angle[i] - hori_angle[i+1]) > 350.0)
|
|||
|
|
{
|
|||
|
|
flag = 1;//˵<><CBB5><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if(flag == 1)
|
|||
|
|
{
|
|||
|
|
for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
if(hori_angle[i] < 5.0)
|
|||
|
|
{
|
|||
|
|
hori_angle[i] = hori_angle[i] + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD>ĽǶ<C4BD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
|
|||
|
|
for(j = 0; j < PTZ_HORI_ANGLE_READ_NUM-1; j++)//<2F><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
for(k = 0; k < PTZ_HORI_ANGLE_READ_NUM-j-1; k++)
|
|||
|
|
{
|
|||
|
|
if(hori_angle[k] > hori_angle[k+1])
|
|||
|
|
{
|
|||
|
|
tem = hori_angle[k];
|
|||
|
|
hori_angle[k] = hori_angle[k+1];
|
|||
|
|
hori_angle[k+1] = tem;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
tem = 0;
|
|||
|
|
for(uint8_t i = 1; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
tem = tem + hori_angle[i];//ȥ<><C8A5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Сֵ
|
|||
|
|
}
|
|||
|
|
tem = tem / ((float)(PTZ_HORI_ANGLE_READ_NUM - 2));
|
|||
|
|
|
|||
|
|
if(tem > 360.0)
|
|||
|
|
{
|
|||
|
|
tem = tem - 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#ifdef PTZ_HORI_GET_ANGLE_SINGLE
|
|||
|
|
tem = as5047d_hori_get_angle_a();
|
|||
|
|
// hori_as5047d_angle_save[hori_as5047d_angle_save_count] = tem;
|
|||
|
|
// hori_as5047d_angle_save_count ++;
|
|||
|
|
// if(hori_as5047d_angle_save_lock >= 1)
|
|||
|
|
// {
|
|||
|
|
// hori_as5047d_angle_save_lock ++;
|
|||
|
|
// }
|
|||
|
|
// if(hori_as5047d_angle_save_lock >= 10)
|
|||
|
|
// {
|
|||
|
|
// hori_as5047d_angle_save_lock = 0;
|
|||
|
|
// }
|
|||
|
|
// if(hori_as5047d_angle_save_count > 48)
|
|||
|
|
// {
|
|||
|
|
// hori_as5047d_angle_save_count = 0;
|
|||
|
|
// }
|
|||
|
|
if(tem < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
tem > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(tem) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>¶<EFBFBD>ȡ
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_data_reset == 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_num = 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_data_reset = 0;
|
|||
|
|
tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
hori_cycle_angle_right = tem1 + 350.0;
|
|||
|
|
hori_cycle_angle_left = tem1 + 10.0;
|
|||
|
|
}
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
// if(g_ptz.hori_ps_sw3_right_fall >= 1)
|
|||
|
|
// {
|
|||
|
|
// if(PS_HORI_SW3_READ == PS_COVER)
|
|||
|
|
// {
|
|||
|
|
// 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_ps_sw3_right_fall = 0;
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_rise >= 1)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_SW3_LEFT_RISE_UPDATE
|
|||
|
|
ptz_hori_ps_delay_ms();
|
|||
|
|
if(PS_HORI_SW3_READ == PS_NO_COVER)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual + g_ptz.hori_as5047d.as5047d_remain_angle;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_angle_state = 1;
|
|||
|
|
g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_fall >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_right_fall = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_rise >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_right_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_fall >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_left_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
hori_cycle_angle_right = tem1 + 350.0;
|
|||
|
|
hori_cycle_angle_left = tem1 + 10.0;
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
left_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
left_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& left_diff_x1 <= PTZ_LEFT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& left_diff_x2 <= PTZ_LEFT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>ű<F3A3ACB4><C5B1><EFBFBD><EFBFBD>Ƕȼ<C7B6>С
|
|||
|
|
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
// if(g_ptz.hori_ps_sw3_left_rise >= 1)
|
|||
|
|
// {
|
|||
|
|
// if(PS_HORI_SW3_READ == PS_NO_COVER)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual +
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_remain_angle;
|
|||
|
|
// if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
// }
|
|||
|
|
// g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_fall >= 1)
|
|||
|
|
{
|
|||
|
|
ptz_hori_ps_delay_ms();
|
|||
|
|
if(PS_HORI_SW3_READ == PS_COVER)
|
|||
|
|
{
|
|||
|
|
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_angle_state = 1;
|
|||
|
|
g_ptz.hori_ps_sw3_right_fall = 0;
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_ps_sw3_right_fall = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_rise >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_rise >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_right_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_fall >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_left_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
hori_cycle_angle_right = tem1 + 350.0;
|
|||
|
|
hori_cycle_angle_left = tem1 + 10.0;
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
right_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
right_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual) +
|
|||
|
|
fabs(g_ptz.hori_as5047d.as5047d_ptz_init_angle - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual <= g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& right_diff_x1 <= PTZ_RIGHT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual >= g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& right_diff_x2 <= PTZ_RIGHT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>㵱ǰ<E3B5B1>Ƕ<EFBFBD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
fabs(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual) +
|
|||
|
|
fabs(g_ptz.hori_as5047d.as5047d_ptz_init_angle - 0) +
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
(g_ptz.hori_as5047d.as5047d_ptz_init_angle -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual) +
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual == g_ptz.hori_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Դű<D4B4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǶȽ<C7B6><C8BD><EFBFBD><EFBFBD>˲<EFBFBD>
|
|||
|
|
if((fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last)
|
|||
|
|
< PTZ_HORI_AS5047D_ANGLE_WF_A) ||
|
|||
|
|
(fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last)
|
|||
|
|
> PTZ_HORI_AS5047D_ANGLE_WF_B))
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
//break;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
// if(hori_as5047d_angle_save_lock == 0)
|
|||
|
|
// {
|
|||
|
|
// hori_as5047d_angle_save_lock = 1;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><>ֹ<EFBFBD>Ƕȷ<C7B6>Χ<EFBFBD><CEA7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_angle_max)
|
|||
|
|
{
|
|||
|
|
tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_max);
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual + tem2;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual < 0)
|
|||
|
|
{
|
|||
|
|
tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - 0);
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual +
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual + g_ptz.hori_as5047d.as5047d_remain_angle - tem2;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual == g_ptz.hori_as5047d.as5047d_ptz_angle_max
|
|||
|
|
|| g_ptz.hori_as5047d.as5047d_ptz_angle_actual == 0)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual + g_ptz.hori_as5047d.as5047d_remain_angle;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>̨<EFBFBD>ǶȽ<C7B6><C8BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3λС<CEBB><D0A1>
|
|||
|
|
tem3 = (unsigned int)((g_ptz.hori_as5047d.as5047d_ptz_angle_actual *
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_K) * 1000.0 + 0.5);
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨ʵ<CCA8>ʽǶ<CABD>
|
|||
|
|
tem4 = tem3 / 1000.0;
|
|||
|
|
g_ptz.hori_angle_actual_a = tem4;
|
|||
|
|
if(g_ptz.offset_angle.hori_offset_switch == 2)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 - g_ptz.offset_angle.hori_zero_offset_angle;
|
|||
|
|
if(tem4 < 0)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_angle_actual = tem4;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>浱ǰ<E6B5B1>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD>ֵ
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_last = g_ptz.hori_as5047d.as5047d_angle_actual;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_last = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
|
|||
|
|
g_ptz.hori_angle_last = g_ptz.hori_angle_actual;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static char ptz_hori_get_angle_a()
|
|||
|
|
{
|
|||
|
|
float right_diff_x1 = 0;
|
|||
|
|
float right_diff_x2 = 0;
|
|||
|
|
|
|||
|
|
float hori_angle[PTZ_HORI_ANGLE_READ_NUM] = {0};
|
|||
|
|
char i = 0, j = 0, k = 0, m = 0;
|
|||
|
|
float tem = 0;
|
|||
|
|
float tem1 = 0;
|
|||
|
|
float tem2 = 0;
|
|||
|
|
float left_diff_x1 = 0;
|
|||
|
|
float left_diff_x2 = 0;
|
|||
|
|
float x1 = 0, x2 = 0, x3 = 0;
|
|||
|
|
unsigned int tem3 = 0;
|
|||
|
|
float tem4 = 0;
|
|||
|
|
static float hori_cycle_angle_right;
|
|||
|
|
static float hori_cycle_angle_left;
|
|||
|
|
char flag;
|
|||
|
|
int cycle_count_save = 0;
|
|||
|
|
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.time_end = g_ptz.hori_as5047d.time_now;
|
|||
|
|
g_ptz.hori_as5047d.time_now = OSTimeGet();
|
|||
|
|
g_ptz.hori_as5047d.time = time_ticks(g_ptz.hori_as5047d.time_now, g_ptz.hori_as5047d.time_end);
|
|||
|
|
|
|||
|
|
#ifdef CLK_50M
|
|||
|
|
if(g_ptz.hori_as5047d.time < 2)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.time = 2;
|
|||
|
|
}
|
|||
|
|
#else
|
|||
|
|
if(g_ptz.hori_as5047d.time < 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.time = 1;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#ifdef PTZ_HORI_GET_ANGLE_MANY
|
|||
|
|
for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
hori_angle[i] = as5047d_hori_get_angle_a();
|
|||
|
|
|
|||
|
|
|
|||
|
|
// test_angle[test_angle_count] = hori_angle[i];
|
|||
|
|
// test_angle_count ++;
|
|||
|
|
// if(test_angle_count >= 500)
|
|||
|
|
// {
|
|||
|
|
// test_angle_count = 0;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
|
|||
|
|
if(hori_angle[i] < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
hori_angle[i] > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(hori_angle[i]) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
i --;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ڴű<DAB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD>ֹ<EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>ӽ<EFBFBD>360<36>ȣ<EFBFBD><C8A3>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>0.2<EFBFBD><EFBFBD>0.1<EFBFBD><EFBFBD>359.9<EFBFBD><EFBFBD>359.8<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݾ<EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
flag = 0;
|
|||
|
|
for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
if(fabs(hori_angle[i] - hori_angle[i+1]) > 350.0)
|
|||
|
|
{
|
|||
|
|
flag = 1;//˵<><CBB5><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if(flag == 1)
|
|||
|
|
{
|
|||
|
|
for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
if(hori_angle[i] < 5.0)
|
|||
|
|
{
|
|||
|
|
hori_angle[i] = hori_angle[i] + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD>ĽǶ<C4BD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
|
|||
|
|
for(j = 0; j < PTZ_HORI_ANGLE_READ_NUM-1; j++)//<2F><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
for(k = 0; k < PTZ_HORI_ANGLE_READ_NUM-j-1; k++)
|
|||
|
|
{
|
|||
|
|
if(hori_angle[k] > hori_angle[k+1])
|
|||
|
|
{
|
|||
|
|
tem = hori_angle[k];
|
|||
|
|
hori_angle[k] = hori_angle[k+1];
|
|||
|
|
hori_angle[k+1] = tem;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
tem = 0;
|
|||
|
|
for(uint8_t i = 1; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
tem = tem + hori_angle[i];//ȥ<><C8A5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Сֵ
|
|||
|
|
}
|
|||
|
|
tem = tem / ((float)(PTZ_HORI_ANGLE_READ_NUM - 2));
|
|||
|
|
|
|||
|
|
if(tem > 360.0)
|
|||
|
|
{
|
|||
|
|
tem = tem - 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#ifdef PTZ_HORI_GET_ANGLE_SINGLE
|
|||
|
|
tem = as5047d_hori_get_angle_a();
|
|||
|
|
// hori_as5047d_angle_save[hori_as5047d_angle_save_count] = tem;
|
|||
|
|
// hori_as5047d_angle_save_count ++;
|
|||
|
|
// if(hori_as5047d_angle_save_lock >= 1)
|
|||
|
|
// {
|
|||
|
|
// hori_as5047d_angle_save_lock ++;
|
|||
|
|
// }
|
|||
|
|
// if(hori_as5047d_angle_save_lock >= 10)
|
|||
|
|
// {
|
|||
|
|
// hori_as5047d_angle_save_lock = 0;
|
|||
|
|
// }
|
|||
|
|
// if(hori_as5047d_angle_save_count > 48)
|
|||
|
|
// {
|
|||
|
|
// hori_as5047d_angle_save_count = 0;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
|
|||
|
|
// if(tem < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
// tem > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
// isnan(tem) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
// {//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>¶<EFBFBD>ȡ
|
|||
|
|
// return 0;
|
|||
|
|
// }
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
|
|||
|
|
for(m = 0; m < 5; m++)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
tem = as5047d_hori_get_angle_a();
|
|||
|
|
|
|||
|
|
if(tem < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
tem > 360.0 ||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(tem) == 1 ||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
((fabs(tem - g_ptz.hori_as5047d.as5047d_angle_last) > PTZ_HORI_ANGLE_DIFF_A) && (fabs(tem - g_ptz.hori_as5047d.as5047d_angle_last) < PTZ_HORI_ANGLE_DIFF_B)))
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>¶<EFBFBD>ȡ
|
|||
|
|
asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_data_reset == 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_num = 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_data_reset = 0;
|
|||
|
|
tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
hori_cycle_angle_right = tem1 + 350.0;
|
|||
|
|
hori_cycle_angle_left = tem1 + 10.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_last);
|
|||
|
|
|
|||
|
|
x2 = (g_ptz.hori_as5047d.as5047d_angle_actual - 0) +
|
|||
|
|
(360.0 - g_ptz.hori_as5047d.as5047d_angle_last);
|
|||
|
|
|
|||
|
|
x3 = (g_ptz.hori_as5047d.as5047d_angle_last - 0) +
|
|||
|
|
(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
|
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_angle_last
|
|||
|
|
&& x1 < x3)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_FWD;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_angle_last
|
|||
|
|
&& x2 < x1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_FWD;
|
|||
|
|
}
|
|||
|
|
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ƕȼ<C7B6>С
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_angle_last
|
|||
|
|
&& x3 < x1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_REV;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_angle_last
|
|||
|
|
&& x1 < x2)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_REV;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(x1 == x2 || x1 == x3)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_FWD;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_REV;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_dir == AS5047D_DIR_FWD)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
// if(g_ptz.hori_ps_sw3_right_fall >= 1)
|
|||
|
|
// {
|
|||
|
|
// if(PS_HORI_SW3_READ == PS_COVER)
|
|||
|
|
// {
|
|||
|
|
// 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_ps_sw3_right_fall = 0;
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_rise >= 1)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_SW3_LEFT_RISE_UPDATE
|
|||
|
|
if(g_ptz.hori_angle_erro_switch == 0)
|
|||
|
|
{//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>عرգ<D8B1>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>ؽǶȸ<C7B6><C8B8>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
|
|||
|
|
ptz_hori_ps_delay_ms();
|
|||
|
|
if(PS_HORI_SW3_READ == PS_NO_COVER)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual + g_ptz.hori_as5047d.as5047d_remain_angle;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_angle_state = 1;
|
|||
|
|
g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_fall >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_right_fall = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_rise >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_right_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_fall >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_left_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
hori_cycle_angle_right = tem1 + 350.0;
|
|||
|
|
hori_cycle_angle_left = tem1 + 10.0;
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
left_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
left_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& left_diff_x1 <= PTZ_LEFT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& left_diff_x2 <= PTZ_LEFT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>ű<F3A3ACB4><C5B1><EFBFBD><EFBFBD>Ƕȼ<C7B6>С
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_dir == AS5047D_DIR_REV)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
// if(g_ptz.hori_ps_sw3_left_rise >= 1)
|
|||
|
|
// {
|
|||
|
|
// if(PS_HORI_SW3_READ == PS_NO_COVER)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual +
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_remain_angle;
|
|||
|
|
// if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
// }
|
|||
|
|
// g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_fall >= 1)
|
|||
|
|
{
|
|||
|
|
ptz_hori_ps_delay_ms();
|
|||
|
|
if(PS_HORI_SW3_READ == PS_COVER)
|
|||
|
|
{
|
|||
|
|
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_angle_state = 1;
|
|||
|
|
g_ptz.hori_ps_sw3_right_fall = 0;
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_ps_sw3_right_fall = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_rise >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_rise >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_right_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_fall >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_left_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
hori_cycle_angle_right = tem1 + 350.0;
|
|||
|
|
hori_cycle_angle_left = tem1 + 10.0;
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
right_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
right_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual) +
|
|||
|
|
fabs(g_ptz.hori_as5047d.as5047d_ptz_init_angle - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual <= g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& right_diff_x1 <= PTZ_RIGHT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual >= g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& right_diff_x2 <= PTZ_RIGHT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count_last = g_ptz.hori_as5047d.as5047d_cycle_count;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>浱ǰ<E6B5B1>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6>
|
|||
|
|
cycle_count_save = g_ptz.hori_as5047d.as5047d_cycle_count;
|
|||
|
|
|
|||
|
|
for(i = 0; i < 4; i++)
|
|||
|
|
{//<2F><><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>Ϊ<EFBFBD>˷<EFBFBD>ֹ<EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<F3A3ACBB><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĵű<C4B4><C5B1><EFBFBD>Ȧ<EFBFBD><C8A6>
|
|||
|
|
//<2F><><EFBFBD>㵱ǰ<E3B5B1>Ƕ<EFBFBD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
fabs(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual) +
|
|||
|
|
fabs(g_ptz.hori_as5047d.as5047d_ptz_init_angle - 0) +
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
(g_ptz.hori_as5047d.as5047d_ptz_init_angle -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual) +
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual == g_ptz.hori_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Դű<D4B4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܽǶȽ<C7B6><C8BD><EFBFBD><EFBFBD>˲<EFBFBD>
|
|||
|
|
if((fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last)
|
|||
|
|
< PTZ_HORI_AS5047D_ANGLE_WF_A) ||
|
|||
|
|
(fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last)
|
|||
|
|
> PTZ_HORI_AS5047D_ANGLE_WF_B))
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>жϣ<D0B6><CFA3>ָ<EFBFBD>ʵ<EFBFBD>ʵĴű<C4B4><C5B1><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6>
|
|||
|
|
switch(i)
|
|||
|
|
{
|
|||
|
|
case 0:
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case 1:
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case 2: //<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>Ȧ<EFBFBD><C8A6><EFBFBD>ָ<EFBFBD>ʧ<EFBFBD><CAA7>
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case 3:
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//<2F><>ֹ<EFBFBD>Ƕȷ<C7B6>Χ<EFBFBD><CEA7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_angle_max)
|
|||
|
|
{
|
|||
|
|
tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_max);
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = (int)(g_ptz.hori_as5047d.as5047d_ptz_angle_actual / 360.0);
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual + tem2;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual < 0)
|
|||
|
|
{
|
|||
|
|
tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - 0);
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_actual + g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = (int)(g_ptz.hori_as5047d.as5047d_ptz_angle_actual / 360.0);
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual + g_ptz.hori_as5047d.as5047d_remain_angle - tem2;
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual == g_ptz.hori_as5047d.as5047d_ptz_angle_max
|
|||
|
|
|| g_ptz.hori_as5047d.as5047d_ptz_angle_actual == 0)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual + g_ptz.hori_as5047d.as5047d_remain_angle;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>̨<EFBFBD>ǶȽ<C7B6><C8BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3λС<CEBB><D0A1>
|
|||
|
|
tem3 = (unsigned int)((g_ptz.hori_as5047d.as5047d_ptz_angle_actual *
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_K) * 1000.0 + 0.5);
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨ʵ<CCA8>ʽǶ<CABD>
|
|||
|
|
tem4 = tem3 / 1000.0;
|
|||
|
|
g_ptz.hori_angle_actual_a = tem4;
|
|||
|
|
if(g_ptz.offset_angle.hori_offset_switch == 2)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 - g_ptz.offset_angle.hori_zero_offset_angle;
|
|||
|
|
if(tem4 < 0)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_angle_actual = tem4;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>浱ǰ<E6B5B1>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD>ֵ
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_last = g_ptz.hori_as5047d.as5047d_angle_actual;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_last = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
|
|||
|
|
g_ptz.hori_angle_last = g_ptz.hori_angle_actual;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
|
|||
|
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ֱ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
#ifdef PTZ_VERT_ANGLE_AS5047D_ANGLE_ASY
|
|||
|
|
static char ptz_vert_get_angle()
|
|||
|
|
{
|
|||
|
|
float up_diff_x1 = 0;
|
|||
|
|
float up_diff_x2 = 0;
|
|||
|
|
|
|||
|
|
float vert_angle[PTZ_VERT_ANGLE_READ_NUM] = {0};
|
|||
|
|
char i = 0, j = 0, k = 0;
|
|||
|
|
float tem = 0;
|
|||
|
|
float tem1 = 0;
|
|||
|
|
static float vert_cycle_angle_up;
|
|||
|
|
static float vert_cycle_angle_down;
|
|||
|
|
long int tem3 = 0;
|
|||
|
|
float tem4 = 0;
|
|||
|
|
float down_diff_x1 = 0;
|
|||
|
|
float down_diff_x2 = 0;
|
|||
|
|
char flag;
|
|||
|
|
// static float vert_angle_actual;
|
|||
|
|
// static float vert_angle_last;
|
|||
|
|
#ifdef PTZ_VERT_GET_ANGLE_MANY
|
|||
|
|
for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
vert_angle[i] = as5047d_vert_get_angle_a();
|
|||
|
|
|
|||
|
|
// test_angle[test_angle_count] = vert_angle[i];
|
|||
|
|
// test_angle_count ++;
|
|||
|
|
// if(test_angle_count >= 500)
|
|||
|
|
// {
|
|||
|
|
// test_angle_count = 0;
|
|||
|
|
// }
|
|||
|
|
//
|
|||
|
|
if(vert_angle[i] < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
vert_angle[i] > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(vert_angle[i]) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
i --;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ڴű<DAB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD>ֹ<EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>ӽ<EFBFBD>360<36>ȣ<EFBFBD><C8A3>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>0.2<EFBFBD><EFBFBD>0.1<EFBFBD><EFBFBD>359.9<EFBFBD><EFBFBD>359.8<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݾ<EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
flag = 0;
|
|||
|
|
for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
if(fabs(vert_angle[i] - vert_angle[i+1]) > 350.0)
|
|||
|
|
{
|
|||
|
|
flag = 1;//˵<><CBB5><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if(flag == 1)
|
|||
|
|
{
|
|||
|
|
for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
if(vert_angle[i] < 5.0)
|
|||
|
|
{
|
|||
|
|
vert_angle[i] = vert_angle[i] + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD>ĽǶ<C4BD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
|
|||
|
|
for(j = 0; j < PTZ_VERT_ANGLE_READ_NUM-1; j++)//<2F><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
for(k = 0; k < PTZ_VERT_ANGLE_READ_NUM-j-1; k++)
|
|||
|
|
{
|
|||
|
|
if(vert_angle[k] > vert_angle[k+1])
|
|||
|
|
{
|
|||
|
|
tem = vert_angle[k];
|
|||
|
|
vert_angle[k] = vert_angle[k+1];
|
|||
|
|
vert_angle[k+1] = tem;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
tem = 0;
|
|||
|
|
for(uint8_t i = 1; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
tem = tem + vert_angle[i];//ȥ<><C8A5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Сֵ
|
|||
|
|
}
|
|||
|
|
tem = tem / ((float)(PTZ_VERT_ANGLE_READ_NUM - 2));
|
|||
|
|
|
|||
|
|
if(tem > 360.0)
|
|||
|
|
{
|
|||
|
|
tem = tem - 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#ifdef PTZ_VERT_GET_ANGLE_SINGLE
|
|||
|
|
tem = as5047d_vert_get_angle_a();
|
|||
|
|
|
|||
|
|
if(tem < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
tem > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(tem) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>¶<EFBFBD>ȡ
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_data_reset == 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_num = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_data_reset = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
|
|||
|
|
tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
vert_cycle_angle_up = tem1 + 350.0;
|
|||
|
|
vert_cycle_angle_down = tem1 + 10.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.vert_ps_sw1_up_rise > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw1_up_rise = 0;
|
|||
|
|
}
|
|||
|
|
if( g_ptz.vert_ps_sw1_down_fall > 0)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
|
|||
|
|
ptz_vert_ps_delay_ms();
|
|||
|
|
if(PS_VERT_SW1_READ == PS_COVER)
|
|||
|
|
{
|
|||
|
|
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_angle_state = 1;
|
|||
|
|
g_ptz.vert_ps_sw1_down_fall = 0;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
g_ptz.vert_ps_sw1_down_fall = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// if(g_ptz.vert_ps_sw2_down_rise > 0)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num;
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_angle_actual + g_ptz.vert_as5047d.as5047d_remain_angle;
|
|||
|
|
// if(g_ptz.vert_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
// }
|
|||
|
|
// g_ptz.vert_ps_sw2_down_rise = 0;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_down_rise > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw2_down_rise = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_up_fall > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw2_up_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
vert_cycle_angle_up = tem1 + 350.0;
|
|||
|
|
vert_cycle_angle_down = tem1 + 10.0;
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
down_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
down_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& down_diff_x1 <= PTZ_DOWN_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& down_diff_x2 <= PTZ_DOWN_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>ű<F3A3ACB4><C5B1><EFBFBD><EFBFBD>Ƕȼ<C7B6>С
|
|||
|
|
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
// if(g_ptz.vert_ps_sw1_up_rise > 0)
|
|||
|
|
// {
|
|||
|
|
// 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_ps_sw1_up_rise = 0;
|
|||
|
|
// }
|
|||
|
|
if(g_ptz.vert_ps_sw1_up_rise > 0)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_SW1_UP_RISE_UPDATE
|
|||
|
|
ptz_vert_ps_delay_ms();
|
|||
|
|
if(PS_VERT_SW1_READ == PS_NO_COVER)
|
|||
|
|
{
|
|||
|
|
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_angle_state = 1;
|
|||
|
|
g_ptz.vert_ps_sw1_up_rise = 0;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
g_ptz.vert_ps_sw1_up_rise = 0;
|
|||
|
|
}
|
|||
|
|
if( g_ptz.vert_ps_sw1_down_fall > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw1_down_fall = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_down_rise > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw2_down_rise = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_up_fall > 0)
|
|||
|
|
{
|
|||
|
|
ptz_vert_ps_delay_ms();
|
|||
|
|
if(PS_VERT_SW2_READ == PS_COVER)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual + g_ptz.vert_as5047d.as5047d_remain_angle;
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_angle_state = 1;
|
|||
|
|
g_ptz.vert_ps_sw2_up_fall = 0;
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_ps_sw2_up_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
vert_cycle_angle_up = tem1 + 350.0;
|
|||
|
|
vert_cycle_angle_down = tem1 + 10.0;
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
up_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
up_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual) +
|
|||
|
|
fabs(g_ptz.vert_as5047d.as5047d_ptz_init_angle - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual <= g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& up_diff_x1 <= PTZ_UP_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual >= g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& up_diff_x2 <= PTZ_UP_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>㵱ǰ<E3B5B1>Ƕ<EFBFBD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
fabs(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual) +
|
|||
|
|
fabs(g_ptz.vert_as5047d.as5047d_ptz_init_angle - 0) +
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
(g_ptz.vert_as5047d.as5047d_ptz_init_angle -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual) +
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual == g_ptz.vert_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĵű<C4B4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܽǶȽ<C7B6><C8BD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(fabs(g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last)
|
|||
|
|
< PTZ_VERT_AS5047D_ANGLE_WF_A)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>̨<EFBFBD>ǶȽ<C7B6><C8BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3λС<CEBB><D0A1>
|
|||
|
|
tem3 = (long int)((g_ptz.vert_as5047d.as5047d_ptz_angle_K *
|
|||
|
|
(g_ptz.vert_as5047d.as5047d_ptz_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_max / 2.0)) * 1000.0 + 0.5);
|
|||
|
|
|
|||
|
|
|
|||
|
|
tem4 = tem3 / 1000.0;
|
|||
|
|
g_ptz.vert_angle_actual_a = tem4;
|
|||
|
|
if(g_ptz.offset_angle.vert_offset_switch == 2)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 - g_ptz.offset_angle.vert_zero_offset_angle;
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_angle_actual = tem4;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>浱ǰ<E6B5B1>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD>ֵ
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_last = g_ptz.vert_as5047d.as5047d_angle_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_last = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
|
|||
|
|
g_ptz.vert_angle_last = g_ptz.vert_angle_actual;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static char ptz_vert_get_angle_a()
|
|||
|
|
{
|
|||
|
|
float up_diff_x1 = 0;
|
|||
|
|
float up_diff_x2 = 0;
|
|||
|
|
float x1 = 0, x2 = 0, x3 = 0;
|
|||
|
|
float vert_angle[PTZ_VERT_ANGLE_READ_NUM] = {0};
|
|||
|
|
char i = 0, j = 0, k = 0, m = 0;
|
|||
|
|
float tem = 0;
|
|||
|
|
float tem1 = 0;
|
|||
|
|
static float vert_cycle_angle_up;
|
|||
|
|
static float vert_cycle_angle_down;
|
|||
|
|
long int tem3 = 0;
|
|||
|
|
float down_diff_x1 = 0;
|
|||
|
|
float down_diff_x2 = 0;
|
|||
|
|
float tem4 = 0;
|
|||
|
|
char flag;
|
|||
|
|
int cycle_count_save = 0;
|
|||
|
|
// static float vert_angle_actual;
|
|||
|
|
// static float vert_angle_last;
|
|||
|
|
|
|||
|
|
|
|||
|
|
g_ptz.vert_as5047d.time_end = g_ptz.vert_as5047d.time_now;
|
|||
|
|
g_ptz.vert_as5047d.time_now = OSTimeGet();
|
|||
|
|
g_ptz.vert_as5047d.time = time_ticks(g_ptz.vert_as5047d.time_now, g_ptz.vert_as5047d.time_end);
|
|||
|
|
|
|||
|
|
#ifdef CLK_50M
|
|||
|
|
if(g_ptz.vert_as5047d.time < 2)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.time = 2;
|
|||
|
|
}
|
|||
|
|
#else
|
|||
|
|
if(g_ptz.vert_as5047d.time < 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.time = 1;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
|
|||
|
|
#ifdef PTZ_VERT_GET_ANGLE_MANY
|
|||
|
|
for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
vert_angle[i] = as5047d_vert_get_angle_a();
|
|||
|
|
|
|||
|
|
// test_angle[test_angle_count] = vert_angle[i];
|
|||
|
|
// test_angle_count ++;
|
|||
|
|
// if(test_angle_count >= 500)
|
|||
|
|
// {
|
|||
|
|
// test_angle_count = 0;
|
|||
|
|
// }
|
|||
|
|
//
|
|||
|
|
if(vert_angle[i] < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
vert_angle[i] > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(vert_angle[i]) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
i --;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ڴű<DAB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD>ֹ<EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>ӽ<EFBFBD>360<36>ȣ<EFBFBD><C8A3>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>0.2<EFBFBD><EFBFBD>0.1<EFBFBD><EFBFBD>359.9<EFBFBD><EFBFBD>359.8<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݾ<EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
flag = 0;
|
|||
|
|
for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
if(fabs(vert_angle[i] - vert_angle[i+1]) > 350.0)
|
|||
|
|
{
|
|||
|
|
flag = 1;//˵<><CBB5><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if(flag == 1)
|
|||
|
|
{
|
|||
|
|
for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
if(vert_angle[i] < 5.0)
|
|||
|
|
{
|
|||
|
|
vert_angle[i] = vert_angle[i] + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD>ĽǶ<C4BD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
|
|||
|
|
for(j = 0; j < PTZ_VERT_ANGLE_READ_NUM-1; j++)//<2F><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
for(k = 0; k < PTZ_VERT_ANGLE_READ_NUM-j-1; k++)
|
|||
|
|
{
|
|||
|
|
if(vert_angle[k] > vert_angle[k+1])
|
|||
|
|
{
|
|||
|
|
tem = vert_angle[k];
|
|||
|
|
vert_angle[k] = vert_angle[k+1];
|
|||
|
|
vert_angle[k+1] = tem;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
tem = 0;
|
|||
|
|
for(uint8_t i = 1; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
tem = tem + vert_angle[i];//ȥ<><C8A5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Сֵ
|
|||
|
|
}
|
|||
|
|
tem = tem / ((float)(PTZ_VERT_ANGLE_READ_NUM - 2));
|
|||
|
|
|
|||
|
|
if(tem > 360.0)
|
|||
|
|
{
|
|||
|
|
tem = tem - 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#ifdef PTZ_VERT_GET_ANGLE_SINGLE
|
|||
|
|
// tem = as5047d_vert_get_angle_a();
|
|||
|
|
//
|
|||
|
|
// if(tem < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
// tem > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
// isnan(tem) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
// {//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>¶<EFBFBD>ȡ
|
|||
|
|
// return 0;
|
|||
|
|
// }
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
|
|||
|
|
|
|||
|
|
for(m = 0; m < 5; m++)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
tem = as5047d_vert_get_angle_a();
|
|||
|
|
|
|||
|
|
if(tem < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
tem > 360.0 ||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(tem) == 1 ||
|
|||
|
|
((fabs(tem - g_ptz.vert_as5047d.as5047d_angle_last) > PTZ_VERT_ANGLE_DIFF_A) && (fabs(tem - g_ptz.vert_as5047d.as5047d_angle_last) < PTZ_VERT_ANGLE_DIFF_B)))//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>¶<EFBFBD>ȡ
|
|||
|
|
asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_data_reset == 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_num = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_data_reset = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
|
|||
|
|
tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
vert_cycle_angle_up = tem1 + 350.0;
|
|||
|
|
vert_cycle_angle_down = tem1 + 10.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_last);
|
|||
|
|
|
|||
|
|
x2 = (g_ptz.vert_as5047d.as5047d_angle_actual - 0) +
|
|||
|
|
(360.0 - g_ptz.vert_as5047d.as5047d_angle_last);
|
|||
|
|
|
|||
|
|
x3 = (g_ptz.vert_as5047d.as5047d_angle_last - 0) +
|
|||
|
|
(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
|
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_angle_last
|
|||
|
|
&& x1 < x3)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_FWD;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_angle_last
|
|||
|
|
&& x2 < x1)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_FWD;
|
|||
|
|
}
|
|||
|
|
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ƕȼ<C7B6>С
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_angle_last
|
|||
|
|
&& x3 < x1)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_REV;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_angle_last
|
|||
|
|
&& x1 < x2)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_REV;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(x1 == x2 || x1 == x3)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_FWD;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_REV;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_dir == AS5047D_DIR_FWD)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.vert_ps_sw1_up_rise > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw1_up_rise = 0;
|
|||
|
|
}
|
|||
|
|
if( g_ptz.vert_ps_sw1_down_fall > 0)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
|
|||
|
|
ptz_vert_ps_delay_ms();
|
|||
|
|
if(PS_VERT_SW1_READ == PS_COVER)
|
|||
|
|
{
|
|||
|
|
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_angle_state = 1;
|
|||
|
|
g_ptz.vert_ps_sw1_down_fall = 0;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
g_ptz.vert_ps_sw1_down_fall = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_down_rise > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw2_down_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_ps_sw2_up_fall > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw2_up_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
vert_cycle_angle_up = tem1 + 350.0;
|
|||
|
|
vert_cycle_angle_down = tem1 + 10.0;
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
down_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
down_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& down_diff_x1 <= PTZ_DOWN_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& down_diff_x2 <= PTZ_DOWN_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>ű<F3A3ACB4><C5B1><EFBFBD><EFBFBD>Ƕȼ<C7B6>С
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_dir == AS5047D_DIR_REV)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
// if(g_ptz.vert_ps_sw1_up_rise > 0)
|
|||
|
|
// {
|
|||
|
|
// 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_ps_sw1_up_rise = 0;
|
|||
|
|
// }
|
|||
|
|
if(g_ptz.vert_ps_sw1_up_rise > 0)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_SW1_UP_RISE_UPDATE
|
|||
|
|
ptz_vert_ps_delay_ms();
|
|||
|
|
if(PS_VERT_SW1_READ == PS_NO_COVER)
|
|||
|
|
{
|
|||
|
|
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_angle_state = 1;
|
|||
|
|
g_ptz.vert_ps_sw1_up_rise = 0;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
g_ptz.vert_ps_sw1_up_rise = 0;
|
|||
|
|
}
|
|||
|
|
if( g_ptz.vert_ps_sw1_down_fall > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw1_down_fall = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_down_rise > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw2_down_rise = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_up_fall > 0)
|
|||
|
|
{
|
|||
|
|
ptz_vert_ps_delay_ms();
|
|||
|
|
if(PS_VERT_SW2_READ == PS_COVER)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual + g_ptz.vert_as5047d.as5047d_remain_angle;
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_angle_state = 1;
|
|||
|
|
g_ptz.vert_ps_sw2_up_fall = 0;
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_ps_sw2_up_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
vert_cycle_angle_up = tem1 + 350.0;
|
|||
|
|
vert_cycle_angle_down = tem1 + 10.0;
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
up_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
up_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual) +
|
|||
|
|
fabs(g_ptz.vert_as5047d.as5047d_ptz_init_angle - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual <= g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& up_diff_x1 <= PTZ_UP_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual >= g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& up_diff_x2 <= PTZ_UP_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>浱ǰ<E6B5B1>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6>
|
|||
|
|
cycle_count_save = g_ptz.vert_as5047d.as5047d_cycle_count;
|
|||
|
|
|
|||
|
|
for(i = 0; i < 4; i++)
|
|||
|
|
{//<2F><><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>Ϊ<EFBFBD>˷<EFBFBD>ֹ<EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<F3A3ACBB><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĵű<C4B4><C5B1><EFBFBD>Ȧ<EFBFBD><C8A6>
|
|||
|
|
//<2F><><EFBFBD>㵱ǰ<E3B5B1>Ƕ<EFBFBD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
fabs(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual) +
|
|||
|
|
fabs(g_ptz.vert_as5047d.as5047d_ptz_init_angle - 0) +
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
(g_ptz.vert_as5047d.as5047d_ptz_init_angle -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual) +
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual == g_ptz.vert_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĵű<C4B4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܽǶȽ<C7B6><C8BD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(fabs(g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last)
|
|||
|
|
< PTZ_VERT_AS5047D_ANGLE_WF_A)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>жϣ<D0B6><CFA3>ָ<EFBFBD>ʵ<EFBFBD>ʵĴű<C4B4><C5B1><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6>
|
|||
|
|
switch(i)
|
|||
|
|
{
|
|||
|
|
case 0:
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case 1:
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case 2: //<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>Ȧ<EFBFBD><C8A6><EFBFBD>ָ<EFBFBD>ʧ<EFBFBD><CAA7>
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case 3:
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>̨<EFBFBD>ǶȽ<C7B6><C8BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3λС<CEBB><D0A1>
|
|||
|
|
tem3 = (long int)((g_ptz.vert_as5047d.as5047d_ptz_angle_K *
|
|||
|
|
(g_ptz.vert_as5047d.as5047d_ptz_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_max / 2.0)) * 1000.0 + 0.5);
|
|||
|
|
|
|||
|
|
|
|||
|
|
tem4 = tem3 / 1000.0;
|
|||
|
|
g_ptz.vert_angle_actual_a = tem4;
|
|||
|
|
if(g_ptz.offset_angle.vert_offset_switch == 2)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 - g_ptz.offset_angle.vert_zero_offset_angle;
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_angle_actual = tem4;
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>浱ǰ<E6B5B1>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD>ֵ
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_last = g_ptz.vert_as5047d.as5047d_angle_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_last = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
|
|||
|
|
g_ptz.vert_angle_last = g_ptz.vert_angle_actual;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
|
|||
|
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨ˮƽ<CBAE>Ƕ<EFBFBD>
|
|||
|
|
#ifdef PTZ_HORI_ANGLE_AS5047D_ANGLE_SYN
|
|||
|
|
static char ptz_hori_get_angle()
|
|||
|
|
{
|
|||
|
|
float right_diff_x1 = 0;
|
|||
|
|
float right_diff_x2 = 0;
|
|||
|
|
|
|||
|
|
float hori_angle[PTZ_HORI_ANGLE_READ_NUM] = {0};
|
|||
|
|
char i = 0, j = 0, k = 0;
|
|||
|
|
float tem = 0;
|
|||
|
|
float tem1 = 0;
|
|||
|
|
float tem2 = 0;
|
|||
|
|
float left_diff_x1 = 0;
|
|||
|
|
float left_diff_x2 = 0;
|
|||
|
|
unsigned int tem3 = 0;
|
|||
|
|
float tem4 = 0;
|
|||
|
|
static float hori_cycle_angle_right;
|
|||
|
|
static float hori_cycle_angle_left;
|
|||
|
|
char flag;
|
|||
|
|
#ifdef PTZ_HORI_GET_ANGLE_MANY
|
|||
|
|
for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
hori_angle[i] = as5047d_hori_get_angle_a();
|
|||
|
|
if(hori_angle[i] < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
hori_angle[i] > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(hori_angle[i]) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
i --;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ڴű<DAB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD>ֹ<EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>ӽ<EFBFBD>360<36>ȣ<EFBFBD><C8A3>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>0.2<EFBFBD><EFBFBD>0.1<EFBFBD><EFBFBD>359.9<EFBFBD><EFBFBD>359.8<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݾ<EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
flag = 0;
|
|||
|
|
for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
if(fabs(hori_angle[i] - hori_angle[i+1]) > 350.0)
|
|||
|
|
{
|
|||
|
|
flag = 1;//˵<><CBB5><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if(flag == 1)
|
|||
|
|
{
|
|||
|
|
for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
if(hori_angle[i] < 5.0)
|
|||
|
|
{
|
|||
|
|
hori_angle[i] = hori_angle[i] + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD>ĽǶ<C4BD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
|
|||
|
|
for(j = 0; j < PTZ_HORI_ANGLE_READ_NUM-1; j++)//<2F><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
for(k = 0; k < PTZ_HORI_ANGLE_READ_NUM-j-1; k++)
|
|||
|
|
{
|
|||
|
|
if(hori_angle[k] > hori_angle[k+1])
|
|||
|
|
{
|
|||
|
|
tem = hori_angle[k];
|
|||
|
|
hori_angle[k] = hori_angle[k+1];
|
|||
|
|
hori_angle[k+1] = tem;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
tem = 0;
|
|||
|
|
for(uint8_t i = 1; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
tem = tem + hori_angle[i];//ȥ<><C8A5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Сֵ
|
|||
|
|
}
|
|||
|
|
tem = tem / ((float)(PTZ_HORI_ANGLE_READ_NUM - 2));
|
|||
|
|
|
|||
|
|
if(tem > 360.0)
|
|||
|
|
{
|
|||
|
|
tem = tem - 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#ifdef PTZ_HORI_GET_ANGLE_SINGLE
|
|||
|
|
tem = as5047d_hori_get_angle_a();
|
|||
|
|
|
|||
|
|
if(tem < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
tem > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(tem) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>¶<EFBFBD>ȡ
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
#endif
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_data_reset == 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_num = 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_data_reset = 0;
|
|||
|
|
tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
hori_cycle_angle_right = tem1 + 350.0;
|
|||
|
|
hori_cycle_angle_left = tem1 + 10.0;
|
|||
|
|
}
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD>Ƕȼ<C7B6>С
|
|||
|
|
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
// if(g_ptz.hori_ps_sw3_right_fall >= 1)
|
|||
|
|
// {
|
|||
|
|
// if(PS_HORI_SW3_READ == PS_COVER)
|
|||
|
|
// {
|
|||
|
|
// 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_ps_sw3_right_fall = 0;
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_rise >= 1)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_SW3_LEFT_RISE_UPDATE
|
|||
|
|
ptz_hori_ps_delay_ms();
|
|||
|
|
if(PS_HORI_SW3_READ == PS_NO_COVER)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_angle_state = 1;
|
|||
|
|
g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_fall >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_right_fall = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_rise >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_right_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_fall >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_left_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
hori_cycle_angle_right = tem1 + 350.0;
|
|||
|
|
hori_cycle_angle_left = tem1 + 10.0;
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
left_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
left_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual) +
|
|||
|
|
fabs(g_ptz.hori_as5047d.as5047d_ptz_init_angle - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& left_diff_x1 <= PTZ_LEFT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& left_diff_x2 <= PTZ_LEFT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>ű<F3A3ACB4><C5B1><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
// if(g_ptz.hori_ps_sw3_left_rise >= 1)
|
|||
|
|
// {
|
|||
|
|
// if(PS_HORI_SW3_READ == PS_NO_COVER)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle;
|
|||
|
|
// if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
// }
|
|||
|
|
// g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_fall >= 1)
|
|||
|
|
{
|
|||
|
|
ptz_hori_ps_delay_ms();
|
|||
|
|
if(PS_HORI_SW3_READ == PS_COVER)
|
|||
|
|
{
|
|||
|
|
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_ps_sw3_right_fall = 0;
|
|||
|
|
g_ptz.hori_angle_state = 1;
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_ps_sw3_right_fall = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_rise >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_rise >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_right_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_fall >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_left_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
hori_cycle_angle_right = tem1 + 350.0;
|
|||
|
|
hori_cycle_angle_left = tem1 + 10.0;
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
right_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
right_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual >= g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& right_diff_x1 <= PTZ_RIGHT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual <= g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& right_diff_x2 <= PTZ_RIGHT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>㵱ǰ<E3B5B1>Ƕ<EFBFBD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
(g_ptz.hori_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
fabs(360.0 - g_ptz.hori_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 0) +
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual == g_ptz.hori_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Դű<D4B4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǶȽ<C7B6><C8BD><EFBFBD><EFBFBD>˲<EFBFBD>
|
|||
|
|
if((fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last)
|
|||
|
|
< PTZ_HORI_AS5047D_ANGLE_WF_A) ||
|
|||
|
|
(fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last)
|
|||
|
|
> PTZ_HORI_AS5047D_ANGLE_WF_B))
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><>ֹ<EFBFBD>Ƕȷ<C7B6>Χ<EFBFBD><CEA7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_angle_max)
|
|||
|
|
{
|
|||
|
|
tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_max);
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual - tem2;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual < 0)
|
|||
|
|
{
|
|||
|
|
tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - 0);
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual +
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle + tem2;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual == g_ptz.hori_as5047d.as5047d_ptz_angle_max
|
|||
|
|
|| g_ptz.hori_as5047d.as5047d_ptz_angle_actual == 0)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>̨<EFBFBD>ǶȽ<C7B6><C8BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3λС<CEBB><D0A1>
|
|||
|
|
tem3 = (unsigned int)((g_ptz.hori_as5047d.as5047d_ptz_angle_actual *
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_K) * 1000.0 + 0.5);
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
tem4 = tem3 / 1000.0;
|
|||
|
|
g_ptz.hori_angle_actual_a = tem4;
|
|||
|
|
if(g_ptz.offset_angle.hori_offset_switch == 2)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 - g_ptz.offset_angle.hori_zero_offset_angle;
|
|||
|
|
if(tem4 < 0)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_angle_actual = tem4;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>浱ǰ<E6B5B1>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD>ֵ
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_last = g_ptz.hori_as5047d.as5047d_angle_actual;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_last = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
|
|||
|
|
g_ptz.hori_angle_last = g_ptz.hori_angle_actual;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static char ptz_hori_get_angle_a()
|
|||
|
|
{
|
|||
|
|
float right_diff_x1 = 0;
|
|||
|
|
float right_diff_x2 = 0;
|
|||
|
|
float x1 = 0, x2 = 0, x3 = 0;
|
|||
|
|
float hori_angle[PTZ_HORI_ANGLE_READ_NUM] = {0};
|
|||
|
|
char i = 0, j = 0, k = 0, m = 0;
|
|||
|
|
float tem = 0;
|
|||
|
|
float tem1 = 0;
|
|||
|
|
float tem2 = 0;
|
|||
|
|
float left_diff_x1 = 0;
|
|||
|
|
float left_diff_x2 = 0;
|
|||
|
|
unsigned int tem3 = 0;
|
|||
|
|
float tem4 = 0;
|
|||
|
|
static float hori_cycle_angle_right;
|
|||
|
|
static float hori_cycle_angle_left;
|
|||
|
|
char flag;
|
|||
|
|
int cycle_count_save = 0;
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.time_end = g_ptz.hori_as5047d.time_now;
|
|||
|
|
g_ptz.hori_as5047d.time_now = OSTimeGet();
|
|||
|
|
g_ptz.hori_as5047d.time = time_ticks(g_ptz.hori_as5047d.time_now, g_ptz.hori_as5047d.time_end);
|
|||
|
|
|
|||
|
|
#ifdef CLK_50M
|
|||
|
|
if(g_ptz.hori_as5047d.time < 2)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.time = 2;
|
|||
|
|
}
|
|||
|
|
#else
|
|||
|
|
if(g_ptz.hori_as5047d.time < 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.time = 1;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#ifdef PTZ_HORI_GET_ANGLE_MANY
|
|||
|
|
for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
hori_angle[i] = as5047d_hori_get_angle_a();
|
|||
|
|
if(hori_angle[i] < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
hori_angle[i] > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(hori_angle[i]) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
i --;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ڴű<DAB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD>ֹ<EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>ӽ<EFBFBD>360<36>ȣ<EFBFBD><C8A3>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>0.2<EFBFBD><EFBFBD>0.1<EFBFBD><EFBFBD>359.9<EFBFBD><EFBFBD>359.8<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݾ<EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
flag = 0;
|
|||
|
|
for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
if(fabs(hori_angle[i] - hori_angle[i+1]) > 350.0)
|
|||
|
|
{
|
|||
|
|
flag = 1;//˵<><CBB5><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if(flag == 1)
|
|||
|
|
{
|
|||
|
|
for(i = 0; i < PTZ_HORI_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
if(hori_angle[i] < 5.0)
|
|||
|
|
{
|
|||
|
|
hori_angle[i] = hori_angle[i] + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD>ĽǶ<C4BD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
|
|||
|
|
for(j = 0; j < PTZ_HORI_ANGLE_READ_NUM-1; j++)//<2F><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
for(k = 0; k < PTZ_HORI_ANGLE_READ_NUM-j-1; k++)
|
|||
|
|
{
|
|||
|
|
if(hori_angle[k] > hori_angle[k+1])
|
|||
|
|
{
|
|||
|
|
tem = hori_angle[k];
|
|||
|
|
hori_angle[k] = hori_angle[k+1];
|
|||
|
|
hori_angle[k+1] = tem;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
tem = 0;
|
|||
|
|
for(uint8_t i = 1; i < PTZ_HORI_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
tem = tem + hori_angle[i];//ȥ<><C8A5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Сֵ
|
|||
|
|
}
|
|||
|
|
tem = tem / ((float)(PTZ_HORI_ANGLE_READ_NUM - 2));
|
|||
|
|
|
|||
|
|
if(tem > 360.0)
|
|||
|
|
{
|
|||
|
|
tem = tem - 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#ifdef PTZ_HORI_GET_ANGLE_SINGLE
|
|||
|
|
for(m = 0; m < 5; m++)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
tem = as5047d_hori_get_angle_a();
|
|||
|
|
|
|||
|
|
if(tem < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
tem > 360.0 ||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(tem) == 1 ||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
((fabs(tem - g_ptz.hori_as5047d.as5047d_angle_last) > PTZ_HORI_ANGLE_DIFF_A) && (fabs(tem - g_ptz.hori_as5047d.as5047d_angle_last) < PTZ_HORI_ANGLE_DIFF_B)))
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>¶<EFBFBD>ȡ
|
|||
|
|
asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
|
|||
|
|
|
|||
|
|
// tem = as5047d_hori_get_angle_a();
|
|||
|
|
//
|
|||
|
|
// if(tem < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
// tem > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
// isnan(tem) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
// {//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>¶<EFBFBD>ȡ
|
|||
|
|
// return 0;
|
|||
|
|
// }
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
//
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_data_reset == 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_num = 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_data_reset = 0;
|
|||
|
|
tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
hori_cycle_angle_right = tem1 + 350.0;
|
|||
|
|
hori_cycle_angle_left = tem1 + 10.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_last);
|
|||
|
|
|
|||
|
|
x2 = (g_ptz.hori_as5047d.as5047d_angle_actual - 0) +
|
|||
|
|
(360.0 - g_ptz.hori_as5047d.as5047d_angle_last);
|
|||
|
|
|
|||
|
|
x3 = (g_ptz.hori_as5047d.as5047d_angle_last - 0) +
|
|||
|
|
(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
|
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_angle_last
|
|||
|
|
&& x1 < x3)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_FWD;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_angle_last
|
|||
|
|
&& x2 < x1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_FWD;
|
|||
|
|
}
|
|||
|
|
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ƕȼ<C7B6>С
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_angle_last
|
|||
|
|
&& x3 < x1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_REV;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_angle_last
|
|||
|
|
&& x1 < x2)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_REV;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(x1 == x2 || x1 == x3)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_REV;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_dir = AS5047D_DIR_FWD;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD>Ƕȼ<C7B6>С
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_dir == AS5047D_DIR_REV)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
// if(g_ptz.hori_ps_sw3_right_fall >= 1)
|
|||
|
|
// {
|
|||
|
|
// if(PS_HORI_SW3_READ == PS_COVER)
|
|||
|
|
// {
|
|||
|
|
// 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_ps_sw3_right_fall = 0;
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_rise >= 1)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_SW3_LEFT_RISE_UPDATE
|
|||
|
|
if(g_ptz.hori_angle_erro_switch == 0)
|
|||
|
|
{//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>عرգ<D8B1>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>ؽǶȸ<C7B6><C8B8>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
|
|||
|
|
ptz_hori_ps_delay_ms();
|
|||
|
|
if(PS_HORI_SW3_READ == PS_NO_COVER)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_angle_state = 1;
|
|||
|
|
g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
ptz_printf("\n hori_ps_sw3_left_rise angle max update!! \r\n",strlen((const char *)"\n hori_ps_sw3_left_rise angle max update!! \r\n"));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_fall >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_right_fall = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_rise >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_right_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_fall >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_left_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
hori_cycle_angle_right = tem1 + 350.0;
|
|||
|
|
hori_cycle_angle_left = tem1 + 10.0;
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
left_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
left_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_angle_actual) +
|
|||
|
|
fabs(g_ptz.hori_as5047d.as5047d_ptz_init_angle - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& left_diff_x1 <= PTZ_LEFT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& left_diff_x2 <= PTZ_LEFT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual < hori_cycle_angle_left)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>ű<F3A3ACB4><C5B1><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_dir == AS5047D_DIR_FWD)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
// if(g_ptz.hori_ps_sw3_left_rise >= 1)
|
|||
|
|
// {
|
|||
|
|
// if(PS_HORI_SW3_READ == PS_NO_COVER)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle;
|
|||
|
|
// if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
// g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
// }
|
|||
|
|
// g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
// }
|
|||
|
|
// }
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_fall >= 1)
|
|||
|
|
{
|
|||
|
|
ptz_hori_ps_delay_ms();
|
|||
|
|
if(PS_HORI_SW3_READ == PS_COVER)
|
|||
|
|
{
|
|||
|
|
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_ps_sw3_right_fall = 0;
|
|||
|
|
g_ptz.hori_angle_state = 1;
|
|||
|
|
ptz_printf("\n hori_ps_sw3_right_fall angle min update!! \r\n",strlen((const char *)"\n hori_ps_sw3_right_fall angle min update!! \r\n"));
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_ps_sw3_right_fall = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_rise >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_left_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_right_rise >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_right_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_ps_sw3_left_fall >= 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_ps_sw3_left_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
tem1 = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
hori_cycle_angle_right = tem1 + 350.0;
|
|||
|
|
hori_cycle_angle_left = tem1 + 10.0;
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
right_diff_x1 = fabs(g_ptz.hori_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
right_diff_x2 = fabs(360.0 - g_ptz.hori_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual >= g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& right_diff_x1 <= PTZ_RIGHT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual <= g_ptz.hori_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& right_diff_x2 <= PTZ_RIGHT_CYCLE_C
|
|||
|
|
&& g_ptz.hori_as5047d.as5047d_ptz_angle_actual > hori_cycle_angle_right)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>浱ǰ<E6B5B1>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6>
|
|||
|
|
cycle_count_save = g_ptz.hori_as5047d.as5047d_cycle_count;
|
|||
|
|
|
|||
|
|
for(i = 0; i < 4; i++)
|
|||
|
|
{//<2F><><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>Ϊ<EFBFBD>˷<EFBFBD>ֹ<EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<F3A3ACBB><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĵű<C4B4><C5B1><EFBFBD>Ȧ<EFBFBD><C8A6>
|
|||
|
|
//<2F><><EFBFBD>㵱ǰ<E3B5B1>Ƕ<EFBFBD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
(g_ptz.hori_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual < g_ptz.hori_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
fabs(360.0 - g_ptz.hori_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
fabs(g_ptz.hori_as5047d.as5047d_angle_actual - 0) +
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_angle_actual == g_ptz.hori_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.hori_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Դű<D4B4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǶȽ<C7B6><C8BD><EFBFBD><EFBFBD>˲<EFBFBD>
|
|||
|
|
if((fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last)
|
|||
|
|
< PTZ_HORI_AS5047D_ANGLE_WF_A) ||
|
|||
|
|
(fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last)
|
|||
|
|
> PTZ_HORI_AS5047D_ANGLE_WF_B))
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>жϣ<D0B6><CFA3>ָ<EFBFBD>ʵ<EFBFBD>ʵĴű<C4B4><C5B1><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6>
|
|||
|
|
switch(i)
|
|||
|
|
{
|
|||
|
|
case 0:
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save - 1;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
g_ptz.hori_jia ++;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case 1:
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save + 1;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
g_ptz.hori_jian ++;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case 2: //<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>Ȧ<EFBFBD><C8A6><EFBFBD>ָ<EFBFBD>ʧ<EFBFBD><CAA7>
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save;
|
|||
|
|
g_ptz.hori_cuowu ++;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case 3:
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_wf_last = g_ptz.hori_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><>ֹ<EFBFBD>Ƕȷ<C7B6>Χ<EFBFBD><CEA7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual > g_ptz.hori_as5047d.as5047d_ptz_angle_max)
|
|||
|
|
{
|
|||
|
|
tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_max);
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_actual - g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = (int)(g_ptz.hori_as5047d.as5047d_ptz_angle_actual / 360.0);
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual - tem2;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual < 0)
|
|||
|
|
{
|
|||
|
|
tem2 = fabs(g_ptz.hori_as5047d.as5047d_ptz_angle_actual - 0);
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_actual + g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = (int)(g_ptz.hori_as5047d.as5047d_ptz_angle_actual / 360.0);
|
|||
|
|
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle + tem2;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle >= 360.0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle - 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_angle_actual == g_ptz.hori_as5047d.as5047d_ptz_angle_max
|
|||
|
|
|| g_ptz.hori_as5047d.as5047d_ptz_angle_actual == 0)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle = g_ptz.hori_as5047d.as5047d_angle_actual;
|
|||
|
|
}
|
|||
|
|
else//<2F><>
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_actual = g_ptz.hori_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_cycle_count = g_ptz.hori_as5047d.as5047d_cycle_num;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual - g_ptz.hori_as5047d.as5047d_remain_angle;
|
|||
|
|
if(g_ptz.hori_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>̨<EFBFBD>ǶȽ<C7B6><C8BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3λС<CEBB><D0A1>
|
|||
|
|
tem3 = (unsigned int)((g_ptz.hori_as5047d.as5047d_ptz_angle_actual *
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_K) * 1000.0 + 0.5);
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
tem4 = tem3 / 1000.0;
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ش<EFBFBD><D8B4><EFBFBD>
|
|||
|
|
if(g_ptz.hori_angle_erro_switch == 1)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 + (g_ptz.offset_angle.offset_anle / 50.0);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,,,,ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>Ҫ<EFBFBD>رչ<D8B1><D5B9>翪<EFBFBD><E7BFAA><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>ؽǶȸ<C7B6><C8B8><EFBFBD>
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
g_ptz.hori_angle_actual_a = tem4;
|
|||
|
|
|
|||
|
|
if(g_ptz.offset_angle.hori_offset_switch == 2)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 - g_ptz.offset_angle.hori_zero_offset_angle;
|
|||
|
|
// if(tem4 < 0)
|
|||
|
|
// {
|
|||
|
|
// tem4 = tem4 + 360.0;
|
|||
|
|
// }
|
|||
|
|
}
|
|||
|
|
if(tem4 == 360.0)///////<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ360<36><30><EFBFBD><EFBFBD>ʾ<EFBFBD>Ƕ<EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
tem4 = 0;
|
|||
|
|
}
|
|||
|
|
if(tem4 < 0)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 + 360.0;
|
|||
|
|
}
|
|||
|
|
if(tem4>360.0)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 - 360.0;
|
|||
|
|
}
|
|||
|
|
g_ptz.hori_angle_actual = tem4;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>浱ǰ<E6B5B1>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD>ֵ
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_last = g_ptz.hori_as5047d.as5047d_angle_actual;
|
|||
|
|
g_ptz.hori_as5047d.as5047d_ptz_angle_last = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;
|
|||
|
|
g_ptz.hori_angle_last = g_ptz.hori_angle_actual;
|
|||
|
|
|
|||
|
|
|
|||
|
|
// step_speed_for_hori = g_ptz.hori_as5047d.as5047d_ptz_angle_last;//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD>
|
|||
|
|
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
|
|||
|
|
/// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ֱ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
#ifdef PTZ_VERT_ANGLE_AS5047D_ANGLE_SYN
|
|||
|
|
static char ptz_vert_get_angle()
|
|||
|
|
{
|
|||
|
|
float up_diff_x1 = 0;
|
|||
|
|
float up_diff_x2 = 0;
|
|||
|
|
|
|||
|
|
float vert_angle[PTZ_VERT_ANGLE_READ_NUM] = {0};
|
|||
|
|
char i = 0, j = 0, k = 0;
|
|||
|
|
float tem = 0;
|
|||
|
|
float tem1 = 0;
|
|||
|
|
static float vert_cycle_angle_up;
|
|||
|
|
static float vert_cycle_angle_down;
|
|||
|
|
long int tem3 = 0;
|
|||
|
|
float down_diff_x1 = 0;
|
|||
|
|
float down_diff_x2 = 0;
|
|||
|
|
float tem4 = 0;
|
|||
|
|
char flag;
|
|||
|
|
// static float vert_angle_actual;
|
|||
|
|
// static float vert_angle_last;
|
|||
|
|
#ifdef PTZ_VERT_GET_ANGLE_MANY
|
|||
|
|
for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
vert_angle[i] = as5047d_vert_get_angle_a();
|
|||
|
|
|
|||
|
|
// test_angle[test_angle_count] = vert_angle[i];
|
|||
|
|
// test_angle_count ++;
|
|||
|
|
// if(test_angle_count >= 500)
|
|||
|
|
// {
|
|||
|
|
// test_angle_count = 0;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
if(vert_angle[i] < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
vert_angle[i] > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(vert_angle[i]) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
i --;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ڴű<DAB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD>ֹ<EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>ӽ<EFBFBD>360<36>ȣ<EFBFBD><C8A3>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>0.2<EFBFBD><EFBFBD>0.1<EFBFBD><EFBFBD>359.9<EFBFBD><EFBFBD>359.8<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݾ<EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
flag = 0;
|
|||
|
|
for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
if(fabs(vert_angle[i] - vert_angle[i+1]) > 350.0)
|
|||
|
|
{
|
|||
|
|
flag = 1;//˵<><CBB5><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if(flag == 1)
|
|||
|
|
{
|
|||
|
|
for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
if(vert_angle[i] < 5.0)
|
|||
|
|
{
|
|||
|
|
vert_angle[i] = vert_angle[i] + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD>ĽǶ<C4BD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
|
|||
|
|
for(j = 0; j < PTZ_VERT_ANGLE_READ_NUM-1; j++)//<2F><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
for(k = 0; k < PTZ_VERT_ANGLE_READ_NUM-j-1; k++)
|
|||
|
|
{
|
|||
|
|
if(vert_angle[k] > vert_angle[k+1])
|
|||
|
|
{
|
|||
|
|
tem = vert_angle[k];
|
|||
|
|
vert_angle[k] = vert_angle[k+1];
|
|||
|
|
vert_angle[k+1] = tem;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
tem = 0;
|
|||
|
|
for(uint8_t i = 1; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
tem = tem + vert_angle[i];//ȥ<><C8A5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Сֵ
|
|||
|
|
}
|
|||
|
|
tem = tem / ((float)(PTZ_VERT_ANGLE_READ_NUM - 2));
|
|||
|
|
|
|||
|
|
if(tem > 360.0)
|
|||
|
|
{
|
|||
|
|
tem = tem - 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#ifdef PTZ_VERT_GET_ANGLE_SINGLE
|
|||
|
|
tem = as5047d_vert_get_angle_a();
|
|||
|
|
|
|||
|
|
if(tem < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
tem > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(tem) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>¶<EFBFBD>ȡ
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_data_reset == 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_num = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_data_reset = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
|
|||
|
|
tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
vert_cycle_angle_up = tem1 + 350.0;
|
|||
|
|
vert_cycle_angle_down = tem1 + 10.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD>Ƕȼ<C7B6>С
|
|||
|
|
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.vert_ps_sw1_up_rise > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw1_up_rise = 0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_ps_sw1_down_fall > 0)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
|
|||
|
|
ptz_vert_ps_delay_ms();
|
|||
|
|
if(PS_VERT_SW1_READ == PS_COVER)
|
|||
|
|
{
|
|||
|
|
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_angle_state = 1;
|
|||
|
|
g_ptz.vert_ps_sw1_down_fall = 0;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
g_ptz.vert_ps_sw1_down_fall = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// if(g_ptz.vert_ps_sw2_down_rise > 0)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num;
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_angle_actual - g_ptz.vert_as5047d.as5047d_remain_angle;
|
|||
|
|
// if(g_ptz.vert_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
// }
|
|||
|
|
// g_ptz.vert_ps_sw2_down_rise = 0;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_down_rise > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw2_down_rise = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_up_fall > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw2_up_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
vert_cycle_angle_up = tem1 + 350.0;
|
|||
|
|
vert_cycle_angle_down = tem1 + 10.0;
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
down_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
down_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual) +
|
|||
|
|
fabs(g_ptz.vert_as5047d.as5047d_ptz_init_angle - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& down_diff_x1 <= PTZ_DOWN_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& down_diff_x2 <= PTZ_DOWN_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>ű<F3A3ACB4><C5B1><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
// if(g_ptz.vert_ps_sw1_up_rise > 0)
|
|||
|
|
// {
|
|||
|
|
// 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_ps_sw1_up_rise = 0;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw1_up_rise > 0)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_SW1_UP_RISE_UPDATE
|
|||
|
|
ptz_vert_ps_delay_ms();
|
|||
|
|
if(PS_VERT_SW1_READ == PS_NO_COVER)
|
|||
|
|
{
|
|||
|
|
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_angle_state = 1;
|
|||
|
|
g_ptz.vert_ps_sw1_up_rise = 0;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
g_ptz.vert_ps_sw1_up_rise = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if( g_ptz.vert_ps_sw1_down_fall > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw1_down_fall = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_down_rise > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw2_down_rise = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_up_fall > 0)
|
|||
|
|
{
|
|||
|
|
ptz_vert_ps_delay_ms();
|
|||
|
|
if(PS_VERT_SW2_READ == PS_COVER)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual - g_ptz.vert_as5047d.as5047d_remain_angle;
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_angle_state = 1;
|
|||
|
|
g_ptz.vert_ps_sw2_up_fall = 0;
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_ps_sw2_up_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
vert_cycle_angle_up = tem1 + 350.0;
|
|||
|
|
vert_cycle_angle_down = tem1 + 10.0;
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
up_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
up_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual >= g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& up_diff_x1 <= PTZ_UP_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual <= g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& up_diff_x2 <= PTZ_UP_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>㵱ǰ<E3B5B1>Ƕ<EFBFBD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
(g_ptz.vert_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
fabs(360.0 - g_ptz.vert_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 0) +
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual == g_ptz.vert_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
/**/
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĵű<C4B4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܽǶȽ<C7B6><C8BD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(fabs(g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last)
|
|||
|
|
< PTZ_VERT_AS5047D_ANGLE_WF_A)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>̨<EFBFBD>ǶȽ<C7B6><C8BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3λС<CEBB><D0A1>
|
|||
|
|
tem3 = (long int)((g_ptz.vert_as5047d.as5047d_ptz_angle_K *
|
|||
|
|
(g_ptz.vert_as5047d.as5047d_ptz_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_max / 2.0)) * 1000.0 + 0.5);
|
|||
|
|
|
|||
|
|
tem4 = tem3 / 1000.0;
|
|||
|
|
g_ptz.vert_angle_actual_a = tem4;
|
|||
|
|
if(g_ptz.offset_angle.vert_offset_switch == 2)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 - g_ptz.offset_angle.vert_zero_offset_angle;
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_angle_actual = tem4;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>浱ǰ<E6B5B1>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD>ֵ
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_last = g_ptz.vert_as5047d.as5047d_angle_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_last = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
|
|||
|
|
g_ptz.vert_angle_last = g_ptz.vert_angle_actual;
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static char ptz_vert_get_angle_a()
|
|||
|
|
{
|
|||
|
|
float up_diff_x1 = 0;
|
|||
|
|
float up_diff_x2 = 0;
|
|||
|
|
float x1 = 0, x2 = 0, x3 = 0;
|
|||
|
|
float vert_angle[PTZ_VERT_ANGLE_READ_NUM] = {0};
|
|||
|
|
char i = 0, j = 0, k = 0, m = 0;
|
|||
|
|
float tem = 0;
|
|||
|
|
float tem1 = 0;
|
|||
|
|
static float vert_cycle_angle_up;
|
|||
|
|
static float vert_cycle_angle_down;
|
|||
|
|
long int tem3 = 0;
|
|||
|
|
float down_diff_x1 = 0;
|
|||
|
|
float down_diff_x2 = 0;
|
|||
|
|
float tem4 = 0;
|
|||
|
|
char flag;
|
|||
|
|
int cycle_count_save = 0;
|
|||
|
|
// static float vert_angle_actual;
|
|||
|
|
// static float vert_angle_last;
|
|||
|
|
|
|||
|
|
g_ptz.vert_as5047d.time_end = g_ptz.vert_as5047d.time_now;
|
|||
|
|
g_ptz.vert_as5047d.time_now = OSTimeGet();
|
|||
|
|
g_ptz.vert_as5047d.time = time_ticks(g_ptz.vert_as5047d.time_now, g_ptz.vert_as5047d.time_end);
|
|||
|
|
|
|||
|
|
#ifdef CLK_50M
|
|||
|
|
if(g_ptz.vert_as5047d.time < 2)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.time = 2;
|
|||
|
|
}
|
|||
|
|
#else
|
|||
|
|
if(g_ptz.vert_as5047d.time < 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.time = 1;
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
|
|||
|
|
#ifdef PTZ_VERT_GET_ANGLE_MANY
|
|||
|
|
for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
vert_angle[i] = as5047d_vert_get_angle_a();
|
|||
|
|
|
|||
|
|
// test_angle[test_angle_count] = vert_angle[i];
|
|||
|
|
// test_angle_count ++;
|
|||
|
|
// if(test_angle_count >= 500)
|
|||
|
|
// {
|
|||
|
|
// test_angle_count = 0;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
if(vert_angle[i] < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
vert_angle[i] > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(vert_angle[i]) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
i --;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ڴű<DAB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD>ֹ<EFBFBD>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD>ӽ<EFBFBD>360<36>ȣ<EFBFBD><C8A3>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>0.2<EFBFBD><EFBFBD>0.1<EFBFBD><EFBFBD>359.9<EFBFBD><EFBFBD>359.8<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݾ<EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
flag = 0;
|
|||
|
|
for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
if(fabs(vert_angle[i] - vert_angle[i+1]) > 350.0)
|
|||
|
|
{
|
|||
|
|
flag = 1;//˵<><CBB5><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0λ<30><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if(flag == 1)
|
|||
|
|
{
|
|||
|
|
for(i = 0; i < PTZ_VERT_ANGLE_READ_NUM; i++)
|
|||
|
|
{
|
|||
|
|
if(vert_angle[i] < 5.0)
|
|||
|
|
{
|
|||
|
|
vert_angle[i] = vert_angle[i] + 360.0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F>Բɼ<D4B2><C9BC><EFBFBD><EFBFBD>ĽǶ<C4BD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
|
|||
|
|
for(j = 0; j < PTZ_VERT_ANGLE_READ_NUM-1; j++)//<2F><><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
{
|
|||
|
|
for(k = 0; k < PTZ_VERT_ANGLE_READ_NUM-j-1; k++)
|
|||
|
|
{
|
|||
|
|
if(vert_angle[k] > vert_angle[k+1])
|
|||
|
|
{
|
|||
|
|
tem = vert_angle[k];
|
|||
|
|
vert_angle[k] = vert_angle[k+1];
|
|||
|
|
vert_angle[k+1] = tem;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
tem = 0;
|
|||
|
|
for(uint8_t i = 1; i < PTZ_VERT_ANGLE_READ_NUM - 1; i++)
|
|||
|
|
{
|
|||
|
|
tem = tem + vert_angle[i];//ȥ<><C8A5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Сֵ
|
|||
|
|
}
|
|||
|
|
tem = tem / ((float)(PTZ_VERT_ANGLE_READ_NUM - 2));
|
|||
|
|
|
|||
|
|
if(tem > 360.0)
|
|||
|
|
{
|
|||
|
|
tem = tem - 360.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#ifdef PTZ_VERT_GET_ANGLE_SINGLE
|
|||
|
|
// tem = as5047d_vert_get_angle_a();
|
|||
|
|
//
|
|||
|
|
// if(tem < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
// tem > 360.0||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
// isnan(tem) == 1)//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
// {//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>¶<EFBFBD>ȡ
|
|||
|
|
// return 0;
|
|||
|
|
// }
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
|
|||
|
|
for(m = 0; m < 5; m++)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
tem = as5047d_vert_get_angle_a();
|
|||
|
|
|
|||
|
|
if(tem < 0 || //<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ĽǶ<C4BD>Ϊ<EFBFBD><CEAA>ֵ
|
|||
|
|
tem > 360.0 ||//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>360
|
|||
|
|
isnan(tem) == 1 ||
|
|||
|
|
((fabs(tem - g_ptz.vert_as5047d.as5047d_angle_last) > PTZ_VERT_ANGLE_DIFF_A) && (fabs(tem - g_ptz.vert_as5047d.as5047d_angle_last) < PTZ_VERT_ANGLE_DIFF_B)))//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
|
|||
|
|
{//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD>¶<EFBFBD>ȡ
|
|||
|
|
asm("nop");asm("nop");asm("nop");asm("nop");asm("nop");
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual = tem;
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_data_reset == 1)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_num = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle = g_ptz.vert_as5047d.as5047d_angle_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_data_reset = 0;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = 0;
|
|||
|
|
tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
vert_cycle_angle_up = tem1 + 350.0;
|
|||
|
|
vert_cycle_angle_down = tem1 + 10.0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_last);
|
|||
|
|
|
|||
|
|
x2 = (g_ptz.vert_as5047d.as5047d_angle_actual - 0) +
|
|||
|
|
(360.0 - g_ptz.vert_as5047d.as5047d_angle_last);
|
|||
|
|
|
|||
|
|
x3 = (g_ptz.vert_as5047d.as5047d_angle_last - 0) +
|
|||
|
|
(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
|
|||
|
|
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_angle_last
|
|||
|
|
&& x1 < x3)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_FWD;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_angle_last
|
|||
|
|
&& x2 < x1)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_FWD;
|
|||
|
|
}
|
|||
|
|
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ƕȼ<C7B6>С
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_angle_last
|
|||
|
|
&& x3 < x1)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_REV;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_angle_last
|
|||
|
|
&& x1 < x2)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_REV;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(x1 == x2 || x1 == x3)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_REV;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_dir = AS5047D_DIR_FWD;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD>Ƕȼ<C7B6>С
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_dir == AS5047D_DIR_REV)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.vert_ps_sw1_up_rise > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw1_up_rise = 0;
|
|||
|
|
}
|
|||
|
|
if( g_ptz.vert_ps_sw1_down_fall > 0)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_SW1_DOWN_FALL_UPDATE
|
|||
|
|
ptz_vert_ps_delay_ms();
|
|||
|
|
if(PS_VERT_SW1_READ == PS_COVER)
|
|||
|
|
{
|
|||
|
|
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_angle_state = 1;
|
|||
|
|
g_ptz.vert_ps_sw1_down_fall = 0;
|
|||
|
|
ptz_printf("\n vert_ps_sw1_down_fall angle min update!! \r\n",strlen((const char *)"\n vert_ps_sw1_down_fall angle min update!! \r\n"));
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
g_ptz.vert_ps_sw1_down_fall = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// if(g_ptz.vert_ps_sw2_down_rise > 0)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num;
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_angle_actual - g_ptz.vert_as5047d.as5047d_remain_angle;
|
|||
|
|
// if(g_ptz.vert_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
// {
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
// g_ptz.vert_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
// }
|
|||
|
|
// g_ptz.vert_ps_sw2_down_rise = 0;
|
|||
|
|
// }
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_down_rise > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw2_down_rise = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_up_fall > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw2_up_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
vert_cycle_angle_up = tem1 + 350.0;
|
|||
|
|
vert_cycle_angle_down = tem1 + 10.0;
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
down_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
down_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_angle_actual) +
|
|||
|
|
fabs(g_ptz.vert_as5047d.as5047d_ptz_init_angle - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& down_diff_x1 <= PTZ_DOWN_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& down_diff_x2 <= PTZ_DOWN_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual < vert_cycle_angle_down)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>ű<F3A3ACB4><C5B1><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_dir == AS5047D_DIR_FWD)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
|||
|
|
//<2F><>Ƭ<EFBFBD><C6AC><EFBFBD>½Ƕ<C2BD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
if(g_ptz.vert_ps_sw1_up_rise > 0)
|
|||
|
|
{
|
|||
|
|
#ifdef PTZ_SW1_UP_RISE_UPDATE
|
|||
|
|
ptz_vert_ps_delay_ms();
|
|||
|
|
if(PS_VERT_SW1_READ == PS_NO_COVER)
|
|||
|
|
{
|
|||
|
|
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_angle_state = 1;
|
|||
|
|
g_ptz.vert_ps_sw1_up_rise = 0;
|
|||
|
|
ptz_printf("\n vert_ps_sw1_up_rise angle min update!! \r\n",strlen((const char *)"\n vert_ps_sw1_up_rise angle min update!! \r\n"));
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
g_ptz.vert_ps_sw1_up_rise = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if( g_ptz.vert_ps_sw1_down_fall > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw1_down_fall = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_down_rise > 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_ps_sw2_down_rise = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_ps_sw2_up_fall > 0)
|
|||
|
|
{
|
|||
|
|
ptz_vert_ps_delay_ms();
|
|||
|
|
if(PS_VERT_SW2_READ == PS_COVER)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = g_ptz.vert_as5047d.as5047d_cycle_num;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_max;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual - g_ptz.vert_as5047d.as5047d_remain_angle;
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_ptz_init_angle < 0)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle =
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle + 360.0;
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_angle_state = 1;
|
|||
|
|
g_ptz.vert_ps_sw2_up_fall = 0;
|
|||
|
|
ptz_printf("\n vert_ps_sw2_up_fall angle max update!! \r\n",strlen((const char *)"\n vert_ps_sw2_up_fall angle max update!! \r\n"));
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_ps_sw2_up_fall = 0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
tem1 = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
vert_cycle_angle_up = tem1 + 350.0;
|
|||
|
|
vert_cycle_angle_down = tem1 + 10.0;
|
|||
|
|
//<2F><><EFBFBD>ݼ<EFBFBD><DDBC>㷽<EFBFBD><E3B7BD>b<EFBFBD><62><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ǰ<EFBFBD>Ƕ<EFBFBD>
|
|||
|
|
|
|||
|
|
//<2F>жϴű<CFB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD>ת<EFBFBD><D7AA>һȦ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>ȼ<EFBFBD><C8BC>㵱ǰ<E3B5B1>ǶȶȺͳ<C8BA>ʼ<EFBFBD>Ƕ<EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ĽǶȾ<C7B6><C8BE><EFBFBD>
|
|||
|
|
up_diff_x1 = fabs(g_ptz.vert_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle);
|
|||
|
|
|
|||
|
|
up_diff_x2 = fabs(360.0 - g_ptz.vert_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 0);
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual >= g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& up_diff_x1 <= PTZ_UP_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual <= g_ptz.vert_as5047d.as5047d_ptz_init_angle
|
|||
|
|
&& up_diff_x2 <= PTZ_UP_CYCLE_C
|
|||
|
|
&& g_ptz.vert_as5047d.as5047d_ptz_angle_actual > vert_cycle_angle_up)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>浱ǰ<E6B5B1>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6>
|
|||
|
|
cycle_count_save = g_ptz.vert_as5047d.as5047d_cycle_count;
|
|||
|
|
|
|||
|
|
for(i = 0; i < 4; i++)
|
|||
|
|
{//<2F><><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>Ϊ<EFBFBD>˷<EFBFBD>ֹ<EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<F3A3ACBB><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĵű<C4B4><C5B1><EFBFBD>Ȧ<EFBFBD><C8A6>
|
|||
|
|
//<2F><><EFBFBD>㵱ǰ<E3B5B1>Ƕ<EFBFBD>
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual > g_ptz.vert_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
(g_ptz.vert_as5047d.as5047d_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual < g_ptz.vert_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual =
|
|||
|
|
fabs(360.0 - g_ptz.vert_as5047d.as5047d_ptz_init_angle) +
|
|||
|
|
fabs(g_ptz.vert_as5047d.as5047d_angle_actual - 0) +
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_angle_actual == g_ptz.vert_as5047d.as5047d_ptz_init_angle)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual = g_ptz.vert_as5047d.as5047d_cycle_count * 360.0;
|
|||
|
|
}
|
|||
|
|
/**/
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĵű<C4B4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܽǶȽ<C7B6><C8BD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if(fabs(g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual - g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last)
|
|||
|
|
< PTZ_VERT_AS5047D_ANGLE_WF_A)
|
|||
|
|
{
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
break;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD>жϣ<D0B6><CFA3>ָ<EFBFBD>ʵ<EFBFBD>ʵĴű<C4B4><C5B1><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6>
|
|||
|
|
switch(i)
|
|||
|
|
{
|
|||
|
|
case 0:
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count --;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case 1:
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count ++;//ת<><D7AA>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case 2: //<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>Ȧ<EFBFBD><C8A6><EFBFBD>ָ<EFBFBD>ʧ<EFBFBD><CAA7>
|
|||
|
|
g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
case 3:
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_actual = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_wf_last = g_ptz.vert_as5047d.as5047d_ptz_angle_wf_actual;
|
|||
|
|
break;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
if(g_ptz.vert_as5047d.as5047d_state == 1)
|
|||
|
|
{
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>̨<EFBFBD>ǶȽ<C7B6><C8BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3λС<CEBB><D0A1>
|
|||
|
|
tem3 = (long int)((g_ptz.vert_as5047d.as5047d_ptz_angle_K *
|
|||
|
|
(g_ptz.vert_as5047d.as5047d_ptz_angle_actual -
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_max / 2.0)) * 1000.0 + 0.5);
|
|||
|
|
|
|||
|
|
|
|||
|
|
tem4 = tem3 / 1000.0;
|
|||
|
|
g_ptz.vert_angle_actual_a = tem4;
|
|||
|
|
if(g_ptz.offset_angle.vert_offset_switch == 2)
|
|||
|
|
{
|
|||
|
|
tem4 = tem4 - g_ptz.offset_angle.vert_zero_offset_angle;
|
|||
|
|
}
|
|||
|
|
g_ptz.vert_angle_actual = tem4;
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD>浱ǰ<E6B5B1>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD>ֵ
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_last = g_ptz.vert_as5047d.as5047d_angle_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_ptz_angle_last = g_ptz.vert_as5047d.as5047d_ptz_angle_actual;
|
|||
|
|
g_ptz.vert_angle_last = g_ptz.vert_angle_actual;
|
|||
|
|
|
|||
|
|
// step_speed_for_vert = g_ptz.vert_as5047d.as5047d_ptz_angle_last;//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȼ<D9B6><C8BC><EFBFBD>
|
|||
|
|
|
|||
|
|
return 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
static void ptz_get_angle_task()
|
|||
|
|
{
|
|||
|
|
while(1)
|
|||
|
|
{
|
|||
|
|
ptz_hori_get_angle_a();
|
|||
|
|
#ifdef CLK_50M
|
|||
|
|
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
|||
|
|
#endif
|
|||
|
|
ptz_vert_get_angle_a();
|
|||
|
|
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
void ptz_get_angle_init()
|
|||
|
|
{
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual = as5047d_hori_get_angle_a();
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_actual = as5047d_hori_get_angle_a();
|
|||
|
|
g_ptz.hori_as5047d.as5047d_angle_last = g_ptz.hori_as5047d.as5047d_angle_actual;
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual = as5047d_vert_get_angle_a();
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_actual = as5047d_vert_get_angle_a();
|
|||
|
|
g_ptz.vert_as5047d.as5047d_angle_last = g_ptz.vert_as5047d.as5047d_angle_actual;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
static OS_STK task_get_angle_stk[TASK_GET_ANGLE_STK_SIZE];
|
|||
|
|
static void creat_task_get_angle(void)
|
|||
|
|
{
|
|||
|
|
CPU_INT08U task_err;
|
|||
|
|
CPU_INT08U name_err;
|
|||
|
|
|
|||
|
|
task_err = OSTaskCreateExt((void (*)(void *)) ptz_get_angle_task,
|
|||
|
|
(void *) 0,
|
|||
|
|
(OS_STK *)&task_get_angle_stk[TASK_GET_ANGLE_STK_SIZE - 1],
|
|||
|
|
(INT8U ) TASK_GET_ANGLE_PRIO,
|
|||
|
|
(INT16U ) TASK_GET_ANGLE_PRIO,
|
|||
|
|
(OS_STK *)&task_get_angle_stk[0],
|
|||
|
|
(INT32U ) TASK_GET_ANGLE_STK_SIZE,
|
|||
|
|
(void *) 0,
|
|||
|
|
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
|
|||
|
|
#if (OS_TASK_NAME_EN > 0)
|
|||
|
|
OSTaskNameSet(TASK_GET_ANGLE_PRIO, "ptz_get_angle_task", &name_err);
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void init_angle_module()
|
|||
|
|
{
|
|||
|
|
if(strstr((const char*)system_info.error_compensation, "on"))
|
|||
|
|
{//ע<><D7A2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ش<EFBFBD><D8B4><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>Ҫ<EFBFBD>ر<EFBFBD>ˮƽ<CBAE>ظ<EFBFBD><D8B8><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
|
|||
|
|
g_ptz.hori_angle_erro_switch = 1;//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
g_ptz.hori_repeat_locate_switch = 0;//<2F>ظ<EFBFBD><D8B8><EFBFBD>λ<EFBFBD><EFBFBD><F2BFAAA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD>ر<EFBFBD>
|
|||
|
|
}else{
|
|||
|
|
g_ptz.hori_angle_erro_switch = 0;//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ر<EFBFBD>
|
|||
|
|
}
|
|||
|
|
ptz_zero_offset_angle_read();//<2F><>ȡ<EFBFBD>Ƕ<EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD>
|
|||
|
|
// g_ptz.offset_angle.offset_anle = 0;//ȡ<><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼֵ<CABC><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>籣<EFBFBD><E7B1A3>
|
|||
|
|
as5047d_init();
|
|||
|
|
g_ptz.hori_as5047d.time_end = 0;
|
|||
|
|
g_ptz.hori_as5047d.time_now = 0;
|
|||
|
|
g_ptz.hori_as5047d.time = 0;
|
|||
|
|
g_ptz.vert_as5047d.time_end = 0;
|
|||
|
|
g_ptz.vert_as5047d.time_now = 0;
|
|||
|
|
g_ptz.vert_as5047d.time = 0;
|
|||
|
|
ptz_get_angle_init();
|
|||
|
|
creat_task_get_angle();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|