2.PID核心控制代码

void Filter(unsigned int set_hight)

{

      static float hight_error=0,hight_errorold=0;

      static float Alt_Hight[3];

      RcvIndex = 0;

      g_Hight = US_distance_mm;

      if(g_Hight>3000||g_Hight<50)

             g_Hight=g_HightOld;

      else

             g_HightOld=g_Hight;

      Alt_Hight[2]=Alt_Hight[1];

      Alt_Hight[1]=Alt_Hight[0];

      Alt_Hight[0]=(float)g_Hight;//*((float)cos(AngE.Roll/RtA)*(float)cos(AngE.Pitch/RtA));

      g_Alt_HightOld=g_Alt_Hight;

      g_Alt_Hight= Alt_Hight[0];

      if(flag.ARMED == 0)

             hight_increment=0;

      hight_errorold=hight_error;

      hight_error=set_hight-g_Alt_Hight;

      if(hight_error > 200)

             hight_error =200;

      if(hight_error<-200)

             hight_error = -200;

      hight_increment+=hight_error;

      if(hight_increment>150000)//150000

             hight_increment=150000;

      else if(hight_increment<70000)//70000

             hight_increment=70000;

      g_HightControlold=g_HightControl;

g_HightControl=g_hight_Kp*hight_error+g_hight_Ki*hight_increment+g_hight_Kd*(hight_error-hight_errorold);

      if(g_HightControl-g_HightControlold>50)                                     

          g_HightControl=g_HightControlold+50;

      else if(g_HightControlold-g_HightControl>50)

             g_HightControl=g_HightControlold-50;

      if(g_HightControl>700)

             g_HightControl=700;

      else if(g_HightControl<0)

             g_HightControl=0;

}