4 // * Heading Hold autopilot function. takes in *
6 // * variables and reference angle as arguments *
7 // * (there are other variable too as arguments *
8 // * as listed below) *
9 // * and returns the aileron and rudder deflection *
10 // * angle at every time step *
13 // * Written 2/14/03 by Vikrant Sharma *
15 // *****************************************************
17 //#include <iostream.h>
20 // define u2prev,x1prev,x2prev,x3prev,x4prev,x5prev and x6prev in the main
22 // that uses this autopilot function. give them initial values at origin.
23 // Pass these values to the A/P function as an argument and pass by
25 // Parameters passed as arguments to the function:
26 // yaw - current yaw angle difference from the trim value
27 // phi - Current roll angle ,,,,,,,,,,,,,,,,,,,,
28 // rollrate - current rate of change of roll angle
29 // yawrate - current rate of change of yaw angle
31 // yaw_ref - reference yaw angle to be tracked (amount of increase/decrease desired from the trim)
32 // sample_t - sampling time
33 // V - aircraft's current velocity
34 // u2prev - u2 value at the previous time step.
35 // x1prev - x1 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
36 // x2prev - x2 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
37 // x3prev - x3 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
38 // x4prev - x4 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
39 // x5prev - x5 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
40 // x6prev - x6 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
41 // the autpilot function (rah_ap) changes these values at every time step.
42 // so the simulator guys don't have to do it. Since these values are
43 // passed by reference to the function.
45 // Another important thing to do and for you simulator guys to check is the
46 // way I return the the deltaa (aileron deflection) and deltar (rudder deflection).
47 // I expect you guys to define an array that holds two float values. The first entry
48 // is for deltaa and the second for deltar and the pointer to this array (ctr_ptr) is also
49 // one of the arguments for the function rah_ap and then this function updates the value for
50 // deltae and deltaa every time its called. PLEASE CHECK IF THE SYNTAX IS RIGHT. Also note that
51 // these values have to be added to the initial trim values of the control deflection to get the
52 // entire control deflection.
54 #include "uiuc_hh_ap.h"
56 // (RD) changed float to double
58 void hh_ap(double phi, double yaw, double phirate, double yaw_ref,
59 double V, double sample_time, double b, double yawrate,
60 double ctr_defl[2], int init)
89 double x1, x2, x3, x4, x5, x6, phi_ref;
90 Kphi = 0.000975*V*V - 0.108*V + 2.335625;
95 drr = -0.000075*V*V + 0.0095*V -0.4606;
96 double u1,u2,u3,u4,u5,u6,u7,ubar,udbar;
97 phi_ref = Kyaw*(yaw_ref-yaw);
98 u1 = Kphi*(phi_ref-phi);
99 u2 = u2prev + Ki*Kphi*(phi_ref-phi)*sample_time;
104 K1 = Kr*9.8*sin(phi)/V;
109 ubar = phirate*b/(2*V);
110 udbar = yawrate*b/(2*V);
111 // the following is using the actuator dynamics to get the aileron
112 // angle, given in Beaver.
113 // the actuator dynamics for Twin Otter are still unavailable.
114 x1 = x1prev +(-10.6*x1prev - 2.64*x2prev -7.58*x3prev +
115 27.46*u4 -0.0008*ubar)*sample_time;
116 x2 = x2prev + x3prev*sample_time;
117 x3 = x3prev + (1.09*x1prev - 558.86*x2prev - 23.35*x3prev +
118 3.02*u4 - 0.164*ubar)*sample_time;
124 // the following is using the actuator dynamics to get the rudder
125 // angle, given in Beaver.
126 // the actuator dynamics for Twin Otter are still unavailable.
127 x4 = x4prev +(-9.2131*x4prev + 5.52*x5prev + 16*x6prev +
128 24.571*u7 + 0.0036*udbar)*sample_time;
129 x5 = x5prev + x6prev*sample_time;
130 x6 = x6prev + (0.672*x4prev - 919.78*x5prev - 56.453*x6prev +
131 7.54*u7 - 0.636*udbar)*sample_time;
136 ctr_defl[0] = deltaa;
137 ctr_defl[1] = deltar;