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., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
72 USA or view http://www.gnu.org/copyleft/gpl.html.
74 **********************************************************************/
76 #include "uiuc_auto_pilot.h"
78 void uiuc_auto_pilot(double dt)
82 //static bool ap_pah_on_prev = false;
83 static int ap_pah_init = 0;
84 //static bool ap_alh_on_prev = false;
85 static int ap_alh_init = 0;
86 static int ap_rah_init = 0;
87 static int ap_hh_init = 0;
91 V_rel_wind_ms = V_rel_wind * 0.3048;
92 Altitude_m = Altitude * 0.3048;
94 if (ap_pah_on && icing_demo==false && Simtime>=ap_pah_start_time)
96 //ap_Theta_ref_rad = ap_Theta_ref_deg * DEG_TO_RAD;
97 //if (ap_pah_on_prev == false) {
99 //ap_pah_on_prev = true;
101 elevator = pah_ap(Theta, Theta_dot, ap_Theta_ref_rad, V_rel_wind_ms, dt,
103 if (elevator*RAD_TO_DEG <= -1*demax)
104 elevator = -1*demax * DEG_TO_RAD;
105 if (elevator*RAD_TO_DEG >= demin)
106 elevator = demin * DEG_TO_RAD;
109 //printf("elv1=%f\n",elevator);
112 if (ap_alh_on && icing_demo==false && Simtime>=ap_alh_start_time)
114 ap_alt_ref_m = ap_alt_ref_ft * 0.3048;
115 //if (ap_alh_on_prev == false)
117 elevator = alh_ap(Theta, Theta_dot, ap_alt_ref_m, Altitude_m,
118 V_rel_wind_ms, dt, ap_alh_init);
119 if (elevator*RAD_TO_DEG <= -1*demax)
120 elevator = -1*demax * DEG_TO_RAD;
121 if (elevator*RAD_TO_DEG >= demin)
122 elevator = demin * DEG_TO_RAD;
127 if (ap_rah_on && icing_demo==false && Simtime>=ap_rah_start_time)
130 rah_ap(Phi, Phi_dot, ap_Phi_ref_rad, V_rel_wind_ms, dt,
131 bw_m, Psi_dot, ail_rud, ap_rah_init);
132 aileron = ail_rud[0];
134 if (aileron*RAD_TO_DEG <= -1*damax)
135 aileron = -1*damax * DEG_TO_RAD;
136 if (aileron*RAD_TO_DEG >= damin)
137 aileron = damin * DEG_TO_RAD;
138 if (rudder*RAD_TO_DEG <= -1*drmax)
139 rudder = -1*drmax * DEG_TO_RAD;
140 if (rudder*RAD_TO_DEG >= drmin)
141 rudder = drmin * DEG_TO_RAD;
147 if (ap_hh_on && icing_demo==false && Simtime>=ap_hh_start_time)
150 hh_ap(Phi, Psi, Phi_dot, ap_Psi_ref_rad, V_rel_wind_ms, dt,
151 bw_m, Psi_dot, ail_rud, ap_hh_init);
152 aileron = ail_rud[0];
154 if (aileron*RAD_TO_DEG <= -1*damax)
155 aileron = -1*damax * DEG_TO_RAD;
156 if (aileron*RAD_TO_DEG >= damin)
157 aileron = damin * DEG_TO_RAD;
158 if (rudder*RAD_TO_DEG <= -1*drmax)
159 rudder = -1*drmax * DEG_TO_RAD;
160 if (rudder*RAD_TO_DEG >= drmin)
161 rudder = drmin * DEG_TO_RAD;