// (RD) changed from float to double
#include "uiuc_pah_ap.h"
-
+//#include <stdio.h>
double pah_ap(double pitch, double pitchrate, double pitch_ref, double V,
- double sample_t)
+ double sample_time, int init)
{
// changes by RD so function keeps previous values
static double u2prev;
static double x1prev;
static double x2prev;
static double x3prev;
- static int init = 0;
- if (init==0)
+ if (init == 0)
{
- init = -1;
u2prev = 0;
x1prev = 0;
x2prev = 0;
Ki = 0.5;
double u1,u2,u3;
u1 = Ktheta*(pitch_ref-pitch);
- u2 = u2prev + Ki*Ktheta*(pitch_ref-pitch)*sample_t;
+ u2 = u2prev + Ki*Ktheta*(pitch_ref-pitch)*sample_time;
u3 = Kq*pitchrate;
double totalU;
totalU = u1 + u2 - u3;
+ //printf("\nu1=%f\n",u1);
+ //printf("u2=%f\n",u2);
+ //printf("u3=%f\n",u3);
+ //printf("totalU=%f\n",totalU);
u2prev = u2;
// 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;
+ //deltae = x2;
+ //printf("x1prev=%f\n",x1prev);
+ //printf("x2prev=%f\n",x2prev);
+ //printf("x3prev=%f\n",x3prev);
x1prev = x1;
x2prev = x2;
x3prev = x3;
+ //printf("x1=%f\n",x1);
+ //printf("x2=%f\n",x2);
+ //printf("x3=%f\n",x3);
+ //printf("deltae=%f\n",deltae);
return deltae;
}