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., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
79 USA or view http://www.gnu.org/copyleft/gpl.html.
81 **********************************************************************/
85 #include "uiuc_aerodeflections.h"
87 void uiuc_aerodeflections( double dt )
91 static double elev_trim;
93 if (use_uiuc_network) {
100 if (elev_trim < -1.0)
102 Flap_handle = flap_percent * flap_max;
103 if (outside_control) {
104 pilot_elev_no = true;
107 pilot_throttle_no = true;
108 Long_trim = elev_trim;
112 if (zero_Long_trim) {
117 if (Lat_control <= 0)
118 aileron = - Lat_control * damin * DEG_TO_RAD;
120 aileron = - Lat_control * damax * DEG_TO_RAD;
123 if (Long_trim <= 0) {
124 elevator = Long_trim * demax * DEG_TO_RAD;
125 demax_remain = demax + Long_trim * demax;
126 demin_remain = -Long_trim * demax + demin;
127 if (Long_control <= 0)
128 elevator += Long_control * demax_remain * DEG_TO_RAD;
130 elevator += Long_control * demin_remain * DEG_TO_RAD;
132 elevator = Long_trim * demin * DEG_TO_RAD;
133 demin_remain = demin - Long_trim * demin;
134 demax_remain = Long_trim * demin + demax;
135 if (Long_control >=0)
136 elevator += Long_control * demin_remain * DEG_TO_RAD;
138 elevator += Long_control * demax_remain * DEG_TO_RAD;
141 if ((Long_control+Long_trim) <= 0)
142 elevator = (Long_control + Long_trim) * demax * DEG_TO_RAD;
144 elevator = (Long_control + Long_trim) * demin * DEG_TO_RAD;
147 if (Rudder_pedal <= 0)
148 rudder = -Rudder_pedal * drmin * DEG_TO_RAD;
150 rudder = -Rudder_pedal * drmax * DEG_TO_RAD;
152 /* at this point aileron, elevator, and rudder commands are known (in radians)
153 * add in SAS and any other mixing control laws
156 // SAS for pitch, positive elevator is TED
157 if (use_elevator_stick_gain)
158 elevator *= elevator_stick_gain;
159 if (use_elevator_sas_type1) {
160 elevator_sas = elevator_sas_KQ * Q_body;
161 if (use_elevator_sas_max && (elevator_sas > (elevator_sas_max * DEG_TO_RAD))) {
162 elevator += elevator_sas_max * DEG_TO_RAD;
163 //elevator_sas = elevator_sas_max;
164 } else if (use_elevator_sas_min && (elevator_sas < (elevator_sas_min * DEG_TO_RAD))) {
165 elevator += elevator_sas_min * DEG_TO_RAD;
166 //elevator_sas = elevator_sas_min;
169 elevator += elevator_sas;
170 // don't exceed the elevator deflection limits
171 if (elevator > demax * DEG_TO_RAD)
172 elevator = demax * DEG_TO_RAD;
173 else if (elevator < (-demin * DEG_TO_RAD))
174 elevator = -demin * DEG_TO_RAD;
177 // SAS for roll, positive aileron is right aileron TED
178 if (use_aileron_stick_gain)
179 aileron *= aileron_stick_gain;
180 if (use_aileron_sas_type1) {
181 aileron_sas = aileron_sas_KP * P_body;
182 if (use_aileron_sas_max && (fabs(aileron_sas) > (aileron_sas_max * DEG_TO_RAD)))
183 if (aileron_sas >= 0) {
184 aileron += aileron_sas_max * DEG_TO_RAD;
185 //aileron_sas = aileron_sas_max;
187 aileron += -aileron_sas_max * DEG_TO_RAD;
188 //aileron_sas = -aileron_sas;
191 aileron += aileron_sas;
192 // don't exceed aileron deflection limits
193 if (fabs(aileron) > (damax * DEG_TO_RAD))
195 aileron = damax * DEG_TO_RAD;
197 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;
224 /* This old code in the first part of the if-block needs to get removed from FGFS. 030222 MSS
225 The second part of the if-block is rewritten below.
226 double prevFlapHandle = 0.0f;
227 double flap_transit_rate;
228 bool flaps_in_transit = false;
230 if (old_flap_routine) {
232 // check for lowest flap setting
233 if (Flap_handle < dfArray[1]) {
234 Flap_handle = dfArray[1];
235 prevFlapHandle = Flap_handle;
237 } else if (Flap_handle > dfArray[ndf]) {
238 // check for highest flap setting
239 Flap_handle = dfArray[ndf];
240 prevFlapHandle = Flap_handle;
243 // otherwise in between
244 if(Flap_handle != prevFlapHandle)
245 flaps_in_transit = true;
246 if(flaps_in_transit) {
248 while (dfArray[iflap] < Flap_handle)
250 if (flap < Flap_handle) {
251 if (TimeArray[iflap] > 0)
252 flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / TimeArray[iflap+1];
254 flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / 5;
256 if (TimeArray[iflap+1] > 0)
257 flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / TimeArray[iflap+1];
259 flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / 5;
261 if(fabs (flap - Flap_handle) > dt * flap_transit_rate)
262 flap += flap_transit_rate * dt;
264 flaps_in_transit = false;
269 prevFlapHandle = Flap_handle;
272 // designed for the twin otter non-linear model
273 if (!outside_control) {
274 flap_percent = Flap_handle / 30.0; // percent of flaps desired
275 if (flap_percent>=0.31 && flap_percent<=0.35)
276 flap_percent = 1.0 / 3.0;
277 if (flap_percent>=0.65 && flap_percent<=0.69)
278 flap_percent = 2.0 / 3.0;
280 flap_cmd_deg = flap_percent * flap_max; // angle of flaps desired
281 flap_moving_rate = flap_rate * dt; // amount flaps move per time step
283 // determine flap position with respect to the flap goal
284 if (flap_pos < flap_cmd_deg) {
285 flap_pos += flap_moving_rate;
286 if (flap_pos > flap_cmd_deg)
287 flap_pos = flap_cmd_deg;
288 } else if (flap_pos > flap_cmd_deg) {
289 flap_pos -= flap_moving_rate;
290 if (flap_pos < flap_cmd_deg)
291 flap_pos = flap_cmd_deg;
297 if (!outside_control && use_flaps) {
298 // angle of flaps desired [rad]
299 flap_cmd = Flap_handle * DEG_TO_RAD;
300 // amount flaps move per time step [rad/sec]
301 flap_increment_per_timestep = flap_rate * dt * DEG_TO_RAD;
302 // determine flap position [rad] with respect to flap command
303 flap_pos = uiuc_find_position(flap_cmd,flap_increment_per_timestep,flap_pos);
304 // get the normalized position
305 flap_pos_norm = flap_pos/(flap_max * DEG_TO_RAD);
310 // angle of spoilers desired [rad]
311 spoiler_cmd = Spoiler_handle * DEG_TO_RAD;
312 // amount spoilers move per time step [rad/sec]
313 spoiler_increment_per_timestep = spoiler_rate * dt * DEG_TO_RAD;
314 // determine spoiler position [rad] with respect to spoiler command
315 spoiler_pos = uiuc_find_position(spoiler_cmd,spoiler_increment_per_timestep,spoiler_pos);
316 // get the normailized position
317 spoiler_pos_norm = spoiler_pos/(spoiler_max * DEG_TO_RAD);
324 // end uiuc_aerodeflections.cpp