MW22-02A/APP/Device/Device_angle/get_angle.c

3834 lines
130 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#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();
}