#include "bsp_pid.h"
#include "bldc_ctrl.h"

//¶¨ÒåÈ«¾Ö±äÁ¿
pid_t pid;

/**
  * @brief  PID²ÎÊý³õʼ»¯
	*	@note 	ÎÞ
  * @retval ÎÞ
  */
void PID_param_init()
{
  /* ³õʼ»¯²ÎÊý */
  pid.target_val = 0.0;
  pid.actual_val = 0.0;
  pid.err = 0.0;
  pid.err_last = 0.0;
  pid.integral = 0.0;

  pid.Kp = 0.07; //24
  pid.Ki = 0.03;
  pid.Kd = 0.0;

#if defined(PID_ASSISTANT_EN)
  float pid_temp[3] = {pid.Kp, pid.Ki, pid.Kd};
  set_computer_value(SEND_P_I_D_CMD, CURVES_CH1, pid_temp, 3); // ¸øͨµÀ 1 ·¢ËÍ P I D Öµ
#endif
}

/**
  * @brief  ÉèÖÃÄ¿±êÖµ
  * @param  val		Ä¿±êÖµ
	*	@note 	ÎÞ
  * @retval ÎÞ
  */
void set_pid_target(float temp_val)
{
  pid.target_val = temp_val; // ÉèÖõ±Ç°µÄÄ¿±êÖµ
}

/**
  * @brief  »ñÈ¡Ä¿±êÖµ
  * @param  ÎÞ
	*	@note 	ÎÞ
  * @retval Ä¿±êÖµ
  */
float get_pid_target(void)
{
  return pid.target_val; // ÉèÖõ±Ç°µÄÄ¿±êÖµ
}

/**
  * @brief  ÉèÖñÈÀý¡¢»ý·Ö¡¢Î¢·ÖϵÊý
  * @param  p£º±ÈÀýϵÊý P
  * @param  i£º»ý·ÖϵÊý i
  * @param  d£ºÎ¢·ÖϵÊý d
	*	@note 	ÎÞ
  * @retval ÎÞ
  */
void set_p_i_d(float p, float i, float d)
{
  pid.Kp = p; // ÉèÖñÈÀýϵÊý P
  pid.Ki = i; // ÉèÖûý·ÖϵÊý I
  pid.Kd = d; // ÉèÖÃ΢·ÖϵÊý D
}

/**
  * @brief  PIDË㷨ʵÏÖ
  * @param  actual_val:ʵ¼ÊÖµ
	*	@note 	ÎÞ
  * @retval ͨ¹ýPID¼ÆËãºóµÄÊä³ö
  */
float PID_realize(float actual_val)
{
  /*¼ÆËãÄ¿±êÖµÓëʵ¼ÊÖµµÄÎó²î*/
  pid.err = pid.target_val - actual_val;
  pid.integral += pid.err;

  /*PIDË㷨ʵÏÖ*/
  pid.actual_val = pid.Kp * pid.err +
                   pid.Ki * pid.integral +
                   pid.Kd * (pid.err - pid.err_last);

  /*Îó²î´«µÝ*/
  pid.err_last = pid.err;

  /*·µ»Øµ±Ç°Êµ¼ÊÖµ*/
  return pid.actual_val;
}