//云台电子稳定，由于直齿轮云台自锁能力太差，增加电子自锁功能。
//当云台处于滑动状态时，给予反向力矩，防止云台转动
#include "ptz_header_file.h"
#include "rotate_electricstable.h"
#include "ptz_struct.h"
#include "full_bridge.h"



//char vert_electric_stable_switch;

#ifdef PTZ_BLDC_MOTOR
#ifdef PTZ_ELECTRIC_STABLE_L6235D
//数据初始化
void ptz_hori_electric_stable_init()
{
  g_ptz.hori_electric_stable.stable_stop_switch = 1;
  g_ptz.hori_electric_stable.torque_switch = 0;
  g_ptz.hori_electric_stable.stop_hall_count = 0;
  g_ptz.hori_electric_stable.slide_dir_change_num = 0;
  g_ptz.hori_electric_stable.stable_vref = 0;
  g_ptz.hori_electric_stable.stop_hall_count = 0;
  g_ptz.hori_electric_stable.state = 0;
}


static char ptz_hori_electric_stable_task()
{
  if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP &&//判断是否处于刹车状态
     g_ptz.hori_electric_stable.stable_stop_switch == 1 &&//刹车停止时间是否结束
       g_ptz.hori_electric_stable.stop_hall_count > HORI_STOP_HALL_NUM)//刹车后霍尔脉冲满足多少个时
  {
    
    
    if(g_ptz.hori_electric_stable.torque_switch == 1)//判断刹车PWM的占空比
    {
      g_ptz.hori_electric_stable.stable_vref = g_ptz.hori_electric_stable.stable_vref + 5;//刹车周期为100%时还刹不住，则增加力矩
      if(g_ptz.hori_electric_stable.stable_vref >= 200)
      {
        g_ptz.hori_electric_stable.stable_vref =200;
      }
    }
    
    if(g_ptz.hori_electric_stable.torque_switch == 0)//初始化反向力矩
    {
      
      //输入指定模拟电压
      g_ptz.hori_electric_stable.stable_vref = TORQUE_VREF;
      
      
      //计算刹车PWM波的初始刹车时间和解除刹车初始时间
      g_ptz.hori_electric_stable.torque_start_t = TORQUE_TOTAL_T / 2;
      g_ptz.hori_electric_stable.torque_stop_t  = 
        TORQUE_TOTAL_T - g_ptz.hori_electric_stable.torque_start_t;
      g_ptz.hori_electric_stable.torque_switch = 1;
    }
    g_ptz.hori_electric_stable.stop_hall_count --;
  }
  
  if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP &&//判断是否处于刹车状态
     g_ptz.hori_electric_stable.stable_stop_switch == 1 &&//刹车停止时间是否结束
       g_ptz.hori_electric_stable.stop_hall_count == HORI_STOP_HALL_NUM)//刹车后霍尔脉冲满足多少个时
  {
    g_ptz.hori_electric_stable.stable_vref = g_ptz.hori_electric_stable.stable_vref - 5;
  }
  
  return 1;
}

void ptz_vert_electric_stable_init()
{
  g_ptz.vert_electric_stable.stable_stop_switch = 1;
  g_ptz.vert_electric_stable.torque_switch = 0;
  g_ptz.vert_electric_stable.stop_hall_count = 0;
//  g_ptz.vert_electric_stable.sample_count = 0;
//  g_ptz.vert_electric_stable.stable_sample_switch = 0;
  g_ptz.vert_electric_stable.slide_dir_change_num = 0;
  g_ptz.vert_electric_stable.stable_vref = 0;
  g_ptz.vert_electric_stable.stop_hall_count = 0;
  g_ptz.vert_electric_stable.state = 0;
//  memset(g_ptz.vert_electric_stable.stable_angle, 0, sizeof(g_ptz.vert_electric_stable.stable_angle));
}

static char ptz_vert_electric_stable_task()
{
  if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP &&//判断是否处于刹车状态
     g_ptz.vert_electric_stable.stable_stop_switch == 1 &&//刹车停止时间是否结束
     g_ptz.vert_electric_stable.stop_hall_count > VERT_STOP_HALL_NUM)//刹车后霍尔脉冲满足多少个时
  {
    
    //判断反向力矩方向
    g_ptz.vert_electric_stable.slide_direction_actual = g_ptz.vert_direction_actual;
    if(g_ptz.vert_electric_stable.slide_direction_actual == PTZ_VERT_DIR_DOWN)
    {//如果云台向下滑动
      g_ptz.vert_electric_stable.reverse_torque_direction = PTZ_VERT_DIR_UP;
    }
    //如果云台向上滑动
    if(g_ptz.vert_electric_stable.slide_direction_actual == PTZ_VERT_DIR_UP)
    {
      g_ptz.vert_electric_stable.reverse_torque_direction = PTZ_VERT_DIR_DOWN;
    }
    
    if(g_ptz.vert_electric_stable.torque_switch == 1)//判断刹车PWM的占空比
    {
      
      g_ptz.vert_electric_stable.stable_vref = g_ptz.vert_electric_stable.stable_vref + 10;//刹车周期为100%时还刹不住，则增加力矩
      if(g_ptz.vert_electric_stable.stable_vref >= 200)
      {
        g_ptz.vert_electric_stable.stable_vref =200;
      }
      
    }
    
    if(g_ptz.vert_electric_stable.torque_switch == 0)//初始化反向力矩
    {
      //保存初始力矩方向
      g_ptz.vert_electric_stable.reverse_torque_direction_a = 
      g_ptz.vert_electric_stable.reverse_torque_direction;
      
      
      //输入指定模拟电压
      g_ptz.vert_electric_stable.stable_vref = TORQUE_VREF;
//      v_pwm_duty_change(g_ptz.vert_electric_stable.stable_vref);
      
      //计算刹车PWM波的初始刹车时间和解除刹车初始时间
      g_ptz.vert_electric_stable.torque_start_t = TORQUE_TOTAL_T / 2;
      g_ptz.vert_electric_stable.torque_stop_t  = 
        TORQUE_TOTAL_T - g_ptz.vert_electric_stable.torque_start_t;
      g_ptz.vert_electric_stable.torque_switch = 1;
    }
    
    
    g_ptz.vert_electric_stable.reverse_torque_direction_last = 
    g_ptz.vert_electric_stable.reverse_torque_direction;//保存上一次反向力矩的方向
    g_ptz.vert_electric_stable.stop_hall_count = VERT_STOP_HALL_NUM;
  }
  
  return 1;
}

static void ptz_vert_electric_stable_pwm_task()
{
  while(1)
  {
    static char flag;
    if(g_ptz.vert_start_stop_set == PTZ_VERT_START && flag == 1)
    {//防止反向力矩PWM波干扰电机正常启动
      flag = 0;
      v_stop_motor();
    }
    
    if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP && 
    g_ptz.vert_electric_stable.torque_switch == 1)
    {//提供刹车用的PWM波
      vert_dir_speed_start(g_ptz.vert_electric_stable.stable_vref);
      OSTimeDlyHMSM(0u, 0u, 0u, STABLE_T);
      flag = 1;
    }
    else
    {
       OSTimeDlyHMSM(0u, 0u, 0u, STABLE_T);
    }
    
  }
}


//#define TASK_VERT_ELECTRIC_STABLE_PWM_PRIO             17u
//#define TASK_VERT_ELECTRIC_STABLE_PWM_SIZE             100u
static OS_STK  task_vert_electric_stable_pwm_stk[TASK_VERT_ELECTRIC_STABLE_PWM_SIZE];
static  void  creat_task_vert_electric_stable_pwm(void)
{ 
   CPU_INT08U  task_err;   
   CPU_INT08U  name_err;
   
   task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_electric_stable_pwm_task,           
		   (void           *) 0,
		   (OS_STK         *)&task_vert_electric_stable_pwm_stk[TASK_VERT_ELECTRIC_STABLE_PWM_SIZE - 1],
		   (INT8U           ) TASK_VERT_ELECTRIC_STABLE_PWM_PRIO,                   
		   (INT16U          ) TASK_VERT_ELECTRIC_STABLE_PWM_PRIO,
		   (OS_STK         *)&task_vert_electric_stable_pwm_stk[0],
		   (INT32U          ) TASK_VERT_ELECTRIC_STABLE_PWM_SIZE,
		   (void           *) 0,
		   (INT16U          )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));     
    #if (OS_TASK_NAME_EN > 0)
    OSTaskNameSet(TASK_VERT_ELECTRIC_STABLE_PWM_PRIO, "ptz_vert_electric_stable_pwm_task", &name_err);
    #endif
       
    if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
        pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_electric_stable_pwm_task  success...\n\r");
	} else { 
        pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_electric_stable_pwm_task  failed...\n\r");
	}
}



static void ptz_electric_stable_task()
{
  while(1)
  {
    ptz_vert_electric_stable_task();
//    ptz_hori_electric_stable_task();
//    g_ptz.hori_electric_stable.state = 0;
    OSTimeDlyHMSM(0u, 0u, 0u, STABLE_T);
//    g_ptz.hori_electric_stable.state = 1;
  }
}



static OS_STK  task_electric_stable_stk[TASK_ELECTRIC_STABLE_STK_SIZE];
static  void  creat_task_electric_stable(void)
{ 
   CPU_INT08U  task_err;   
   CPU_INT08U  name_err;
   
   task_err = OSTaskCreateExt((void (*)(void *)) ptz_electric_stable_task,           
		   (void           *) 0,
		   (OS_STK         *)&task_electric_stable_stk[TASK_ELECTRIC_STABLE_STK_SIZE - 1],
		   (INT8U           ) TASK_ELECTRIC_STABLE_PRIO,                   
		   (INT16U          ) TASK_ELECTRIC_STABLE_PRIO,
		   (OS_STK         *)&task_electric_stable_stk[0],
		   (INT32U          ) TASK_ELECTRIC_STABLE_STK_SIZE,
		   (void           *) 0,
		   (INT16U          )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));     
    #if (OS_TASK_NAME_EN > 0)
    OSTaskNameSet(TASK_ELECTRIC_STABLE_PRIO, "ptz_electric_stable_task", &name_err);
    #endif
       
    if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
        pdebug(DEBUG_LEVEL_INFO,"create ptz_electric_stable_task  success...\n\r");
	} else { 
        pdebug(DEBUG_LEVEL_FATAL,"create ptz_electric_stable_task  failed...\n\r");
	}
}

#endif

#endif

void task_electric_stable_init(void)
{
#ifdef PTZ_BLDC_MOTOR
  #ifdef PTZ_ELECTRIC_STABLE_ON
        creat_task_electric_stable();
        #ifdef PTZ_ELECTRIC_STABLE_L6235D
             creat_task_vert_electric_stable_pwm();
        #endif
  #endif
#endif
}