]> git.mxchange.org Git - flightgear.git/blob - src/FDM/UIUCModel/uiuc_pah_ap.cpp
Make sure sign is preserved for turbulence direction.
[flightgear.git] / src / FDM / UIUCModel / uiuc_pah_ap.cpp
1 // *                                                   *
2 // *   pah_ap.C                                        *
3 // *                                                   *
4 // *  Pah autopilot function. takes in the state       *
5 // *  variables and reference angle as arguments       *
6 // *  (there are other variable too as arguments       *
7 // *  as listed below)                                 *
8 // *  and returns the elevator deflection angle at     *
9 // *  every time step.                                 * 
10 // *                                                   *
11 // *   Written 2/11/02 by Vikrant Sharma               *
12 // *                                                   *
13 // *****************************************************
14
15 //#include <iostream.h>
16 //#include <stddef.h>  
17
18 // define u2prev,x1prev,x2prev and x3prev in the main function
19 // that uses this autopilot function. give them initial values at origin.
20 // Pass these values to the A/P function as an argument and pass by
21 // reference 
22 // Parameters passed as arguments to the function:
23 // pitch - Current pitch angle 
24 // pitchrate - current rate of change of pitch angle
25 // pitch_ref - reference pitch angle to be tracked
26 // sample_t - sampling time
27 // V - aircraft's current velocity
28 // u2prev - u2 value at the previous time step. 
29 // x1prev - x1 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
30 // x2prev - x2 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
31 // x3prev - x3 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, 
32 // the autpilot function (pah_ap) changes these values at every time step.
33 // so the simulator guys don't have to do it. Since these values are
34 // passed by reference to the function.  
35
36 // (RD) Units for the variables
37 // pitch = radians
38 // pitchrate = rad/s
39 // pitch_ref = rad
40 // sample_t = seconds
41 // V = m/s
42
43 // (RD) changed from float to double
44
45 #include "uiuc_pah_ap.h"
46
47 double pah_ap(double pitch, double pitchrate, double pitch_ref, double V,
48               double sample_t, int init)
49 {
50   // changes by RD so function keeps previous values
51   static double u2prev;
52   static double x1prev;
53   static double x2prev;
54   static double x3prev;
55
56   if (init == 0)
57     {
58       u2prev = 0;
59       x1prev = 0;
60       x2prev = 0;
61       x3prev = 0;
62     }
63   // end changes
64
65   double Ki;
66   double Ktheta;
67   double Kq;
68   double deltae;
69   double x1, x2, x3;
70   Ktheta = -0.0004*V*V + 0.0479*V - 2.409;
71   Kq = -0.0005*V*V + 0.054*V - 1.5931;
72   Ki = 0.5;
73   double u1,u2,u3;
74   u1 = Ktheta*(pitch_ref-pitch);
75   u2 = u2prev + Ki*Ktheta*(pitch_ref-pitch)*sample_t;
76   u3 = Kq*pitchrate;
77   double totalU;
78   totalU = u1 + u2 - u3;
79   u2prev = u2;
80   // the following is using the actuator dynamics given in Beaver.
81   // the actuator dynamics for Twin Otter are still unavailable.
82   x1 = x1prev +(-10.951*x1prev + 7.2721*x2prev + 20.7985*x3prev +
83                 25.1568*totalU)*sample_t;
84   x2 = x2prev + x3prev*sample_t;
85   x3 = x3prev + (7.3446*x1prev - 668.6713*x2prev - 16.8697*x3prev +
86                  5.8694*totalU)*sample_t;
87   deltae = 57.2958*x2;
88   x1prev = x1;
89   x2prev = x2;
90   x3prev = x3;
91   return deltae;
92
93                         
94
95                 
96