求大神指导这个姿态控制PID有木有问题

yufeng881005

     以前发过帖子，还是没搞明白，贴上自己的程序，求大家能给小弟一点指导：
   思路是这样的，大家看对不对：一个一个通道的调，先调俯仰通道，姿态是有商业产品以100HZ的频率发送姿态数据，DSPF2812串口A接四旋翼姿态收数据，串口B接收上位机发送的设定的俯仰偏航角度值（程序里roll_sp,pitch_sp,调试是设的值都为0）P，I，D （下面主程序 set_KP， set_KI， set_KD），中断接收数据，这样一个简单的但PID回路，用姿态角反馈，可调试时怎么也调不好这几个参数，不知道这个思路有没有问题，如果没问题该怎么调，是先把I，D设成0，调P，好了之后再调I，然后再调D吗？这样我也试过，但效果也不好，弄了好久都没进展，求大神们指导，非常感谢！
<1>这个是PID控制程序，是师兄参考论坛上的写的
void PID_control( struct control *axis)
{
      axis->result = axis->error*axis->kp;//;set_KP  
   axis->accerror = axis->accerror + axis->error*0.01;//
   if( axis->accerror > axis->accerror_limt  )
   { axis->accerror = axis->accerror_limt; }
   else if( axis->accerror < -axis->accerror_limt  )
   { axis->accerror = -axis->accerror_limt;}
   axis->result = axis->result + axis->accerror*axis->ki;
   
   axis->result = axis->result + 100*( axis->error - axis->errorp )*axis->kd;
   axis->errorp = axis->error;
}

void init_PID()
{
   roll_c.errorp = 0;
   roll_c.accerror = 0;
   roll_c.accerror_limt = 500;
   roll_c.kp = 3.80;
   roll_c.ki = 0.00;
   roll_c.kd = 0.480;
   roll_c.result = 0;

   pitch_c.errorp = 0;
   pitch_c.accerror = 0;
   pitch_c.accerror_limt = 500;
   pitch_c.kp = 3.95;         //1.58
   pitch_c.ki =0.00; //;0.30
   pitch_c.kd =0.680; //;0.42  0.38
   pitch_c.result = 0;

   head_c.errorp = 0;
   head_c.accerror = 0;
   head_c.accerror_limt = 1000;
   head_c.kp = 2;
   head_c.ki = 0.1;
   head_c.kd = 10;
   head_c.result = 0;

   height_c.errorp = 0;
   height_c.accerror = 0;
   height_c.accerror_limt = 1000;
   height_c.kp = 2;
   height_c.ki = 0;
   height_c.kd = 0;
   height_c.result = 0;
}
<主程序>
for(;;)
           {
             
          
            if(zitai_flag == 1 )
                        {
                           zitai_flag = 0;
                           roll_c.error = roll_sp - mti_roll;
                           pitch_c.error = mti_pitch - pitch_sp;
                           head_c.error = mti_heading - head_sp;
                           height_c.error = height - head_sp;

                           phi = mti_roll;
                           theta = mti_pitch;
                           psi = mti_heading;
                          
                           //roll_c.kp = set_KP;
                                                    //roll_c.ki = set_KI;
                           //roll_c.kd = set_KD;
                           pitch_c.kp = set_KP;
                                                    pitch_c.ki = set_KI;
                           pitch_c.kd = set_KD;
                           //PID_control( &roll_c );
                           PID_control( &pitch_c );
                          // PID_control( &head_c );
                                                 // PID_control( &height_c );
                                                     thr2 = -thr;//
                           DriveMotor2( (int)roll_c.result, (int)head_c.result, (int)pitch_c.result, (int)thr2 );
                         
                        }

40342zz

遇到同样问题  摇摆非常厉害  无法起飞  我把P参数从很小到很大都试了个遍了  莫非是PID用的不对