小弟刚学使用PID算法，从网上借鉴到一个比较容易理解的PID算法，可是我不清楚它是一个双环PID还是一个单环PID，所以请大家帮忙看看，源码如下：


void ALG_PID_Att_Loop(T_float_angle *IMU_Angle)
{
int32_t PIDTerm[3];
uint8_t i;

/* 计算姿态差*/
errorAngle[ROLL] = constrain_int32(Rx_Command[ROLL], -(int)FLYANGLE_MAX, +FLYANGLE_MAX - IMU_Angle->Roll);
errorAngle[PITCH] = constrain_int32(Rx_Command[PITCH], -(int)FLYANGLE_MAX, +FLYANGLE_MAX - IMU_Angle->Pitch);
/* YAW误差计算有待改进 */
errorAngle[YAW] = Rx_Command[YAW]*5 - MEMS_DataStructure.MEMS_Gyro_Z_Data;

for(i = 0; i < 3; i++)
{
  //当油门低于检查值时积分清零
  if(Rx_Command[THROTTLE]  < RC_MINCHECK)
  {
   PID_reset_I(&pid_par[i]);
  }
  
  //得到PID输出
  PIDTerm[i] = PID_get_pid(errorAngle[i], PID_LOOP_tiME, &pid_par[i]);
}

PIDTerm[YAW] = -constrain_int32(PIDTerm[YAW], -300 - abs(Rx_Command[YAW]), +300 + abs(Rx_Command[YAW]));  
}
int32_t PID_get_p(int32_t error, PID_Par *pid_par)
{
    return error * pid_par->kP / 64;
}
int32_t PID_get_i(int32_t error, uint16_t dt, PID_Par *pid_par)
{
    if((pid_par->kI != 0) && (dt != 0)) {
        pid_par->integrator += (error * dt / 2048 ) * pid_par->kI;
    //积分限幅
    pid_par->integrator = constrain_int32(pid_par->integrator,
               -(pid_par->iMax), +(pid_par->iMax));  
   
        return pid_par->integrator / 8192;
    }
    return 0;
}
void PID_reset_I(PID_Par *pid_par)
{
pid_par->integrator = 0;
}
int32_t PID_get_d(int32_t error, uint16_t dt, PID_Par *pid_par)
{
    if ((pid_par->kD != 0) && (dt != 0)) {   
   int32_t derivative;
   derivative = error - pid_par->last_error;
   pid_par->last_error = error;
   derivative = (derivative * ((uint16_t)0xFFFF / (dt / 16 ))) / 64;
   return (derivative * pid_par->kD) / 4;
    }
    return 0;
}
int32_t PID_get_pi(int32_t error, uint16_t dt, PID_Par *pid_par)
{
    return PID_get_p(error, pid_par) + PID_get_i(error, dt, pid_par);
}
int32_t PID_get_pid(int32_t error, uint16_t dt, PID_Par *pid_par)
{
    return PID_get_p(error, pid_par) + PID_get_i(error, dt, pid_par) +
         PID_get_d(error, dt, pid_par);
}