#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;
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);
+ 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_t)*ubarprev + Ktheta*pitchrate*sample_t;
+ 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_t;
+ u2 = u2prev + Ki*(Kh*(H_ref-H)-ubar)*sample_time;
u3 = Kq*pitchrate;
double totalU;
totalU = u1 + u2 - u3;
// 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;
+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_t;
+5.8694*totalU)*sample_time;
deltae = 57.2958*x2;
x1prev = x1;
x2prev = x2;