///Copyright (c) 2022, 四川汇源光通信有限公司 ///All rights reserved. ///@file service_error_count.C ///@brief 计算结构误差，用于补偿 /// ///@details ///@note /// ///@version Hyt2.0 2022/11/24 #include "bsp_os.h" #include "ptz_global_variable.h" #include "angle_poweroffsave.h" #include "comm_cfginfo.h" #include "tmc2160.h" #include "service_error_count.h" char error_conut_state; #define COUNT_STATE 1 #define ERROR_COUNT_SPEED 1.5 /* **水平右转一圈，以右转下降沿为参考信号，计算编码器转过的角度hori_angle_count; **以第二个右转下降沿信号停止（不能延时，立即停止），而后左转一圈，以左转上升沿为信号 **计算编码器转过的角度vert_angle_count;，，再计算左转与右转的差值error=abs(hori_angle_count - vert_angle_count); **误差计算时，编码器状态为不可用，防止角度被光电开关更新 */ ///方案一：误差计算在自检之前，计算并存储，作为误差补偿的值，补偿开关常开； ///方案二：任意时刻启动误差计算，当误差超过固定值（比如0.2°）后，打开补偿开关，进行固定差值补偿 /*! \brief 误差计算 \param[in] none \param[out] none \retval none \note LH @2022.11.24 */ static void ptz_hori_error_count_task() { unsigned short int uint16_angle; static float right_hori_angle_count, left_hori_angle_count, error; // unsigned char angle_error[7] = {0xff,0x01,0xAA,0xBB,0x00,0x00,0x00}; while(1) { TMC2160Control data; switch(error_conut_state) { //首先让云台水平向右转动 case COUNT_STATE: g_ptz.hori_as5047d.as5047d_state = 0;//数据不可正常使用 OSTimeDlyHMSM(0u, 0u, 0u, 10u); data = ptz_hori_choice_microstep(ERROR_COUNT_SPEED); g_ptz.hori_tmc2160 = data; if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住 { ptz_hori_start(PTZ_HORI_DIR_LEFT, ERROR_COUNT_SPEED); error_conut_state = COUNT_STATE + 1; } else//没有被挡住 { g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; ptz_hori_start(PTZ_HORI_DIR_RIGHT, ERROR_COUNT_SPEED); error_conut_state = COUNT_STATE + 2; } break; case (COUNT_STATE + 1): if(g_ptz.hori_ps_sw3_state == PS_NO_COVER) { OSTimeDlyHMSM(0u, 0u, 2u, 0u);//延时转动 ptz_hori_stop(PTZ_HORI_STOP_TIME); g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; ptz_hori_start(PTZ_HORI_DIR_RIGHT, ERROR_COUNT_SPEED); error_conut_state = COUNT_STATE + 2; } break; //等待第一次水平光电开关右转下降沿中断 case (COUNT_STATE + 2): if(g_ptz.hori_ps_sw3_right_fall >= 1) { g_ptz.hori_as5047d.as5047d_data_reset = 1;//清除编码器数据 g_ptz.hori_ps_sw3_right_fall = 0; error_conut_state++; OSTimeDlyHMSM(0u, 0u, 3u, 0u); g_ptz.hori_ps_sw3_right_fall = 0; } break; //等待第二次水平光电开关右转下降沿中断 case (COUNT_STATE + 3): if(g_ptz.hori_ps_sw3_right_fall >= 1) { right_hori_angle_count = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;//赋值右转一圈编码器角度 ptz_hori_stop(PTZ_HORI_STOP_TIME); g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; error_conut_state++; OSTimeDlyHMSM(0u, 0u, 1u, 0u); } break; case (COUNT_STATE + 4)://左转 ptz_hori_start(PTZ_HORI_DIR_LEFT, ERROR_COUNT_SPEED); error_conut_state++; break; //等待第一次水平光电开关左转上升沿中断 case (COUNT_STATE + 5): if(g_ptz.hori_ps_sw3_left_rise >= 1) { g_ptz.hori_as5047d.as5047d_data_reset = 1;//清除编码器数据 g_ptz.hori_ps_sw3_left_rise = 0; error_conut_state++; OSTimeDlyHMSM(0u, 0u, 5u, 0u); g_ptz.hori_ps_sw3_left_rise = 0; } break; //等待第二次水平光电开关左转上升沿中断 case (COUNT_STATE + 6): if(g_ptz.hori_ps_sw3_left_rise >= 1) { left_hori_angle_count = g_ptz.hori_as5047d.as5047d_ptz_angle_actual;//赋值右转一圈编码器角度 ptz_hori_stop(PTZ_HORI_STOP_TIME); g_ptz.hori_ps_sw3_right_fall = 0; g_ptz.hori_ps_sw3_left_rise = 0; g_ptz.hori_ps_sw3_right_rise = 0; g_ptz.hori_ps_sw3_left_fall = 0; error_conut_state = 0; OSTimeDlyHMSM(0u, 0u, 1u, 0u); // g_ptz.hori_as5047d.as5047d_state = 1;//自检后再可用 if(left_hori_angle_count < 0) {//负数转正 left_hori_angle_count = (-1)*left_hori_angle_count; } if(left_hori_angle_count > right_hori_angle_count) {//保证差值为正数 error = left_hori_angle_count - right_hori_angle_count; }else{ error = right_hori_angle_count - left_hori_angle_count; } //控制精度为1% uint16_angle = (unsigned short int)(error * 100 + 0.5); g_ptz.hori_angle_error = error;//(float)(uint16_angle / 100.0); // uint16_angle = (unsigned short int)(error * 100 + 0.5); // angle_error[1] = g_ptz.address; // angle_error[4] = (unsigned char)(uint16_angle>>8 & 0x00ff); // angle_error[5] = (unsigned char)(uint16_angle & 0x00ff); // angle_error[6] = MotorCalPelcoDSUM(angle_error,sizeof(angle_error)); // ptz_send_data(PTZ_UART_485, angle_error, sizeof(angle_error)); } break; } OSTimeDlyHMSM(0u, 0u, 0u, 1u); if(error_conut_state == 0) { OSTaskDel(OS_PRIO_SELF);//关闭该任务 } } } //水平自检任务 static OS_STK task_hori_error_count_stk[TASK_PTZ_ERROR_COUNT_STK_SIZE]; static void creat_task_hori_error_count(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_error_count_task, (void *) 0, (OS_STK *)&task_hori_error_count_stk[TASK_PTZ_ERROR_COUNT_STK_SIZE - 1], (INT8U ) TASK_PTZ_ERROR_COUNT_PRIO, (INT16U ) TASK_PTZ_ERROR_COUNT_PRIO, (OS_STK *)&task_hori_error_count_stk[0], (INT32U ) TASK_PTZ_ERROR_COUNT_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_PTZ_ERROR_COUNT_PRIO, "ptz_hori_error_count_task", &name_err); #endif if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_error_count_task success...\n\r"); } else { pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_error_count_task failed...\n\r"); } } void init_error_count_task() { // error_conut_state = 0; creat_task_hori_error_count(); }