- double Ki;
- double Ktheta;
- double Kq;
- double deltae;
- double Kh,Kd;
- double x1, x2, x3;
- Ktheta = -0.0004*V*V + 0.0479*V - 2.409;
- Kq = -0.0005*V*V + 0.054*V - 1.5931;
- Ki = 0.5;
- Kh = -0.25*pi/180 + (((-0.15 + 0.25)*pi/180)/(20))*(V-60);
- Kd = -0.0025*V + 0.2875;
- double u1,u2,u3,ubar;
- ubar = (1-Kd*sample_t)*ubarprev + Ktheta*pitchrate*sample_t;
- u1 = Kh*(H_ref-H) - ubar;
- u2 = u2prev + Ki*(Kh*(H_ref-H)-ubar)*sample_t;
- u3 = Kq*pitchrate;
- double totalU;
- totalU = u1 + u2 - u3;
- u2prev = u2;
- ubarprev = ubar;
+ double Ki;
+ double Ktheta;
+ double Kq;
+ double deltae;
+ double Kh,Kd;
+ double x1, x2, x3;
+ Ktheta = -0.0004*V*V + 0.0479*V - 2.409;
+ Kq = -0.0005*V*V + 0.054*V - 1.5931;
+ Ki = 0.5;
+ Kh = -0.25*LS_PI/180 + (((-0.15 + 0.25)*LS_PI/180)/(20))*(V-60);
+ Kd = -0.0025*V + 0.2875;
+ double u1,u2,u3,ubar;
+ ubar = (1-Kd*sample_time)*ubarprev + Ktheta*pitchrate*sample_time;
+ u1 = Kh*(H_ref-H) - ubar;
+ u2 = u2prev + Ki*(Kh*(H_ref-H)-ubar)*sample_time;
+ u3 = Kq*pitchrate;
+ double totalU;
+ totalU = u1 + u2 - u3;
+ u2prev = u2;
+ ubarprev = ubar;