1 /**********************************************************************
3 FILENAME: uiuc_auto_pilot.cpp
5 ----------------------------------------------------------------------
7 DESCRIPTION: calls autopilot routines
9 ----------------------------------------------------------------------
13 ----------------------------------------------------------------------
17 ----------------------------------------------------------------------
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
23 ----------------------------------------------------------------------
25 AUTHOR(S): Robert Deters <rdeters@uiuc.edu>
27 ----------------------------------------------------------------------
31 ----------------------------------------------------------------------
33 INPUTS: -V_rel_wind (or U_body)
38 ----------------------------------------------------------------------
47 ----------------------------------------------------------------------
49 CALLED BY: uiuc_coefficients
51 ----------------------------------------------------------------------
53 CALLS TO: uiuc_coef_lift
56 ----------------------------------------------------------------------
58 COPYRIGHT: (C) 2003 by Michael Selig
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.
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.
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.
73 **********************************************************************/
75 #include "uiuc_auto_pilot.h"
77 void uiuc_auto_pilot(double dt)
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;
90 V_rel_wind_ms = V_rel_wind * 0.3048;
91 Altitude_m = Altitude * 0.3048;
93 if (ap_pah_on && icing_demo==false && Simtime>=ap_pah_start_time)
95 //ap_Theta_ref_rad = ap_Theta_ref_deg * DEG_TO_RAD;
96 //if (ap_pah_on_prev == false) {
98 //ap_pah_on_prev = true;
100 elevator = pah_ap(Theta, Theta_dot, ap_Theta_ref_rad, V_rel_wind_ms, dt,
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;
108 //printf("elv1=%f\n",elevator);
111 if (ap_alh_on && icing_demo==false && Simtime>=ap_alh_start_time)
113 ap_alt_ref_m = ap_alt_ref_ft * 0.3048;
114 //if (ap_alh_on_prev == false)
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;
126 if (ap_rah_on && icing_demo==false && Simtime>=ap_rah_start_time)
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];
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;
146 if (ap_hh_on && icing_demo==false && Simtime>=ap_hh_start_time)
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];
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;