224 lines
6.9 KiB
C
224 lines
6.9 KiB
C
///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();
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|