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