/**********************************
 * 文件名称: speed_pid.c
 * 功能描述: 速度PID控制器模块
 * 功能: 实现电机速度的PID控制算法
 **********************************/
#include "speed_pid.h"


/**
 * @brief 速度PID控制周期
 * @details 设置为0.001秒（1kHz）
 */
#define SPEED_PID_PERIOD 0.001F

/**
 * @brief 速度PID控制器参数
 */
real32_T SPEED_PI_I = 5.0F;           // 积分系数
real32_T SPEED_PI_KB = 0.015F;        // 反馈系数
real32_T SPEED_PI_LOW_LIMIT = -4.0F;  // 输出下限
real32_T SPEED_PI_P = 0.003F;         // 比例系数
real32_T SPEED_PI_UP_LIMIT = 4.0F;    // 输出上限

/**
 * @brief 速度控制相关变量
 */
real32_T Speed_Ref;       // 速度参考值（转/秒）
real32_T Speed_Fdk;        // 速度反馈值（弧度/秒）
real32_T Speed_Pid_Out;    // 速度PID输出，作为Q轴电流参考值

/**
 * @brief 速度PID控制器结构体
 */
SPEED_PID_DEF Speed_Pid;

/**
 * @brief 速度PID计算函数
 * @param ref_temp 速度参考值（转/秒）
 * @param fdb_temp 速度反馈值（弧度/秒）
 * @param out_temp PID输出值（作为Q轴电流参考值）
 * @param current_pid_temp PID控制器结构体指针
 * @retval 无
 */
void Speed_Pid_Calc(real32_T ref_temp, real32_T fdb_temp, real32_T* out_temp, SPEED_PID_DEF* current_pid_temp)
{
  real32_T error;  // 速度误差
  real32_T temp;   // 临时变量

  // 计算速度误差：将参考速度（转/秒）转换为弧度/秒，减去反馈速度
  error = 6.28318548F * ref_temp - fdb_temp;  // 2*pi 用于将转/秒转换为弧度/秒

  // 计算PID输出的临时值
  temp = (error + current_pid_temp->I_Sum) * current_pid_temp->P_Gain;

  // 输出限幅
  if (temp > current_pid_temp->Max_Output) {
    *out_temp = current_pid_temp->Max_Output;
  } else if (temp < current_pid_temp->Min_Output) {
    *out_temp = current_pid_temp->Min_Output;
  } else {
    *out_temp = temp;
  }

  // 更新积分项
  current_pid_temp->I_Sum += ((*out_temp - temp) * current_pid_temp->B_Gain + current_pid_temp->I_Gain * error) * SPEED_PID_PERIOD;
}



/**
 * @brief 速度PID控制器初始化函数
 * @retval 无
 */
void speed_pid_initialize(void)
{
  // 初始化PID控制器参数
  Speed_Pid.P_Gain = SPEED_PI_P;           // 设置比例系数
  Speed_Pid.I_Gain = SPEED_PI_I;           // 设置积分系数
  Speed_Pid.B_Gain = SPEED_PI_KB;          // 设置反馈系数
  Speed_Pid.Max_Output = SPEED_PI_UP_LIMIT;  // 设置输出上限
  Speed_Pid.Min_Output = SPEED_PI_LOW_LIMIT; // 设置输出下限
  Speed_Pid.I_Sum = 0.0f;                  // 重置积分和
}