Sync. w. JSB CVS as of 15/01/2007

[flightgear.git] / src / FDM / UIUCModel / uiuc_pah_ap.cpp

diff --git a/src/FDM/UIUCModel/uiuc_pah_ap.cpp b/src/FDM/UIUCModel/uiuc_pah_ap.cpp
index 00ad520434a4610c72f9a8dbbb4a2b5c1c30a943..84e4f915c606d63037a0aee4bf5f3fe9007edc4f 100644 (file)
--- a/src/FDM/UIUCModel/uiuc_pah_ap.cpp
+++ b/src/FDM/UIUCModel/uiuc_pah_ap.cpp
@@ -43,20 +43,18 @@
 // (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;
@@ -74,22 +72,34 @@ double pah_ap(double pitch, double pitchrate, double pitch_ref, double V,
   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;
 }