1 /**********************************************************************
2
3 FILENAME: uiuc_auto_pilot.cpp
4
5 ----------------------------------------------------------------------
6
7 DESCRIPTION: calls autopilot routines
8
9 ----------------------------------------------------------------------
10
11 STATUS: alpha version
12
13 ----------------------------------------------------------------------
14
15 REFERENCES:
16
17 ----------------------------------------------------------------------
18
19 HISTORY: 09/04/2003 initial release (with PAH)
20 10/31/2003 added ALH autopilot
21 11/04/2003 added RAH and HH autopilots
22
23 ----------------------------------------------------------------------
24
25 AUTHOR(S): Robert Deters <rdeters@uiuc.edu>
26
27 ----------------------------------------------------------------------
28
29 VARIABLES:
30
31 ----------------------------------------------------------------------
32
33 INPUTS: -V_rel_wind (or U_body)
34 -dyn_on_speed
35 -ice on/off
36 -phugoid on/off
37
38 ----------------------------------------------------------------------
39
40 OUTPUTS: -CL
41 -CD
42 -Cm
43 -CY
44 -Cl
45 -Cn
46
47 ----------------------------------------------------------------------
48
49 CALLED BY: uiuc_coefficients
50
51 ----------------------------------------------------------------------
52
53 CALLS TO: uiuc_coef_lift
54 uiuc_coef_drag
55
56 ----------------------------------------------------------------------
57
58 COPYRIGHT: (C) 2003 by Michael Selig
59
60 This program is free software; you can redistribute it and/or
61 modify it under the terms of the GNU General Public License
62 as published by the Free Software Foundation.
63
64 This program is distributed in the hope that it will be useful,
65 but WITHOUT ANY WARRANTY; without even the implied warranty of
66 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
67 GNU General Public License for more details.
68
69 You should have received a copy of the GNU General Public License
70 along with this program; if not, write to the Free Software
71 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
72
73 **********************************************************************/
74
75 #include "uiuc_auto_pilot.h"
76 //#include <stdio.h>
uiuc_auto_pilot(double dt)77 void uiuc_auto_pilot(double dt)
78 {
79 double V_rel_wind_ms;
80 double Altitude_m;
81 //static bool ap_pah_on_prev = false;
82 static int ap_pah_init = 0;
83 //static bool ap_alh_on_prev = false;
84 static int ap_alh_init = 0;
85 static int ap_rah_init = 0;
86 static int ap_hh_init = 0;
87 double ap_alt_ref_m;
88 double bw_m;
89 double ail_rud[2];
90 V_rel_wind_ms = V_rel_wind * 0.3048;
91 Altitude_m = Altitude * 0.3048;
92
93 if (ap_pah_on && icing_demo==false && Simtime>=ap_pah_start_time)
94 {
95 //ap_Theta_ref_rad = ap_Theta_ref_deg * DEG_TO_RAD;
96 //if (ap_pah_on_prev == false) {
97 //ap_pah_init = 1;
98 //ap_pah_on_prev = true;
99 //}
100 elevator = pah_ap(Theta, Theta_dot, ap_Theta_ref_rad, V_rel_wind_ms, dt,
101 ap_pah_init);
102 if (elevator*RAD_TO_DEG <= -1*demax)
103 elevator = -1*demax * DEG_TO_RAD;
104 if (elevator*RAD_TO_DEG >= demin)
105 elevator = demin * DEG_TO_RAD;
106 pilot_elev_no=true;
107 ap_pah_init=1;
108 //printf("elv1=%f\n",elevator);
109 }
110
111 if (ap_alh_on && icing_demo==false && Simtime>=ap_alh_start_time)
112 {
113 ap_alt_ref_m = ap_alt_ref_ft * 0.3048;
114 //if (ap_alh_on_prev == false)
115 //ap_alh_init = 0;
116 elevator = alh_ap(Theta, Theta_dot, ap_alt_ref_m, Altitude_m,
117 V_rel_wind_ms, dt, ap_alh_init);
118 if (elevator*RAD_TO_DEG <= -1*demax)
119 elevator = -1*demax * DEG_TO_RAD;
120 if (elevator*RAD_TO_DEG >= demin)
121 elevator = demin * DEG_TO_RAD;
122 pilot_elev_no=true;
123 ap_alh_init = 1;
124 }
125
126 if (ap_rah_on && icing_demo==false && Simtime>=ap_rah_start_time)
127 {
128 bw_m = bw * 0.3048;
129 rah_ap(Phi, Phi_dot, ap_Phi_ref_rad, V_rel_wind_ms, dt,
130 bw_m, Psi_dot, ail_rud, ap_rah_init);
131 aileron = ail_rud[0];
132 rudder = ail_rud[1];
133 if (aileron*RAD_TO_DEG <= -1*damax)
134 aileron = -1*damax * DEG_TO_RAD;
135 if (aileron*RAD_TO_DEG >= damin)
136 aileron = damin * DEG_TO_RAD;
137 if (rudder*RAD_TO_DEG <= -1*drmax)
138 rudder = -1*drmax * DEG_TO_RAD;
139 if (rudder*RAD_TO_DEG >= drmin)
140 rudder = drmin * DEG_TO_RAD;
141 pilot_ail_no=true;
142 pilot_rud_no=true;
143 ap_rah_init = 1;
144 }
145
146 if (ap_hh_on && icing_demo==false && Simtime>=ap_hh_start_time)
147 {
148 bw_m = bw * 0.3048;
149 hh_ap(Phi, Psi, Phi_dot, ap_Psi_ref_rad, V_rel_wind_ms, dt,
150 bw_m, Psi_dot, ail_rud, ap_hh_init);
151 aileron = ail_rud[0];
152 rudder = ail_rud[1];
153 if (aileron*RAD_TO_DEG <= -1*damax)
154 aileron = -1*damax * DEG_TO_RAD;
155 if (aileron*RAD_TO_DEG >= damin)
156 aileron = damin * DEG_TO_RAD;
157 if (rudder*RAD_TO_DEG <= -1*drmax)
158 rudder = -1*drmax * DEG_TO_RAD;
159 if (rudder*RAD_TO_DEG >= drmin)
160 rudder = drmin * DEG_TO_RAD;
161 pilot_ail_no=true;
162 pilot_rud_no=true;
163 ap_hh_init = 1;
164 }
165 }
166