///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"

#include "rotate_servo.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();
}








