pengsx
/
MW22-02A
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
MW22-02A/APP/Service/service_error_count.c

224 lines
6.9 KiB
C
Raw Normal View History

汇源原始工程
2025-05-21 01:55:40 +00:00
///Copyright (c) 2022, <20>Ĵ<EFBFBD><C4B4><EFBFBD>Դ<EFBFBD><D4B4>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD>޹<EFBFBD>˾
///All rights reserved.
///@file service_error_count.C
///@brief <20><><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><E1B9B9><EFBFBD><EFBFBD><EEA3AC><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD>
///
///@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
/*
**ˮƽ<EFBFBD><EFBFBD>תһȦ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>½<EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>ο<EFBFBD><EFBFBD>źţ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD>ĽǶ<EFBFBD>hori_angle_count;
**<EFBFBD>Եڶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>½<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>ֹͣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תһȦ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>ź<EFBFBD>
**<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD>ĽǶ<EFBFBD>vert_angle_count;<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ټ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>IJ<EFBFBD>ֵerror=abs(hori_angle_count - vert_angle_count);
**<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD>Ƕȱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>翪<EFBFBD>ظ<EFBFBD><EFBFBD><EFBFBD>
*/
///<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD>֮ǰ<D6AE><C7B0><EFBFBD><EFBFBD><EFBFBD>㲢<EFBFBD>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EEB2B9><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>س<EFBFBD><D8B3><EFBFBD><EFBFBD><EFBFBD>
///<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><E3A3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EEB3AC><EFBFBD>̶<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0.2<EFBFBD>㣩<EFBFBD>󣬴򿪲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>й̶<EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
/*!
\brief <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
\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)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
case COUNT_STATE:
g_ptz.hori_as5047d.as5047d_state = 0;//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>
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)//<2F><><EFBFBD><EFBFBD>ˮƽ<CBAE><C6BD><EFBFBD>翪<EFBFBD>ر<EFBFBD><D8B1><EFBFBD>ס
{
ptz_hori_start(PTZ_HORI_DIR_LEFT, ERROR_COUNT_SPEED);
error_conut_state = COUNT_STATE + 1;
}
else//û<>б<EFBFBD><D0B1><EFBFBD>ס
{
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);//<2F><>ʱת<CAB1><D7AA>
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;
//<2F>ȴ<EFBFBD><C8B4><EFBFBD>һ<EFBFBD><D2BB>ˮƽ<CBAE><C6BD><EFBFBD>翪<EFBFBD><E7BFAA><EFBFBD><EFBFBD>ת<EFBFBD>½<EFBFBD><C2BD><EFBFBD><EFBFBD>ж<EFBFBD>
case (COUNT_STATE + 2):
if(g_ptz.hori_ps_sw3_right_fall >= 1)
{
g_ptz.hori_as5047d.as5047d_data_reset = 1;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
//<2F>ȴ<EFBFBD><C8B4>ڶ<EFBFBD><DAB6><EFBFBD>ˮƽ<CBAE><C6BD><EFBFBD>翪<EFBFBD><E7BFAA><EFBFBD><EFBFBD>ת<EFBFBD>½<EFBFBD><C2BD><EFBFBD><EFBFBD>ж<EFBFBD>
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;//<2F><>ֵ<EFBFBD><D6B5>תһȦ<D2BB><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
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)://<2F><>ת
ptz_hori_start(PTZ_HORI_DIR_LEFT, ERROR_COUNT_SPEED);
error_conut_state++;
break;
//<2F>ȴ<EFBFBD><C8B4><EFBFBD>һ<EFBFBD><D2BB>ˮƽ<CBAE><C6BD><EFBFBD>翪<EFBFBD><E7BFAA><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
case (COUNT_STATE + 5):
if(g_ptz.hori_ps_sw3_left_rise >= 1)
{
g_ptz.hori_as5047d.as5047d_data_reset = 1;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
//<2F>ȴ<EFBFBD><C8B4>ڶ<EFBFBD><DAB6><EFBFBD>ˮƽ<CBAE><C6BD><EFBFBD>翪<EFBFBD><E7BFAA><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
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;//<2F><>ֵ<EFBFBD><D6B5>תһȦ<D2BB><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
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;//<2F>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD>ٿ<EFBFBD><D9BF><EFBFBD>
if(left_hori_angle_count < 0)
{//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
left_hori_angle_count = (-1)*left_hori_angle_count;
}
if(left_hori_angle_count > right_hori_angle_count)
{//<2F><>֤<EFBFBD><D6A4>ֵΪ<D6B5><CEAA><EFBFBD><EFBFBD>
error = left_hori_angle_count - right_hori_angle_count;
}else{
error = right_hori_angle_count - left_hori_angle_count;
}
//<2F><><EFBFBD>ƾ<EFBFBD><C6BE><EFBFBD>Ϊ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);//<2F>رո<D8B1><D5B8><EFBFBD><EFBFBD><EFBFBD>
}
}
}
//ˮƽ<CBAE>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD>
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();
}