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
34 ----------------------------------------------------------------------
36 AUTHOR(S): Jeff Scott http://www.jeffscott.net/
37 Robert Deters <rdeters@uiuc.edu>
38 Michael Selig <m-selig@uiuc.edu>
40 ----------------------------------------------------------------------
44 ----------------------------------------------------------------------
48 ----------------------------------------------------------------------
52 ----------------------------------------------------------------------
56 ----------------------------------------------------------------------
60 ----------------------------------------------------------------------
62 COPYRIGHT: (C) 2000 by Michael Selig
64 This program is free software; you can redistribute it and/or
65 modify it under the terms of the GNU General Public License
66 as published by the Free Software Foundation.
68 This program is distributed in the hope that it will be useful,
69 but WITHOUT ANY WARRANTY; without even the implied warranty of
70 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
71 GNU General Public License for more details.
73 You should have received a copy of the GNU General Public License
74 along with this program; if not, write to the Free Software
75 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
76 USA or view http://www.gnu.org/copyleft/gpl.html.
78 **********************************************************************/
82 #include "uiuc_aerodeflections.h"
84 void uiuc_aerodeflections( double dt )
88 static double elev_trim;
90 if (use_uiuc_network) {
99 Flap_handle = flap_percent * flap_max;
100 if (outside_control) {
101 pilot_elev_no = true;
104 pilot_throttle_no = true;
105 Long_trim = elev_trim;
109 if (zero_Long_trim) {
114 if (Lat_control <= 0)
115 aileron = - Lat_control * damin * DEG_TO_RAD;
117 aileron = - Lat_control * damax * DEG_TO_RAD;
120 if (Long_trim <= 0) {
121 elevator = Long_trim * demax * DEG_TO_RAD;
122 demax_remain = demax + Long_trim * demax;
123 demin_remain = -Long_trim * demax + demin;
124 if (Long_control <= 0)
125 elevator += Long_control * demax_remain * DEG_TO_RAD;
127 elevator += Long_control * demin_remain * DEG_TO_RAD;
129 elevator = Long_trim * demin * DEG_TO_RAD;
130 demin_remain = demin - Long_trim * demin;
131 demax_remain = Long_trim * demin + demax;
132 if (Long_control >=0)
133 elevator += Long_control * demin_remain * DEG_TO_RAD;
135 elevator += Long_control * demax_remain * DEG_TO_RAD;
138 if ((Long_control+Long_trim) <= 0)
139 elevator = (Long_control + Long_trim) * demax * DEG_TO_RAD;
141 elevator = (Long_control + Long_trim) * demin * DEG_TO_RAD;
144 if (Rudder_pedal <= 0)
145 rudder = -Rudder_pedal * drmin * DEG_TO_RAD;
147 rudder = -Rudder_pedal * drmax * DEG_TO_RAD;
149 /* at this point aileron, elevator, and rudder commands are known (in radians)
150 * add in SAS and any other mixing control laws
153 // SAS for pitch, positive elevator is TED
154 if (use_elevator_stick_gain)
155 elevator *= elevator_stick_gain;
156 if (use_elevator_sas_type1) {
157 elevator_sas = elevator_sas_KQ * Q_body;
158 if (use_elevator_sas_max && (elevator_sas > (elevator_sas_max * DEG_TO_RAD))) {
159 elevator += elevator_sas_max * DEG_TO_RAD;
160 //elevator_sas = elevator_sas_max;
161 } else if (use_elevator_sas_min && (elevator_sas < (elevator_sas_min * DEG_TO_RAD))) {
162 elevator += elevator_sas_min * DEG_TO_RAD;
163 //elevator_sas = elevator_sas_min;
166 elevator += elevator_sas;
167 // don't exceed the elevator deflection limits
168 if (elevator > demax * DEG_TO_RAD)
169 elevator = demax * DEG_TO_RAD;
170 else if (elevator < (-demin * DEG_TO_RAD))
171 elevator = -demin * DEG_TO_RAD;
174 // SAS for roll, positive aileron is right aileron TED
175 if (use_aileron_stick_gain)
176 aileron *= aileron_stick_gain;
177 if (use_aileron_sas_type1) {
178 aileron_sas = aileron_sas_KP * P_body;
179 if (use_aileron_sas_max && (fabs(aileron_sas) > (aileron_sas_max * DEG_TO_RAD)))
180 if (aileron_sas >= 0) {
181 aileron += aileron_sas_max * DEG_TO_RAD;
182 //aileron_sas = aileron_sas_max;
184 aileron += -aileron_sas_max * DEG_TO_RAD;
185 //aileron_sas = -aileron_sas;
188 aileron += aileron_sas;
189 // don't exceed aileron deflection limits
190 if (fabs(aileron) > (damax * DEG_TO_RAD))
192 aileron = damax * DEG_TO_RAD;
194 aileron = -damax * DEG_TO_RAD;
197 // SAS for yaw, positive rudder deflection is TEL
198 if (use_rudder_stick_gain)
199 rudder *= rudder_stick_gain;
200 if (use_rudder_sas_type1) {
201 rudder_sas = rudder_sas_KR * R_body;
202 if (use_rudder_sas_max && (fabs(rudder_sas) > (rudder_sas_max*DEG_TO_RAD)))
203 if (rudder_sas >= 0) {
204 rudder += rudder_sas_max * DEG_TO_RAD;
205 //rudder_sas = rudder_sas_max;
207 rudder += -rudder_sas_max * DEG_TO_RAD;
208 //rudder_sas = rudder_sas_max;
211 rudder += rudder_sas;
212 // don't exceed rudder deflection limits, assumes drmax = drmin,
213 // i.e. equal rudder throws left and right
214 if (fabs(rudder) > drmax)
216 rudder = drmax * DEG_TO_RAD;
218 rudder = -drmax * DEG_TO_RAD;
221 /* This old code in the first part of the if-block needs to get removed from FGFS. 030222 MSS
222 The second part of the if-block is rewritten below.
223 double prevFlapHandle = 0.0f;
224 double flap_transit_rate;
225 bool flaps_in_transit = false;
227 if (old_flap_routine) {
229 // check for lowest flap setting
230 if (Flap_handle < dfArray[1]) {
231 Flap_handle = dfArray[1];
232 prevFlapHandle = Flap_handle;
234 } else if (Flap_handle > dfArray[ndf]) {
235 // check for highest flap setting
236 Flap_handle = dfArray[ndf];
237 prevFlapHandle = Flap_handle;
240 // otherwise in between
241 if(Flap_handle != prevFlapHandle)
242 flaps_in_transit = true;
243 if(flaps_in_transit) {
245 while (dfArray[iflap] < Flap_handle)
247 if (flap < Flap_handle) {
248 if (TimeArray[iflap] > 0)
249 flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / TimeArray[iflap+1];
251 flap_transit_rate = (dfArray[iflap] - dfArray[iflap-1]) / 5;
253 if (TimeArray[iflap+1] > 0)
254 flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / TimeArray[iflap+1];
256 flap_transit_rate = (dfArray[iflap] - dfArray[iflap+1]) / 5;
258 if(fabs (flap - Flap_handle) > dt * flap_transit_rate)
259 flap += flap_transit_rate * dt;
261 flaps_in_transit = false;
266 prevFlapHandle = Flap_handle;
269 // designed for the twin otter non-linear model
270 if (!outside_control) {
271 flap_percent = Flap_handle / 30.0; // percent of flaps desired
272 if (flap_percent>=0.31 && flap_percent<=0.35)
273 flap_percent = 1.0 / 3.0;
274 if (flap_percent>=0.65 && flap_percent<=0.69)
275 flap_percent = 2.0 / 3.0;
277 flap_cmd_deg = flap_percent * flap_max; // angle of flaps desired
278 flap_moving_rate = flap_rate * dt; // amount flaps move per time step
280 // determine flap position with respect to the flap goal
281 if (flap_pos < flap_cmd_deg) {
282 flap_pos += flap_moving_rate;
283 if (flap_pos > flap_cmd_deg)
284 flap_pos = flap_cmd_deg;
285 } else 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;
294 if (!outside_control && use_flaps) {
295 // angle of flaps desired [rad]
296 flap_cmd = Flap_handle * DEG_TO_RAD;
297 // amount flaps move per time step [rad/sec]
298 flap_increment_per_timestep = flap_rate * dt * DEG_TO_RAD;
299 // determine flap position [rad] with respect to flap command
300 flap_pos = uiuc_find_position(flap_cmd,flap_increment_per_timestep,flap_pos);
301 // get the normalized position
302 flap_pos_pct = flap_pos/(flap_max * DEG_TO_RAD);
307 // angle of spoilers desired [deg]
308 spoiler_cmd_deg = Spoiler_handle;
309 // amount spoilers move per time step [deg]
310 spoiler_increment_per_timestep = spoiler_rate * dt;
311 // determine spoiler position with respect to spoiler command
312 if (spoiler_pos_deg < spoiler_cmd_deg) {
313 spoiler_pos_deg += spoiler_increment_per_timestep;
314 if (spoiler_pos_deg > spoiler_cmd_deg)
315 spoiler_pos_deg = spoiler_cmd_deg;
316 } else if (spoiler_pos_deg > spoiler_cmd_deg) {
317 spoiler_pos_deg -= spoiler_increment_per_timestep;
318 if (spoiler_pos_deg < spoiler_cmd_deg)
319 spoiler_pos_deg = spoiler_cmd_deg;
321 spoiler_pos = spoiler_pos_deg * DEG_TO_RAD;
328 // end uiuc_aerodeflections.cpp