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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
79 **********************************************************************/
88 #include <simgear/compiler.h>
89 #include <simgear/misc/sg_path.hxx>
90 #include <Main/fg_props.hxx>
92 #include "uiuc_aircraft.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"
107 extern "C" void uiuc_initial_init ();
108 extern "C" void uiuc_defaults_inits ();
109 extern "C" void uiuc_vel_init ();
110 extern "C" void uiuc_init_aeromodel ();
111 extern "C" void uiuc_force_moment(double dt);
112 extern "C" void uiuc_engine_routine();
113 extern "C" void uiuc_wind_routine();
114 extern "C" void uiuc_gear_routine();
115 extern "C" void uiuc_record_routine(double dt);
116 extern "C" void uiuc_network_recv_routine();
117 extern "C" void uiuc_network_send_routine();
119 AIRCRAFT *aircraft_ = 0;
121 // SendArray testarray(4950);
123 /* Convert float to string */
124 //string ftoa(double in)
126 // static char temp[20];
127 // sprintf(temp,"%g",in);
128 // return (string)temp;
131 void uiuc_initial_init ()
133 // This function is called from uiuc_init_2_wrapper (uiuc_aero.c in LaRCsim)
134 // which is called from ls_step and ls_model.
135 // Apply brute force initializations, which override unwanted changes
136 // performed by LaRCsim.
137 // Used during initialization (while Simtime=0).
138 if (P_body_init_true)
139 P_body = P_body_init;
140 if (Q_body_init_true)
141 Q_body = Q_body_init;
142 if (R_body_init_true)
143 R_body = R_body_init;
152 if (U_body_init_true)
153 U_body = U_body_init;
154 if (V_body_init_true)
155 V_body = V_body_init;
156 if (W_body_init_true)
157 W_body = W_body_init;
160 void uiuc_defaults_inits ()
163 aircraft_ = new AIRCRAFT;
165 // set defaults and initialize (called once from uiuc_init_2_wrapper)
170 fog_point_index = -1;
174 fog_current_segment = 0;
177 use_V_rel_wind_2U = false;
178 nondim_rate_V_rel_wind = false;
179 use_abs_U_body_2U = false;
180 use_dyn_on_speed_curve1 = false;
181 use_Alpha_dot_on_speed = false;
182 use_gamma_horiz_on_speed = false;
183 b_downwashMode = false;
184 P_body_init_true = false;
185 Q_body_init_true = false;
186 R_body_init_true = false;
187 Phi_init_true = false;
188 Theta_init_true = false;
189 Psi_init_true = false;
190 Alpha_init_true = false;
191 Beta_init_true = false;
192 U_body_init_true = false;
193 V_body_init_true = false;
194 W_body_init_true = false;
196 use_uiuc_network = false;
198 outside_control = false;
199 set_Long_trim = false;
200 zero_Long_trim = false;
201 elevator_step = false;
202 elevator_singlet = false;
203 elevator_doublet = false;
204 elevator_input = false;
205 elevator_input_ntime = 0;
206 aileron_input = false;
207 aileron_input_ntime = 0;
208 rudder_input = false;
209 rudder_input_ntime = 0;
210 pilot_elev_no = false;
211 pilot_elev_no_check = false;
212 pilot_ail_no = false;
213 pilot_ail_no_check = false;
214 pilot_rud_no = false;
215 pilot_rud_no_check = false;
217 use_spoilers = false;
218 flap_pos_input = false;
219 flap_pos_input_ntime = 0;
220 use_elevator_sas_type1 = false;
221 use_elevator_sas_max = false;
222 use_elevator_sas_min = false;
223 use_elevator_stick_gain = false;
224 use_aileron_sas_type1 = false;
225 use_aileron_sas_max = false;
226 use_aileron_stick_gain = false;
227 use_rudder_sas_type1 = false;
228 use_rudder_sas_max = false;
229 use_rudder_stick_gain = false;
230 simpleSingleModel = false;
231 Throttle_pct_input = false;
232 Throttle_pct_input_ntime = 0;
233 gyroForce_Q_body = false;
234 gyroForce_R_body = false;
235 b_slipstreamEffects = false;
243 // gear_model[MAX_GEAR]
244 memset(gear_model,false,MAX_GEAR*sizeof(gear_model[0]));
246 // start with gear down if it is ultimately used
252 memset(bootTrue,false,20*sizeof(gear_model[0]));
253 eta_from_file = false;
254 eta_wing_left_input = false;
255 eta_wing_right_input = false;
256 eta_tail_input = false;
257 demo_eps_alpha_max = false;
258 demo_eps_pitch_max = false;
259 demo_eps_pitch_min = false;
260 demo_eps_roll_max = false;
261 demo_eps_thrust_min = false;
262 demo_eps_airspeed_max = false;
263 demo_eps_airspeed_min = false;
264 demo_eps_flap_max = false;
265 demo_boot_cycle_tail = false;
266 demo_boot_cycle_wing_left = false;
267 demo_boot_cycle_wing_right = false;
268 demo_eps_pitch_input = false;
269 tactilefadef = false;
270 demo_ap_pah_on = false;
271 demo_ap_alh_on = false;
272 demo_ap_Theta_ref = false;
273 demo_ap_alt_ref = 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;
289 void uiuc_vel_init ()
291 // Calculates the local velocity (V_north, V_east, V_down) from the body
293 // Called from uiuc_local_vel_init which is called from ls_step.
294 // Used during initialization (while Simtime=0)
295 if (U_body_init_true && V_body_init_true && W_body_init_true)
297 double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
299 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);
300 cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
301 cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
302 cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
303 cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
304 cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
305 cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
306 cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
307 cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
308 cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
310 V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
311 V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
312 V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
314 V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
318 void uiuc_init_aeromodel ()
320 // Initializes the UIUC aircraft model.
321 // Called once from uiuc_init_2_wrapper
322 SGPath path(globals->get_fg_root());
323 path.append(fgGetString("/sim/aircraft-dir"));
324 path.append("aircraft.dat");
325 std::cout << "We are using "<< path.str() << std::endl;
326 uiuc_initializemaps(); // Initialize the <string,int> maps
327 uiuc_menu(path.str()); // Read the specified aircraft file
330 void uiuc_force_moment(double dt)
332 double qS = Dynamic_pressure * Sw;
333 double qScbar = qS * cbar;
334 double qSb = qS * bw;
336 uiuc_aerodeflections(dt);
337 uiuc_coefficients(dt);
339 /* Calculate the forces */
348 // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
349 // hence Cos_beta * Cos_beta term included.
350 // Same thing is done w/ moments below.
351 // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
352 // that should not be the case. See FGFS notes 021105
353 F_X_wind = -CD * qS * Cos_beta * Cos_beta;
355 F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
356 // F_X_wind = -CD * qS * Cos_beta * Cos_beta;
357 // F_Y_wind = CY * qS * Cos_beta * Cos_beta;
358 // F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
360 // wind-axis to body-axis transformation
361 F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
362 F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
363 F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
365 // Moment calculations
366 M_l_aero = Cl * qSb ;
367 M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
368 M_n_aero = Cn * qSb ;
369 // M_l_aero = Cl * qSb * Cos_beta * Cos_beta;
370 // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
371 // M_n_aero = Cn * qSb * Cos_beta * Cos_beta;
373 // Adding in apparent mass effects
374 if (Mass_appMass_ratio)
375 F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
376 if (I_xx_appMass_ratio)
377 M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
378 if (I_yy_appMass_ratio)
379 M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
380 if (I_zz_appMass_ratio)
381 M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
383 // adding in apparent mass in body axis X direction
384 // F_X_aero += -(0.05 * Mass) * U_dot_body;
388 F_Z_aero += -Mass_appMass * W_dot_body;
390 M_l_aero += -I_xx_appMass * P_dot_body;
392 M_m_aero += -I_yy_appMass * Q_dot_body;
394 M_n_aero += -I_zz_appMass * R_dot_body;
396 // gyroscopic moments
397 // engineOmega is positive when rotation is ccw when viewed from the front
398 if (gyroForce_Q_body)
399 M_n_aero += polarInertia * engineOmega * Q_body;
400 if (gyroForce_R_body)
401 M_m_aero += -polarInertia * engineOmega * R_body;
403 // ornithopter support
406 uiuc_get_flapper(dt);
407 F_X_aero += F_X_aero_flapper;
408 F_Z_aero += F_Z_aero_flapper;
409 M_m_aero += flapper_Moment;
420 vis = fgGetDouble("/environment/visibility-m");
425 fgSetDouble("/environment/visibility-m", vis);
429 /* Send data on the network to the Glass Cockpit */
433 // input += " stick_right " + ftoa(Lat_control);
434 // input += " rudder_left " + ftoa(-Rudder_pedal);
435 // input += " stick_forward " + ftoa(Long_control);
436 // input += " stick_trim_forward " + ftoa(Long_trim);
437 // input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
438 // input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
439 // input += " vehicle_speed " + ftoa(V_rel_wind);
440 // input += " throttle_forward " + ftoa(Throttle_pct);
441 // input += " altitude " + ftoa(Altitude);
442 // input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
444 // testarray.getHello();
445 // testarray.sendData(input);
447 /* End of Networking */
451 void uiuc_wind_routine()
456 void uiuc_engine_routine()
461 void uiuc_gear_routine ()
466 void uiuc_record_routine(double dt)
468 if (trigger_last_time_step == 0 && trigger_on == 1) {
469 if (trigger_toggle == 0)
474 if (trigger_num % 2 != 0)
478 if (Simtime >= recordStartTime)
481 trigger_last_time_step = trigger_on;
484 void uiuc_network_recv_routine()
486 //if (use_uiuc_network)
490 void uiuc_network_send_routine()
492 //if (use_uiuc_network)
495 //end uiuc_wrapper.cpp