串級PID

static void FLY_PidCal(void)
  {
	    
	  	 	
	     static  float histor_gyro_x,histor_gyro_y,histor_gyro_z;  //歷史xyz
	      
		 static float  roll_err_old,pitch_err_old;    //歷史rollpitch

         static int16_t Motor1,Motor2,Motor3,Motor4;

		 if(ctrl.ctrlRate >= 2)

{	
			//****************外環角度PID**************************//

	/*----------------------------------------*/
                   //滾轉角計算     
   /*---------------------------------------*/
	
	     
	ctrl.roll.shell.err =  RC_Data.ROLL-Q_ANGLE.Rool;
	ctrl.roll.shell.increment += ctrl.roll.shell.err; //須要移動的距離-增量
	
	//limit for the max increment 對增量進行限制。
	if(ctrl.roll.shell.increment > ctrl.roll.shell.increment_max)
	{  	
	ctrl.roll.shell.increment = ctrl.roll.shell.increment_max;
	}
	else if(ctrl.roll.shell.increment < -ctrl.roll.shell.increment_max)	
	{	
	ctrl.roll.shell.increment = -ctrl.roll.shell.increment_max;
	}

    ctrl.roll.shell.pid_out  = (ctrl.roll.shell.kp * ctrl.roll.shell.err + \
                             ctrl.roll.shell.ki * ctrl.roll.shell.increment + \
                             ctrl.roll.shell.kd * (ctrl.roll.shell.err - roll_err_old));
                         //Kp*須要移動的距離.+Ki*增量+kd*（須要移動的量-之前舊的roll值)
    roll_err_old = ctrl.roll.shell.err;  //更新歷史值
   /*------------------------------------------------*/
                     //俯仰角計算    
   /*----------------------------------------------*/	   
	   			
   ctrl.pitch.shell.err =RC_Data.PITCH-Q_ANGLE.Pitch;
   ctrl.pitch.shell.increment += ctrl.pitch.shell.err;
			
	//limit for the max increment
	if(ctrl.pitch.shell.increment > ctrl.pitch.shell.increment_max) 
	{ 	
	ctrl.pitch.shell.increment = ctrl.pitch.shell.increment_max;
	}
	else if(ctrl.pitch.shell.increment < -ctrl.pitch.shell.increment_max)
	{		
	ctrl.pitch.shell.increment = -ctrl.pitch.shell.increment_max;
	}
	
   ctrl.pitch.shell.pid_out =( ctrl.pitch.shell.kp * ctrl.pitch.shell.err+\
                             ctrl.pitch.shell.ki * ctrl.pitch.shell.increment + \
                             ctrl.pitch.shell.kd * (ctrl.pitch.shell.err - pitch_err_old));
	
    pitch_err_old = ctrl.pitch.shell.err;	//更新歷史值
/*-------------------------------------------------------*/
			//偏航計算
/*-------------------------------------------------------*/			
			
    ctrl.yaw.shell.pid_out =(ctrl.yaw.shell.kp * RC_Data.YAW + \
                           ctrl.yaw.shell.kd * GYRO_F.Z);    //kp*當前yaw+kd*指望值。
    ctrl.ctrlRate = 0; 
}
		
		
	ctrl.ctrlRate ++;
/////////////////////////////////////外環是角度。外環pid，內環pd

	  //********************內環角速度******糾正誤差。***************************//
		ctrl.roll.core.kp_out = ctrl.roll.core.kp * (ctrl.roll.shell.pid_out - GYRO_F.X);
		ctrl.roll.core.kd_out = ctrl.roll.core.kd * (GYRO_F.X - histor_gyro_x);
		
		ctrl.pitch.core.kp_out = ctrl.pitch.core.kp * (ctrl.pitch.shell.pid_out-GYRO_F.Y);
		ctrl.pitch.core.kd_out = ctrl.pitch.core.kd * (GYRO_F.Y - histor_gyro_y);
		
		ctrl.yaw.core.kp_out = ctrl.yaw.core.kp * (ctrl.yaw.shell.pid_out - GYRO_F.Z);
		ctrl.yaw.core.kd_out = ctrl.yaw.core.kd * (GYRO_F.Z - histor_gyro_z);
		
		ctrl.roll.core.pid_out = ctrl.roll.core.kp_out + ctrl.roll.core.kd_out;
		ctrl.pitch.core.pid_out = ctrl.pitch.core.kp_out + ctrl.pitch.core.kd_out;

		ctrl.yaw.core.pid_out = ctrl.yaw.core.kp_out + ctrl.yaw.core.kd_out;
	
		histor_gyro_x = GYRO_F.X;   
		histor_gyro_y = GYRO_F.Y;
	    histor_gyro_z = GYRO_F.Z;
		
		
     //*******************四電機輸出配置*********************************//
			
	Motor1 = RC_Data.THROTTLE + ctrl.roll.core.pid_out - ctrl.pitch.core.pid_out - ctrl.yaw.core.pid_out;
	Motor2 = RC_Data.THROTTLE + ctrl.roll.core.pid_out + ctrl.pitch.core.pid_out + ctrl.yaw.core.pid_out;
	Motor3 = RC_Data.THROTTLE - ctrl.roll.core.pid_out + ctrl.pitch.core.pid_out - ctrl.yaw.core.pid_out;
	Motor4 = RC_Data.THROTTLE - ctrl.roll.core.pid_out - ctrl.pitch.core.pid_out + ctrl.yaw.core.pid_out;
		
	
	
	   //**************************油門控制命令&刷新電機*********************************//	
	if(FLY_ENABLE)
	{
	   	
		MOTO_PWMRFLASH(Motor1,Motor2,Motor3,Motor4);
		
	}
	else
	{	  
	    MOTO_PWMRFLASH(RC_Data.THROTTLE,RC_Data.THROTTLE,RC_Data.THROTTLE,RC_Data.THROTTLE);
	}
	
	
	}













struct _ctrl ctrl;

 void  FLY_ParamLoad(void)
{
	

	
	// The data of pitch 俯仰角
	ctrl.pitch.shell.kp = 0;//3;//8;
	ctrl.pitch.shell.ki = 0;//0.025;//0.04;
	ctrl.pitch.shell.kd = 0;//4;
	
	ctrl.pitch.core.kp = 0;//7;
	ctrl.pitch.core.kd = 0;
	
	//The data of roll 滾轉角
	ctrl.roll.shell.kp = 0;//3;//8;
	ctrl.roll.shell.ki = 0;//0.01;//0.04;
	ctrl.roll.shell.kd = 0;//4;

	ctrl.roll.core.kp =0;//8;
	ctrl.roll.core.kd = 0;
	
	//The data of yaw 偏航角
	ctrl.yaw.shell.kp = 0;//2.21;
	ctrl.yaw.shell.kd = 0;//0.04;
	
	ctrl.yaw.core.kp = 0;
	ctrl.yaw.core.kd = 0;
	//limit for the max increment
	ctrl.pitch.shell.increment_max =500;// 200;
	ctrl.roll.shell.increment_max =500;//  200;
	ctrl.yaw.shell.increment_max =500;// 200;
	
	ctrl.ctrlRate = 0;
	

}



struct _pid{
        float kp;			

		float ki;		
	    float kd;		
	    float increment;	    
	    float increment_max;	//?y·?×?′ó?μ￡¨??ê?￡?

	    float kp_out;			  //±èàyê?3?
		float ki_out;			  //?y·?ê?3?
	    float kd_out;			  //?￠·?ê?3?
	      
		float pid_out;			  //PID×üê?3?
		float err;
          };

struct _tache{
    struct _pid shell;			  //外環角度	  
    struct _pid core;			  //內環角速度
          };
	

struct _ctrl{
		  u8 ctrlRate;	     //?????μ?ê
        struct _tache pitch;     //????í¨μà
	    struct _tache roll;  	 //1?×aí¨μà
	    struct _tache yaw;   	 //??o?í¨μà
            };

extern struct _ctrl ctrl;