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;
200 // SAS for yaw, positive rudder deflection is TEL
201 if (use_rudder_stick_gain)
202 rudder *= rudder_stick_gain;
203 if (use_rudder_sas_type1) {
204 rudder_sas = rudder_sas_KR * R_body;
205 if (use_rudder_sas_max && (fabs(rudder_sas) > (rudder_sas_max*DEG_TO_RAD)))
206 if (rudder_sas >= 0) {
207 rudder += rudder_sas_max * DEG_TO_RAD;
208 //rudder_sas = rudder_sas_max;
210 rudder += -rudder_sas_max * DEG_TO_RAD;
211 //rudder_sas = rudder_sas_max;
214 rudder += rudder_sas;
215 // don't exceed rudder deflection limits, assumes drmax = drmin,
216 // i.e. equal rudder throws left and right
217 if (fabs(rudder) > drmax) {
219 rudder = drmax * DEG_TO_RAD;
221 rudder = -drmax * DEG_TO_RAD;
225 /* This old code in the first part of the if-block needs to get removed from FGFS. 030222 MSS
226 The second part of the if-block is rewritten below.
227 double prevFlapHandle = 0.0f;
228 double flap_transit_rate;
229 bool flaps_in_transit = false;
231 if (old_flap_routine) {
233 // check for lowest flap setting
234 if (Flap_handle < dfArray[1]) {
235 Flap_handle = dfArray[1];
236 prevFlapHandle = Flap_handle;
238 } else if (Flap_handle > dfArray[ndf]) {
239 // check for highest flap setting
240 Flap_handle = dfArray[ndf];
241 prevFlapHandle = Flap_handle;
244 // otherwise in between
245 if(Flap_handle != prevFlapHandle)
246 flaps_in_transit = true;
247 if(flaps_in_transit) {
249 while (dfArray[iflap] < Flap_handle)
251 if (flap < Flap_handle) {
252 if (TimeArray[iflap] > 0)
253 flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / TimeArray[iflap+1];
255 flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / 5;
257 if (TimeArray[iflap+1] > 0)
258 flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / TimeArray[iflap+1];
260 flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / 5;
262 if(fabs (flap - Flap_handle) > dt * flap_transit_rate)
263 flap += flap_transit_rate * dt;
265 flaps_in_transit = false;
270 prevFlapHandle = Flap_handle;
273 // designed for the twin otter non-linear model
274 if (!outside_control) {
275 flap_percent = Flap_handle / 30.0; // percent of flaps desired
276 if (flap_percent>=0.31 && flap_percent<=0.35)
277 flap_percent = 1.0 / 3.0;
278 if (flap_percent>=0.65 && flap_percent<=0.69)
279 flap_percent = 2.0 / 3.0;
281 flap_cmd_deg = flap_percent * flap_max; // angle of flaps desired
282 flap_moving_rate = flap_rate * dt; // amount flaps move per time step
284 // determine flap position with respect to the flap goal
285 if (flap_pos < flap_cmd_deg) {
286 flap_pos += flap_moving_rate;
287 if (flap_pos > flap_cmd_deg)
288 flap_pos = flap_cmd_deg;
289 } else if (flap_pos > flap_cmd_deg) {
290 flap_pos -= flap_moving_rate;
291 if (flap_pos < flap_cmd_deg)
292 flap_pos = flap_cmd_deg;
298 if (!outside_control && use_flaps) {
299 // angle of flaps desired [rad]
300 flap_cmd = Flap_handle * DEG_TO_RAD;
301 // amount flaps move per time step [rad/sec]
302 flap_increment_per_timestep = flap_rate * dt * DEG_TO_RAD;
303 // determine flap position [rad] with respect to flap command
304 flap_pos = uiuc_find_position(flap_cmd,flap_increment_per_timestep,flap_pos);
305 // get the normalized position
306 flap_pos_norm = flap_pos/(flap_max * DEG_TO_RAD);
311 // angle of spoilers desired [rad]
312 spoiler_cmd = Spoiler_handle * DEG_TO_RAD;
313 // amount spoilers move per time step [rad/sec]
314 spoiler_increment_per_timestep = spoiler_rate * dt * DEG_TO_RAD;
315 // determine spoiler position [rad] with respect to spoiler command
316 spoiler_pos = uiuc_find_position(spoiler_cmd,spoiler_increment_per_timestep,spoiler_pos);
317 // get the normailized position
318 spoiler_pos_norm = spoiler_pos/(spoiler_max * DEG_TO_RAD);
325 // end uiuc_aerodeflections.cpp