1 /**********************************************************************
3 FILENAME: uiuc_aerodeflections.cpp
5 ----------------------------------------------------------------------
7 DESCRIPTION: determine the aero control surface deflections
12 ----------------------------------------------------------------------
16 ----------------------------------------------------------------------
18 REFERENCES: based on deflection portions of c172_aero.c and
21 ----------------------------------------------------------------------
23 HISTORY: 01/30/2000 initial release
24 04/05/2000 (JS) added zero_Long_trim command
25 07/05/2001 (RD) removed elevator_tab addition to
27 11/12/2001 (RD) added new flap routine. Needed for
28 Twin Otter non-linear model
29 12/28/2002 (MSS) added simple SAS capability
30 03/03/2003 (RD) changed flap code to call
31 uiuc_find_position to determine
33 08/20/2003 (RD) changed spoiler variables and code
34 to match flap conventions. Changed
35 flap_pos_pct to flap_pos_norm
37 ----------------------------------------------------------------------
39 AUTHOR(S): Jeff Scott http://www.jeffscott.net/
40 Robert Deters <rdeters@uiuc.edu>
41 Michael Selig <m-selig@uiuc.edu>
43 ----------------------------------------------------------------------
47 ----------------------------------------------------------------------
51 ----------------------------------------------------------------------
55 ----------------------------------------------------------------------
59 ----------------------------------------------------------------------
63 ----------------------------------------------------------------------
65 COPYRIGHT: (C) 2000 by Michael Selig
67 This program is free software; you can redistribute it and/or
68 modify it under the terms of the GNU General Public License
69 as published by the Free Software Foundation.
71 This program is distributed in the hope that it will be useful,
72 but WITHOUT ANY WARRANTY; without even the implied warranty of
73 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
74 GNU General Public License for more details.
76 You should have received a copy of the GNU General Public License
77 along with this program; if not, write to the Free Software
78 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
80 **********************************************************************/
84 #include "uiuc_aerodeflections.h"
86 void uiuc_aerodeflections( double dt )
90 static double elev_trim;
92 if (use_uiuc_network) {
101 Flap_handle = flap_percent * flap_max;
102 if (outside_control) {
103 pilot_elev_no = true;
106 pilot_throttle_no = true;
107 Long_trim = elev_trim;
111 if (zero_Long_trim) {
116 if (Lat_control <= 0)
117 aileron = - Lat_control * damin * DEG_TO_RAD;
119 aileron = - Lat_control * damax * DEG_TO_RAD;
122 if (Long_trim <= 0) {
123 elevator = Long_trim * demax * DEG_TO_RAD;
124 demax_remain = demax + Long_trim * demax;
125 demin_remain = -Long_trim * demax + demin;
126 if (Long_control <= 0)
127 elevator += Long_control * demax_remain * DEG_TO_RAD;
129 elevator += Long_control * demin_remain * DEG_TO_RAD;
131 elevator = Long_trim * demin * DEG_TO_RAD;
132 demin_remain = demin - Long_trim * demin;
133 demax_remain = Long_trim * demin + demax;
134 if (Long_control >=0)
135 elevator += Long_control * demin_remain * DEG_TO_RAD;
137 elevator += Long_control * demax_remain * DEG_TO_RAD;
140 if ((Long_control+Long_trim) <= 0)
141 elevator = (Long_control + Long_trim) * demax * DEG_TO_RAD;
143 elevator = (Long_control + Long_trim) * demin * DEG_TO_RAD;
146 if (Rudder_pedal <= 0)
147 rudder = -Rudder_pedal * drmin * DEG_TO_RAD;
149 rudder = -Rudder_pedal * drmax * DEG_TO_RAD;
151 /* at this point aileron, elevator, and rudder commands are known (in radians)
152 * add in SAS and any other mixing control laws
155 // SAS for pitch, positive elevator is TED
156 if (use_elevator_stick_gain)
157 elevator *= elevator_stick_gain;
158 if (use_elevator_sas_type1) {
159 elevator_sas = elevator_sas_KQ * Q_body;
160 if (use_elevator_sas_max && (elevator_sas > (elevator_sas_max * DEG_TO_RAD))) {
161 elevator += elevator_sas_max * DEG_TO_RAD;
162 //elevator_sas = elevator_sas_max;
163 } else if (use_elevator_sas_min && (elevator_sas < (elevator_sas_min * DEG_TO_RAD))) {
164 elevator += elevator_sas_min * DEG_TO_RAD;
165 //elevator_sas = elevator_sas_min;
168 elevator += elevator_sas;
169 // don't exceed the elevator deflection limits
170 if (elevator > demax * DEG_TO_RAD)
171 elevator = demax * DEG_TO_RAD;
172 else if (elevator < (-demin * DEG_TO_RAD))
173 elevator = -demin * DEG_TO_RAD;
176 // SAS for roll, positive aileron is right aileron TED
177 if (use_aileron_stick_gain)
178 aileron *= aileron_stick_gain;
179 if (use_aileron_sas_type1) {
180 aileron_sas = aileron_sas_KP * P_body;
181 if (use_aileron_sas_max && (fabs(aileron_sas) > (aileron_sas_max * DEG_TO_RAD)))
182 if (aileron_sas >= 0) {
183 aileron += aileron_sas_max * DEG_TO_RAD;
184 //aileron_sas = aileron_sas_max;
186 aileron += -aileron_sas_max * DEG_TO_RAD;
187 //aileron_sas = -aileron_sas;
190 aileron += aileron_sas;
191 // don't exceed aileron deflection limits
192 if (fabs(aileron) > (damax * DEG_TO_RAD))
194 aileron = damax * DEG_TO_RAD;
196 aileron = -damax * DEG_TO_RAD;
199 // SAS for yaw, positive rudder deflection is TEL
200 if (use_rudder_stick_gain)
201 rudder *= rudder_stick_gain;
202 if (use_rudder_sas_type1) {
203 rudder_sas = rudder_sas_KR * R_body;
204 if (use_rudder_sas_max && (fabs(rudder_sas) > (rudder_sas_max*DEG_TO_RAD)))
205 if (rudder_sas >= 0) {
206 rudder += rudder_sas_max * DEG_TO_RAD;
207 //rudder_sas = rudder_sas_max;
209 rudder += -rudder_sas_max * DEG_TO_RAD;
210 //rudder_sas = rudder_sas_max;
213 rudder += rudder_sas;
214 // don't exceed rudder deflection limits, assumes drmax = drmin,
215 // i.e. equal rudder throws left and right
216 if (fabs(rudder) > drmax)
218 rudder = drmax * DEG_TO_RAD;
220 rudder = -drmax * DEG_TO_RAD;
223 /* This old code in the first part of the if-block needs to get removed from FGFS. 030222 MSS
224 The second part of the if-block is rewritten below.
225 double prevFlapHandle = 0.0f;
226 double flap_transit_rate;
227 bool flaps_in_transit = false;
229 if (old_flap_routine) {
231 // check for lowest flap setting
232 if (Flap_handle < dfArray[1]) {
233 Flap_handle = dfArray[1];
234 prevFlapHandle = Flap_handle;
236 } else if (Flap_handle > dfArray[ndf]) {
237 // check for highest flap setting
238 Flap_handle = dfArray[ndf];
239 prevFlapHandle = Flap_handle;
242 // otherwise in between
243 if(Flap_handle != prevFlapHandle)
244 flaps_in_transit = true;
245 if(flaps_in_transit) {
247 while (dfArray[iflap] < Flap_handle)
249 if (flap < Flap_handle) {
250 if (TimeArray[iflap] > 0)
251 flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / TimeArray[iflap+1];
253 flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / 5;
255 if (TimeArray[iflap+1] > 0)
256 flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / TimeArray[iflap+1];
258 flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / 5;
260 if(fabs (flap - Flap_handle) > dt * flap_transit_rate)
261 flap += flap_transit_rate * dt;
263 flaps_in_transit = false;
268 prevFlapHandle = Flap_handle;
271 // designed for the twin otter non-linear model
272 if (!outside_control) {
273 flap_percent = Flap_handle / 30.0; // percent of flaps desired
274 if (flap_percent>=0.31 && flap_percent<=0.35)
275 flap_percent = 1.0 / 3.0;
276 if (flap_percent>=0.65 && flap_percent<=0.69)
277 flap_percent = 2.0 / 3.0;
279 flap_cmd_deg = flap_percent * flap_max; // angle of flaps desired
280 flap_moving_rate = flap_rate * dt; // amount flaps move per time step
282 // determine flap position with respect to the flap goal
283 if (flap_pos < flap_cmd_deg) {
284 flap_pos += flap_moving_rate;
285 if (flap_pos > flap_cmd_deg)
286 flap_pos = flap_cmd_deg;
287 } else if (flap_pos > flap_cmd_deg) {
288 flap_pos -= flap_moving_rate;
289 if (flap_pos < flap_cmd_deg)
290 flap_pos = flap_cmd_deg;
296 if (!outside_control && use_flaps) {
297 // angle of flaps desired [rad]
298 flap_cmd = Flap_handle * DEG_TO_RAD;
299 // amount flaps move per time step [rad/sec]
300 flap_increment_per_timestep = flap_rate * dt * DEG_TO_RAD;
301 // determine flap position [rad] with respect to flap command
302 flap_pos = uiuc_find_position(flap_cmd,flap_increment_per_timestep,flap_pos);
303 // get the normalized position
304 flap_pos_norm = flap_pos/(flap_max * DEG_TO_RAD);
309 // angle of spoilers desired [rad]
310 spoiler_cmd = Spoiler_handle * DEG_TO_RAD;
311 // amount spoilers move per time step [rad/sec]
312 spoiler_increment_per_timestep = spoiler_rate * dt * DEG_TO_RAD;
313 // determine spoiler position [rad] with respect to spoiler command
314 spoiler_pos = uiuc_find_position(spoiler_cmd,spoiler_increment_per_timestep,spoiler_pos);
315 // get the normailized position
316 spoiler_pos_norm = spoiler_pos/(spoiler_max * DEG_TO_RAD);
323 // end uiuc_aerodeflections.cpp