1 /**********************************************************************
3 FILENAME: uiuc_wrapper.cpp
5 ----------------------------------------------------------------------
7 DESCRIPTION: A wrapper(interface) between the UIUC Aeromodel (C++ files)
8 and the LaRCsim FDM (C files)
10 ----------------------------------------------------------------------
14 ----------------------------------------------------------------------
18 ----------------------------------------------------------------------
20 HISTORY: 01/26/2000 initial release
21 03/09/2001 (DPM) added support for gear
22 06/18/2001 (RD) Made uiuc_recorder its own routine.
23 07/19/2001 (RD) Added uiuc_vel_init() to initialize
25 08/27/2001 (RD) Added uiuc_initial_init() to help
26 in starting an A/C at an initial condition
27 02/24/2002 (GD) Added uiuc_network_routine()
28 03/27/2002 (RD) Changed how forces are calculated when
30 12/11/2002 (RD) Divided uiuc_network_routine into
31 uiuc_network_recv_routine and
32 uiuc_network_send_routine
33 03/16/2003 (RD) Added trigger lines in recorder area
35 ----------------------------------------------------------------------
37 AUTHOR(S): Bipin Sehgal <bsehgal@uiuc.edu>
38 Robert Deters <rdeters@uiuc.edu>
39 Glen Dimock <dimock@uiuc.edu>
40 David Megginson <david@megginson.com>
42 ----------------------------------------------------------------------
46 ----------------------------------------------------------------------
50 ----------------------------------------------------------------------
54 ----------------------------------------------------------------------
58 ----------------------------------------------------------------------
62 ----------------------------------------------------------------------
64 COPYRIGHT: (C) 2000 by Michael Selig
66 This program is free software; you can redistribute it and/or
67 modify it under the terms of the GNU General Public License
68 as published by the Free Software Foundation.
70 This program is distributed in the hope that it will be useful,
71 but WITHOUT ANY WARRANTY; without even the implied warranty of
72 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
73 GNU General Public License for more details.
75 You should have received a copy of the GNU General Public License
76 along with this program; if not, write to the Free Software
77 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
78 USA or view http://www.gnu.org/copyleft/gpl.html.
80 **********************************************************************/
86 #include <simgear/compiler.h>
87 #include <simgear/misc/sg_path.hxx>
88 #include <Aircraft/aircraft.hxx>
89 #include <Main/fg_props.hxx>
91 #include "uiuc_aircraft.h"
92 #include "uiuc_aircraftdir.h"
93 #include "uiuc_coefficients.h"
94 #include "uiuc_getwind.h"
95 #include "uiuc_engine.h"
96 #include "uiuc_gear.h"
97 #include "uiuc_aerodeflections.h"
98 #include "uiuc_recorder.h"
99 #include "uiuc_menu.h"
100 #include "uiuc_betaprobe.h"
101 #include <FDM/LaRCsim/ls_generic.h>
102 //#include "Main/simple_udp.h"
103 #include "uiuc_fog.h" //321654
104 //#include "uiuc_network.h"
105 #include "uiuc_get_flapper.h"
110 extern "C" void uiuc_initial_init ();
111 extern "C" void uiuc_defaults_inits ();
112 extern "C" void uiuc_vel_init ();
113 extern "C" void uiuc_init_aeromodel ();
114 extern "C" void uiuc_force_moment(double dt);
115 extern "C" void uiuc_engine_routine();
116 extern "C" void uiuc_wind_routine();
117 extern "C" void uiuc_gear_routine();
118 extern "C" void uiuc_record_routine(double dt);
119 extern "C" void uiuc_network_recv_routine();
120 extern "C" void uiuc_network_send_routine();
122 AIRCRAFT *aircraft_ = new AIRCRAFT;
123 AIRCRAFTDIR *aircraftdir_ = new AIRCRAFTDIR;
125 // SendArray testarray(4950);
127 /* Convert float to string */
128 //string ftoa(double in)
130 // static char temp[20];
131 // sprintf(temp,"%g",in);
132 // return (string)temp;
135 void uiuc_initial_init ()
137 // This function is called from uiuc_init_2_wrapper (uiuc_aero.c in LaRCsim)
138 // which is called from ls_step and ls_model.
139 // Apply brute force initializations, which override unwanted changes
140 // performed by LaRCsim.
141 // Used during initialization (while Simtime=0).
142 if (P_body_init_true)
143 P_body = P_body_init;
144 if (Q_body_init_true)
145 Q_body = Q_body_init;
146 if (R_body_init_true)
147 R_body = R_body_init;
156 if (U_body_init_true)
157 U_body = U_body_init;
158 if (V_body_init_true)
159 V_body = V_body_init;
160 if (W_body_init_true)
161 W_body = W_body_init;
164 void uiuc_defaults_inits ()
166 // set defaults and initialize (called once from uiuc_init_2_wrapper)
171 fog_point_index = -1;
175 fog_current_segment = 0;
178 use_V_rel_wind_2U = false;
179 nondim_rate_V_rel_wind = false;
180 use_abs_U_body_2U = false;
181 use_dyn_on_speed_curve1 = false;
182 use_Alpha_dot_on_speed = false;
183 use_gamma_horiz_on_speed = false;
184 b_downwashMode = false;
185 P_body_init_true = false;
186 Q_body_init_true = false;
187 R_body_init_true = false;
188 Phi_init_true = false;
189 Theta_init_true = false;
190 Psi_init_true = false;
191 Alpha_init_true = false;
192 Beta_init_true = false;
193 U_body_init_true = false;
194 V_body_init_true = false;
195 W_body_init_true = false;
197 use_uiuc_network = false;
198 old_flap_routine = false;
200 outside_control = false;
201 set_Long_trim = false;
202 zero_Long_trim = false;
203 elevator_step = false;
204 elevator_singlet = false;
205 elevator_doublet = false;
206 elevator_input = false;
207 elevator_input_ntime = 0;
208 aileron_input = false;
209 aileron_input_ntime = 0;
210 rudder_input = false;
211 rudder_input_ntime = 0;
212 pilot_elev_no = false;
213 pilot_elev_no_check = false;
214 pilot_ail_no = false;
215 pilot_ail_no_check = false;
216 pilot_rud_no = false;
217 pilot_rud_no_check = false;
219 use_spoilers = false;
220 flap_pos_input = false;
221 flap_pos_input_ntime = 0;
222 use_elevator_sas_type1 = false;
223 use_elevator_sas_max = false;
224 use_elevator_sas_min = false;
225 use_elevator_stick_gain = false;
226 use_aileron_sas_type1 = false;
227 use_aileron_sas_max = false;
228 use_aileron_stick_gain = false;
229 use_rudder_sas_type1 = false;
230 use_rudder_sas_max = false;
231 use_rudder_stick_gain = false;
232 simpleSingleModel = false;
233 Throttle_pct_input = false;
234 Throttle_pct_input_ntime = 0;
235 gyroForce_Q_body = false;
236 gyroForce_R_body = false;
237 b_slipstreamEffects = false;
245 // gear_model[MAX_GEAR]
246 memset(gear_model,false,MAX_GEAR*sizeof(gear_model[0]));
248 // start with gear down if it is ultimately used
254 memset(bootTrue,false,20*sizeof(gear_model[0]));
255 eta_from_file = false;
256 eta_wing_left_input = false;
257 eta_wing_right_input = false;
258 eta_tail_input = false;
259 demo_eps_alpha_max = false;
260 demo_eps_pitch_max = false;
261 demo_eps_pitch_min = false;
262 demo_eps_roll_max = false;
263 demo_eps_thrust_min = false;
264 demo_eps_airspeed_max = false;
265 demo_eps_airspeed_min = false;
266 demo_eps_flap_max = false;
267 demo_boot_cycle_tail = false;
268 demo_boot_cycle_wing_left = false;
269 demo_boot_cycle_wing_right = false;
270 demo_eps_pitch_input = false;
271 tactilefadef = false;
272 demo_ap_pah_on = false;
273 demo_ap_Theta_ref_deg = false;
274 demo_tactile = false;
275 demo_ice_tail = false;
276 demo_ice_left = false;
277 demo_ice_right = false;
278 flapper_model = false;
279 ignore_unknown_keywords = false;
280 pilot_throttle_no = false;
286 // Calculates the local velocity (V_north, V_east, V_down) from the body
288 // Called from uiuc_local_vel_init which is called from ls_step.
289 // Used during initialization (while Simtime=0)
292 void uiuc_vel_init ()
294 if (U_body_init_true && V_body_init_true && W_body_init_true)
296 double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
298 det_T_l_to_b = T_local_to_body_11*(T_local_to_body_22*T_local_to_body_33-T_local_to_body_23*T_local_to_body_32) - T_local_to_body_12*(T_local_to_body_21*T_local_to_body_33-T_local_to_body_23*T_local_to_body_31) + T_local_to_body_13*(T_local_to_body_21*T_local_to_body_32-T_local_to_body_22*T_local_to_body_31);
299 cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
300 cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
301 cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
302 cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
303 cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
304 cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
305 cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
306 cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
307 cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
309 V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
310 V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
311 V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
313 V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
315 // Initializes the UIUC aircraft model.
316 // Called once from uiuc_init_2_wrapper
319 void uiuc_init_aeromodel ()
321 SGPath path(globals->get_fg_root());
322 path.append(aircraft_dir);
323 path.append("aircraft.dat");
324 cout << "We are using "<< path.str() << endl;
325 uiuc_initializemaps(); // Initialize the <string,int> maps
326 uiuc_menu(path.str()); // Read the specified aircraft file
329 void uiuc_force_moment(double dt)
331 double qS = Dynamic_pressure * Sw;
332 double qScbar = qS * cbar;
333 double qSb = qS * bw;
335 uiuc_aerodeflections(dt);
336 uiuc_coefficients(dt);
338 /* Calculate the forces */
347 // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
348 // hence Cos_beta * Cos_beta term included.
349 // Same thing is done w/ moments below.
350 // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
351 // that should not be the case. See FGFS notes 021105
352 F_X_wind = -CD * qS * Cos_beta * Cos_beta;
354 F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
355 // F_X_wind = -CD * qS * Cos_beta * Cos_beta;
356 // F_Y_wind = CY * qS * Cos_beta * Cos_beta;
357 // F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
359 // wind-axis to body-axis transformation
360 F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
361 F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
362 F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
364 // Moment calculations
365 M_l_aero = Cl * qSb ;
366 M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
367 M_n_aero = Cn * qSb ;
368 // M_l_aero = Cl * qSb * Cos_beta * Cos_beta;
369 // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
370 // M_n_aero = Cn * qSb * Cos_beta * Cos_beta;
372 // Adding in apparent mass effects
373 if (Mass_appMass_ratio)
374 F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
375 if (I_xx_appMass_ratio)
376 M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
377 if (I_yy_appMass_ratio)
378 M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
379 if (I_zz_appMass_ratio)
380 M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
382 // adding in apparent mass in body axis X direction
383 // F_X_aero += -(0.05 * Mass) * U_dot_body;
387 F_Z_aero += -Mass_appMass * W_dot_body;
389 M_l_aero += -I_xx_appMass * P_dot_body;
391 M_m_aero += -I_yy_appMass * Q_dot_body;
393 M_n_aero += -I_zz_appMass * R_dot_body;
395 // gyroscopic moments
396 // engineOmega is positive when rotation is ccw when viewed from the front
397 if (gyroForce_Q_body)
398 M_n_aero += polarInertia * engineOmega * Q_body;
399 if (gyroForce_R_body)
400 M_m_aero += -polarInertia * engineOmega * R_body;
402 // ornithopter support
405 uiuc_get_flapper(dt);
406 F_X_aero += F_X_aero_flapper;
407 F_Z_aero += F_Z_aero_flapper;
408 M_m_aero += flapper_Moment;
419 vis = fgGetDouble("/environment/visibility-m");
424 fgSetDouble("/environment/visibility-m", vis);
428 /* Send data on the network to the Glass Cockpit */
432 // input += " stick_right " + ftoa(Lat_control);
433 // input += " rudder_left " + ftoa(-Rudder_pedal);
434 // input += " stick_forward " + ftoa(Long_control);
435 // input += " stick_trim_forward " + ftoa(Long_trim);
436 // input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
437 // input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
438 // input += " vehicle_speed " + ftoa(V_rel_wind);
439 // input += " throttle_forward " + ftoa(Throttle_pct);
440 // input += " altitude " + ftoa(Altitude);
441 // input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
443 // testarray.getHello();
444 // testarray.sendData(input);
446 /* End of Networking */
450 void uiuc_wind_routine()
455 void uiuc_engine_routine()
460 void uiuc_gear_routine ()
465 void uiuc_record_routine(double dt)
467 if (trigger_last_time_step == 0 && trigger_on == 1) {
468 if (trigger_toggle == 0)
473 if (trigger_num % 2 != 0)
477 if (Simtime >= recordStartTime)
480 trigger_last_time_step = trigger_on;
483 void uiuc_network_recv_routine()
485 //if (use_uiuc_network)
489 void uiuc_network_send_routine()
491 //if (use_uiuc_network)
494 //end uiuc_wrapper.cpp