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"
|
||
|
||
///云台角度回复
|
||
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));
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
//角度状态查询，仅代理层调用
|
||
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));
|
||
|
||
}
|
||
|
||
//时间计算
|
||
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
|
||
/// 计算云台水平角度
|
||
#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 || //防止读出的角度为负值
|
||
hori_angle[i] > 360.0||//防止读出的数值超过360
|
||
isnan(hori_angle[i]) == 1)//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃
|
||
i --;
|
||
}
|
||
}
|
||
|
||
//由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
|
||
//例如0.2、0.1、359.9、359.8这种数据就需要处理
|
||
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;//说明磁编码器处于0位附近，需要进行数据优化处理
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
//对采集到的角度值排序
|
||
for(j = 0; j < PTZ_HORI_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
|
||
{
|
||
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];//去掉一个最大值和一个最小值
|
||
}
|
||
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 || //防止读出的角度为负值
|
||
tem > 360.0||//防止读出的数值超过360
|
||
isnan(tem) == 1)//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃,从新读取
|
||
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;
|
||
}
|
||
//反转，云台角度减小，磁编码角度增大
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 --;//转动圈数减少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 --;//转动圈数减少1
|
||
}
|
||
}
|
||
|
||
|
||
//正转，云台角度增大，磁编码角度减小
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 ++;//转动圈数增加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 ++;//转动圈数增加1
|
||
}
|
||
|
||
}
|
||
|
||
//计算当前角度
|
||
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;
|
||
}
|
||
|
||
//对磁编码器角度进行滤波
|
||
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;
|
||
// }
|
||
|
||
}
|
||
|
||
//防止角度范围超过限度
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
//对云台角度进行四舍五入,保留小数点后3位小数
|
||
tem3 = (unsigned int)((g_ptz.hori_as5047d.as5047d_ptz_angle_actual *
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_K) * 1000.0 + 0.5);
|
||
//计算云台实际角度
|
||
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;
|
||
}
|
||
|
||
//保存当前磁编码器的值以及云台角度值
|
||
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 || //防止读出的角度为负值
|
||
hori_angle[i] > 360.0||//防止读出的数值超过360
|
||
isnan(hori_angle[i]) == 1)//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃
|
||
i --;
|
||
}
|
||
}
|
||
|
||
//由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
|
||
//例如0.2、0.1、359.9、359.8这种数据就需要处理
|
||
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;//说明磁编码器处于0位附近，需要进行数据优化处理
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
//对采集到的角度值排序
|
||
for(j = 0; j < PTZ_HORI_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
|
||
{
|
||
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];//去掉一个最大值和一个最小值
|
||
}
|
||
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 || //防止读出的角度为负值
|
||
// tem > 360.0||//防止读出的数值超过360
|
||
// isnan(tem) == 1)//防止读出的不是一个数
|
||
// {//以上错误的数据都该舍弃,从新读取
|
||
// 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 || //防止读出的角度为负值
|
||
tem > 360.0 ||//防止读出的数值超过360
|
||
isnan(tem) == 1 ||//防止读出的不是一个数
|
||
((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)))
|
||
{//以上错误的数据都该舍弃,从新读取
|
||
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);
|
||
|
||
//首先判断磁编码器角度判断磁编码器转向
|
||
//磁编码器正转角度增大
|
||
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;
|
||
}
|
||
//磁编码器反转角度减小
|
||
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;
|
||
}
|
||
}
|
||
|
||
//反转，云台角度减小，磁编码角度增大
|
||
if(g_ptz.hori_as5047d.as5047d_dir == AS5047D_DIR_FWD)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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)
|
||
{//误差消除开关关闭，使能左转上升沿角度更新，，打开则失能
|
||
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;
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 --;//转动圈数减少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 --;//转动圈数减少1
|
||
}
|
||
}
|
||
|
||
|
||
//正转，云台角度增大，磁编码角度减小
|
||
if(g_ptz.hori_as5047d.as5047d_dir == AS5047D_DIR_REV)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 ++;//转动圈数增加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 ++;//转动圈数增加1
|
||
}
|
||
|
||
}
|
||
|
||
//保存当前磁编码器圈数
|
||
cycle_count_save = g_ptz.hori_as5047d.as5047d_cycle_count;
|
||
|
||
for(i = 0; i < 4; i++)
|
||
{//这个循环的目的是为了防止磁编码器圈数计算错误，恢复正常的磁编码圈数
|
||
//计算当前角度
|
||
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;
|
||
}
|
||
|
||
//对磁编码器总角度进行滤波
|
||
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
|
||
{
|
||
//根据判断，恢复实际的磁编码器圈数
|
||
switch(i)
|
||
{
|
||
case 0:
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save;
|
||
g_ptz.hori_as5047d.as5047d_cycle_count --;//转动圈数减少1
|
||
break;
|
||
|
||
case 1:
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save;
|
||
g_ptz.hori_as5047d.as5047d_cycle_count ++;//转动圈数增加1
|
||
break;
|
||
|
||
case 2: //磁编码器实际圈数恢复失败
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
//防止角度范围超过限度
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
//对云台角度进行四舍五入,保留小数点后3位小数
|
||
tem3 = (unsigned int)((g_ptz.hori_as5047d.as5047d_ptz_angle_actual *
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_K) * 1000.0 + 0.5);
|
||
//计算云台实际角度
|
||
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;
|
||
}
|
||
|
||
//保存当前磁编码器的值以及云台角度值
|
||
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
|
||
|
||
|
||
/// 计算云台垂直角度
|
||
#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 || //防止读出的角度为负值
|
||
vert_angle[i] > 360.0||//防止读出的数值超过360
|
||
isnan(vert_angle[i]) == 1)//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃
|
||
i --;
|
||
}
|
||
}
|
||
|
||
//由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
|
||
//例如0.2、0.1、359.9、359.8这种数据就需要处理
|
||
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;//说明磁编码器处于0位附近，需要进行数据优化处理
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
//对采集到的角度值排序
|
||
for(j = 0; j < PTZ_VERT_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
|
||
{
|
||
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];//去掉一个最大值和一个最小值
|
||
}
|
||
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 || //防止读出的角度为负值
|
||
tem > 360.0||//防止读出的数值超过360
|
||
isnan(tem) == 1)//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃,从新读取
|
||
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;
|
||
}
|
||
|
||
//反转，云台角度减小，磁编码角度增大
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 --;//转动圈数减少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 --;//转动圈数减少1
|
||
}
|
||
|
||
}
|
||
|
||
|
||
//正转，云台角度增大，磁编码角度减小
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 ++;//转动圈数增加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 ++;//转动圈数增加1
|
||
}
|
||
}
|
||
|
||
|
||
//计算当前角度
|
||
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;
|
||
}
|
||
|
||
//对算出的磁编码器总角度进行滤波处理
|
||
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)
|
||
{
|
||
//对云台角度进行四舍五入,保留小数点后3位小数
|
||
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;
|
||
|
||
}
|
||
|
||
//保存当前磁编码器的值以及云台角度值
|
||
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 || //防止读出的角度为负值
|
||
vert_angle[i] > 360.0||//防止读出的数值超过360
|
||
isnan(vert_angle[i]) == 1)//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃
|
||
i --;
|
||
}
|
||
}
|
||
|
||
//由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
|
||
//例如0.2、0.1、359.9、359.8这种数据就需要处理
|
||
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;//说明磁编码器处于0位附近，需要进行数据优化处理
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
//对采集到的角度值排序
|
||
for(j = 0; j < PTZ_VERT_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
|
||
{
|
||
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];//去掉一个最大值和一个最小值
|
||
}
|
||
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 || //防止读出的角度为负值
|
||
// tem > 360.0||//防止读出的数值超过360
|
||
// isnan(tem) == 1)//防止读出的不是一个数
|
||
// {//以上错误的数据都该舍弃,从新读取
|
||
// 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 || //防止读出的角度为负值
|
||
tem > 360.0 ||//防止读出的数值超过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)))//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃,从新读取
|
||
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);
|
||
|
||
//首先判断磁编码器角度判断磁编码器转向
|
||
//磁编码器正转角度增大
|
||
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;
|
||
}
|
||
//磁编码器反转角度减小
|
||
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;
|
||
}
|
||
}
|
||
|
||
|
||
//反转，云台角度减小，磁编码角度增大
|
||
if(g_ptz.vert_as5047d.as5047d_dir == AS5047D_DIR_FWD)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 --;//转动圈数减少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 --;//转动圈数减少1
|
||
}
|
||
|
||
}
|
||
|
||
|
||
//正转，云台角度增大，磁编码角度减小
|
||
if(g_ptz.vert_as5047d.as5047d_dir == AS5047D_DIR_REV)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 ++;//转动圈数增加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 ++;//转动圈数增加1
|
||
}
|
||
}
|
||
|
||
//保存当前磁编码器圈数
|
||
cycle_count_save = g_ptz.vert_as5047d.as5047d_cycle_count;
|
||
|
||
for(i = 0; i < 4; i++)
|
||
{//这个循环的目的是为了防止磁编码器圈数计算错误，恢复正常的磁编码圈数
|
||
//计算当前角度
|
||
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;
|
||
}
|
||
|
||
//对算出的磁编码器总角度进行滤波处理
|
||
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
|
||
{
|
||
//根据判断，恢复实际的磁编码器圈数
|
||
switch(i)
|
||
{
|
||
case 0:
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
|
||
g_ptz.vert_as5047d.as5047d_cycle_count --;//转动圈数减少1
|
||
break;
|
||
|
||
case 1:
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
|
||
g_ptz.vert_as5047d.as5047d_cycle_count ++;//转动圈数增加1
|
||
break;
|
||
|
||
case 2: //磁编码器实际圈数恢复失败
|
||
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)
|
||
{
|
||
//对云台角度进行四舍五入,保留小数点后3位小数
|
||
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;
|
||
|
||
|
||
}
|
||
|
||
//保存当前磁编码器的值以及云台角度值
|
||
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
|
||
|
||
|
||
/// 计算云台水平角度
|
||
#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 || //防止读出的角度为负值
|
||
hori_angle[i] > 360.0||//防止读出的数值超过360
|
||
isnan(hori_angle[i]) == 1)//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃
|
||
i --;
|
||
}
|
||
}
|
||
|
||
//由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
|
||
//例如0.2、0.1、359.9、359.8这种数据就需要处理
|
||
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;//说明磁编码器处于0位附近，需要进行数据优化处理
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
//对采集到的角度值排序
|
||
for(j = 0; j < PTZ_HORI_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
|
||
{
|
||
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];//去掉一个最大值和一个最小值
|
||
}
|
||
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 || //防止读出的角度为负值
|
||
tem > 360.0||//防止读出的数值超过360
|
||
isnan(tem) == 1)//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃,从新读取
|
||
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;
|
||
}
|
||
//反转，云台角度减小，磁编码角度减小
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 --;//转动圈数减少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 --;//转动圈数减少1
|
||
}
|
||
}
|
||
|
||
|
||
//正转，云台角度增大，磁编码角度增大
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 ++;//转动圈数增加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 ++;//转动圈数增加1
|
||
}
|
||
}
|
||
|
||
|
||
//计算当前角度
|
||
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;
|
||
}
|
||
|
||
//对磁编码器角度进行滤波
|
||
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;
|
||
}
|
||
|
||
//防止角度范围超过限度
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
//对云台角度进行四舍五入,保留小数点后3位小数
|
||
tem3 = (unsigned int)((g_ptz.hori_as5047d.as5047d_ptz_angle_actual *
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_K) * 1000.0 + 0.5);
|
||
//计算云台角度
|
||
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;
|
||
}
|
||
|
||
//保存当前磁编码器的值以及云台角度值
|
||
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 || //防止读出的角度为负值
|
||
hori_angle[i] > 360.0||//防止读出的数值超过360
|
||
isnan(hori_angle[i]) == 1)//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃
|
||
i --;
|
||
}
|
||
}
|
||
|
||
//由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
|
||
//例如0.2、0.1、359.9、359.8这种数据就需要处理
|
||
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;//说明磁编码器处于0位附近，需要进行数据优化处理
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
//对采集到的角度值排序
|
||
for(j = 0; j < PTZ_HORI_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
|
||
{
|
||
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];//去掉一个最大值和一个最小值
|
||
}
|
||
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 || //防止读出的角度为负值
|
||
tem > 360.0 ||//防止读出的数值超过360
|
||
isnan(tem) == 1 ||//防止读出的不是一个数
|
||
((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)))
|
||
{//以上错误的数据都该舍弃,从新读取
|
||
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 || //防止读出的角度为负值
|
||
// tem > 360.0||//防止读出的数值超过360
|
||
// isnan(tem) == 1)//防止读出的不是一个数
|
||
// {//以上错误的数据都该舍弃,从新读取
|
||
// 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);
|
||
|
||
//首先判断磁编码器角度判断磁编码器转向
|
||
//磁编码器正转角度增大
|
||
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;
|
||
}
|
||
//磁编码器反转角度减小
|
||
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;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
//反转，云台角度减小，磁编码角度减小
|
||
if(g_ptz.hori_as5047d.as5047d_dir == AS5047D_DIR_REV)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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)
|
||
{//误差消除开关关闭，使能左转上升沿角度更新，，打开则失能
|
||
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;
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 --;//转动圈数减少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 --;//转动圈数减少1
|
||
}
|
||
}
|
||
|
||
//正转，云台角度增大，磁编码角度增大
|
||
if(g_ptz.hori_as5047d.as5047d_dir == AS5047D_DIR_FWD)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 ++;//转动圈数增加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 ++;//转动圈数增加1
|
||
}
|
||
}
|
||
|
||
//保存当前磁编码器圈数
|
||
cycle_count_save = g_ptz.hori_as5047d.as5047d_cycle_count;
|
||
|
||
for(i = 0; i < 4; i++)
|
||
{//这个循环的目的是为了防止磁编码器圈数计算错误，恢复正常的磁编码圈数
|
||
//计算当前角度
|
||
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;
|
||
}
|
||
|
||
//对磁编码器角度进行滤波
|
||
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
|
||
{
|
||
//根据判断，恢复实际的磁编码器圈数
|
||
switch(i)
|
||
{
|
||
case 0:
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save - 1;//转动圈数减少1
|
||
g_ptz.hori_jia ++;
|
||
break;
|
||
|
||
case 1:
|
||
g_ptz.hori_as5047d.as5047d_cycle_count = cycle_count_save + 1;//转动圈数增加1
|
||
g_ptz.hori_jian ++;
|
||
break;
|
||
|
||
case 2: //磁编码器实际圈数恢复失败
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
//防止角度范围超过限度
|
||
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//左
|
||
{
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
//对云台角度进行四舍五入,保留小数点后3位小数
|
||
tem3 = (unsigned int)((g_ptz.hori_as5047d.as5047d_ptz_angle_actual *
|
||
g_ptz.hori_as5047d.as5047d_ptz_angle_K) * 1000.0 + 0.5);
|
||
//计算云台角度
|
||
tem4 = tem3 / 1000.0;
|
||
|
||
|
||
//误差消除开关打开
|
||
if(g_ptz.hori_angle_erro_switch == 1)
|
||
{
|
||
tem4 = tem4 + (g_ptz.offset_angle.offset_anle / 50.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;
|
||
// }
|
||
}
|
||
if(tem4 == 360.0)///////解决既显示360又显示角度0的问题
|
||
{
|
||
tem4 = 0;
|
||
}
|
||
if(tem4 < 0)
|
||
{
|
||
tem4 = tem4 + 360.0;
|
||
}
|
||
if(tem4>360.0)
|
||
{
|
||
tem4 = tem4 - 360.0;
|
||
}
|
||
g_ptz.hori_angle_actual = tem4;
|
||
}
|
||
|
||
//保存当前磁编码器的值以及云台角度值
|
||
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;//用于步进电机速度计算
|
||
|
||
return 1;
|
||
}
|
||
|
||
#endif
|
||
|
||
|
||
/// 计算云台垂直角度
|
||
#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 || //防止读出的角度为负值
|
||
vert_angle[i] > 360.0||//防止读出的数值超过360
|
||
isnan(vert_angle[i]) == 1)//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃
|
||
i --;
|
||
}
|
||
}
|
||
|
||
//由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
|
||
//例如0.2、0.1、359.9、359.8这种数据就需要处理
|
||
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;//说明磁编码器处于0位附近，需要进行数据优化处理
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
//对采集到的角度值排序
|
||
for(j = 0; j < PTZ_VERT_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
|
||
{
|
||
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];//去掉一个最大值和一个最小值
|
||
}
|
||
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 || //防止读出的角度为负值
|
||
tem > 360.0||//防止读出的数值超过360
|
||
isnan(tem) == 1)//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃,从新读取
|
||
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;
|
||
}
|
||
|
||
//反转，云台角度减小，磁编码角度减小
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 --;//转动圈数减少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 --;//转动圈数减少1
|
||
}
|
||
|
||
}
|
||
|
||
|
||
//正转，云台角度增大，磁编码角度增大
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 ++;//转动圈数增加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 ++;//转动圈数增加1
|
||
}
|
||
}
|
||
|
||
|
||
//计算当前角度
|
||
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;
|
||
}
|
||
/**/
|
||
//对算出的磁编码器总角度进行滤波处理
|
||
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)
|
||
{
|
||
//对云台角度进行四舍五入,保留小数点后3位小数
|
||
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;
|
||
|
||
}
|
||
|
||
//保存当前磁编码器的值以及云台角度值
|
||
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 || //防止读出的角度为负值
|
||
vert_angle[i] > 360.0||//防止读出的数值超过360
|
||
isnan(vert_angle[i]) == 1)//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃
|
||
i --;
|
||
}
|
||
}
|
||
|
||
//由于磁编码器存在0位，防止采集到的数据差距接近360度，对采集到的数据进行优化处理
|
||
//例如0.2、0.1、359.9、359.8这种数据就需要处理
|
||
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;//说明磁编码器处于0位附近，需要进行数据优化处理
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
//对采集到的角度值排序
|
||
for(j = 0; j < PTZ_VERT_ANGLE_READ_NUM-1; j++)//采样值由小到大排列
|
||
{
|
||
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];//去掉一个最大值和一个最小值
|
||
}
|
||
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 || //防止读出的角度为负值
|
||
// tem > 360.0||//防止读出的数值超过360
|
||
// isnan(tem) == 1)//防止读出的不是一个数
|
||
// {//以上错误的数据都该舍弃,从新读取
|
||
// 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 || //防止读出的角度为负值
|
||
tem > 360.0 ||//防止读出的数值超过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)))//防止读出的不是一个数
|
||
{//以上错误的数据都该舍弃,从新读取
|
||
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);
|
||
|
||
//首先判断磁编码器角度判断磁编码器转向
|
||
//磁编码器正转角度增大
|
||
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;
|
||
}
|
||
//磁编码器反转角度减小
|
||
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;
|
||
}
|
||
}
|
||
|
||
|
||
//反转，云台角度减小，磁编码角度减小
|
||
if(g_ptz.vert_as5047d.as5047d_dir == AS5047D_DIR_REV)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 --;//转动圈数减少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 --;//转动圈数减少1
|
||
}
|
||
|
||
}
|
||
|
||
|
||
//正转，云台角度增大，磁编码角度增大
|
||
if(g_ptz.vert_as5047d.as5047d_dir == AS5047D_DIR_FWD)
|
||
{
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
//挡片更新角度
|
||
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;
|
||
//根据计算方法b，计算云台当前角度
|
||
|
||
//判断磁编码器是否转完一圈
|
||
|
||
//首先计算当前角度度和初始角度之间的角度距离
|
||
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 ++;//转动圈数增加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 ++;//转动圈数增加1
|
||
}
|
||
}
|
||
|
||
//保存当前磁编码器圈数
|
||
cycle_count_save = g_ptz.vert_as5047d.as5047d_cycle_count;
|
||
|
||
for(i = 0; i < 4; i++)
|
||
{//这个循环的目的是为了防止磁编码器圈数计算错误，恢复正常的磁编码圈数
|
||
//计算当前角度
|
||
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;
|
||
}
|
||
/**/
|
||
//对算出的磁编码器总角度进行滤波处理
|
||
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
|
||
{
|
||
//根据判断，恢复实际的磁编码器圈数
|
||
switch(i)
|
||
{
|
||
case 0:
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
|
||
g_ptz.vert_as5047d.as5047d_cycle_count --;//转动圈数减少1
|
||
break;
|
||
|
||
case 1:
|
||
g_ptz.vert_as5047d.as5047d_cycle_count = cycle_count_save;
|
||
g_ptz.vert_as5047d.as5047d_cycle_count ++;//转动圈数增加1
|
||
break;
|
||
|
||
case 2: //磁编码器实际圈数恢复失败
|
||
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)
|
||
{
|
||
//对云台角度进行四舍五入,保留小数点后3位小数
|
||
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;
|
||
|
||
|
||
}
|
||
|
||
//保存当前磁编码器的值以及云台角度值
|
||
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;//用于步进电机速度计算
|
||
|
||
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"))
|
||
{//注：误差消除开关打开时，需要关闭水平重复定位功能
|
||
g_ptz.hori_angle_erro_switch = 1;//水平误差消除打开
|
||
g_ptz.hori_repeat_locate_switch = 0;//重复定位打开，重型云台关闭
|
||
}else{
|
||
g_ptz.hori_angle_erro_switch = 0;//水平误差消除关闭
|
||
}
|
||
ptz_zero_offset_angle_read();//读取角度偏移量
|
||
// g_ptz.offset_angle.offset_anle = 0;//取消赋初始值，存入掉电保护
|
||
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();
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|