#include "uiuc_alh_ap.h"
double alh_ap(double pitch, double pitchrate, double H_ref, double H,
- double V, double sample_t, int init)
+ double V, double sample_time, int init)
{
// changes by RD so function keeps previous values
static double u2prev;
static double x3prev;
static double ubarprev;
- double pi = 3.14159;
-
if (init == 0)
{
u2prev = 0;
ubarprev = 0;
}
- 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;
// the following is using the actuator dynamics given in Beaver.
// the actuator dynamics for Twin Otter are still unavailable.
- x1 = x1prev +(-10.951*x1prev + 7.2721*x2prev + 20.7985*x3prev +
-25.1568*totalU)*sample_t;
- x2 = x2prev + x3prev*sample_t;
- x3 = x3prev + (7.3446*x1prev - 668.6713*x2prev - 16.8697*x3prev +
-5.8694*totalU)*sample_t;
- deltae = 57.2958*x2;
- x1prev = x1;
- x2prev = x2;
- x3prev = x3;
+ x1 = x1prev +(-10.951*x1prev + 7.2721*x2prev + 20.7985*x3prev +
+25.1568*totalU)*sample_time;
+ x2 = x2prev + x3prev*sample_time;
+ x3 = x3prev + (7.3446*x1prev - 668.6713*x2prev - 16.8697*x3prev +
+5.8694*totalU)*sample_time;
+ deltae = 57.2958*x2;
+ x1prev = x1;
+ x2prev = x2;
+ x3prev = x3;
return deltae;
}