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 **********************************************************************/
85 #include <simgear/compiler.h>
86 #include <simgear/misc/sg_path.hxx>
87 #include <Aircraft/aircraft.hxx>
88 #include <Main/fg_props.hxx>
90 #include "uiuc_aircraft.h"
91 #include "uiuc_coefficients.h"
92 #include "uiuc_getwind.h"
93 #include "uiuc_engine.h"
94 #include "uiuc_gear.h"
95 #include "uiuc_aerodeflections.h"
96 #include "uiuc_recorder.h"
97 #include "uiuc_menu.h"
98 #include "uiuc_betaprobe.h"
99 #include <FDM/LaRCsim/ls_generic.h>
100 //#include "Main/simple_udp.h"
101 #include "uiuc_fog.h" //321654
102 //#include "uiuc_network.h"
103 #include "uiuc_get_flapper.h"
108 extern "C" void uiuc_initial_init ();
109 extern "C" void uiuc_defaults_inits ();
110 extern "C" void uiuc_vel_init ();
111 extern "C" void uiuc_init_aeromodel ();
112 extern "C" void uiuc_force_moment(double dt);
113 extern "C" void uiuc_engine_routine();
114 extern "C" void uiuc_wind_routine();
115 extern "C" void uiuc_gear_routine();
116 extern "C" void uiuc_record_routine(double dt);
117 extern "C" void uiuc_network_recv_routine();
118 extern "C" void uiuc_network_send_routine();
120 AIRCRAFT *aircraft_ = new AIRCRAFT;
122 // SendArray testarray(4950);
124 /* Convert float to string */
125 //string ftoa(double in)
127 // static char temp[20];
128 // sprintf(temp,"%g",in);
129 // return (string)temp;
132 void uiuc_initial_init ()
134 // This function is called from uiuc_init_2_wrapper (uiuc_aero.c in LaRCsim)
135 // which is called from ls_step and ls_model.
136 // Apply brute force initializations, which override unwanted changes
137 // performed by LaRCsim.
138 // Used during initialization (while Simtime=0).
139 if (P_body_init_true)
140 P_body = P_body_init;
141 if (Q_body_init_true)
142 Q_body = Q_body_init;
143 if (R_body_init_true)
144 R_body = R_body_init;
153 if (U_body_init_true)
154 U_body = U_body_init;
155 if (V_body_init_true)
156 V_body = V_body_init;
157 if (W_body_init_true)
158 W_body = W_body_init;
161 void uiuc_defaults_inits ()
163 // set defaults and initialize (called once from uiuc_init_2_wrapper)
168 fog_point_index = -1;
172 fog_current_segment = 0;
175 use_V_rel_wind_2U = false;
176 nondim_rate_V_rel_wind = false;
177 use_abs_U_body_2U = false;
178 use_dyn_on_speed_curve1 = false;
179 use_Alpha_dot_on_speed = false;
180 use_gamma_horiz_on_speed = false;
181 b_downwashMode = false;
182 P_body_init_true = false;
183 Q_body_init_true = false;
184 R_body_init_true = false;
185 Phi_init_true = false;
186 Theta_init_true = false;
187 Psi_init_true = false;
188 Alpha_init_true = false;
189 Beta_init_true = false;
190 U_body_init_true = false;
191 V_body_init_true = false;
192 W_body_init_true = false;
194 use_uiuc_network = false;
196 outside_control = false;
197 set_Long_trim = false;
198 zero_Long_trim = false;
199 elevator_step = false;
200 elevator_singlet = false;
201 elevator_doublet = false;
202 elevator_input = false;
203 elevator_input_ntime = 0;
204 aileron_input = false;
205 aileron_input_ntime = 0;
206 rudder_input = false;
207 rudder_input_ntime = 0;
208 pilot_elev_no = false;
209 pilot_elev_no_check = false;
210 pilot_ail_no = false;
211 pilot_ail_no_check = false;
212 pilot_rud_no = false;
213 pilot_rud_no_check = false;
215 use_spoilers = false;
216 flap_pos_input = false;
217 flap_pos_input_ntime = 0;
218 use_elevator_sas_type1 = false;
219 use_elevator_sas_max = false;
220 use_elevator_sas_min = false;
221 use_elevator_stick_gain = false;
222 use_aileron_sas_type1 = false;
223 use_aileron_sas_max = false;
224 use_aileron_stick_gain = false;
225 use_rudder_sas_type1 = false;
226 use_rudder_sas_max = false;
227 use_rudder_stick_gain = false;
228 simpleSingleModel = false;
229 Throttle_pct_input = false;
230 Throttle_pct_input_ntime = 0;
231 gyroForce_Q_body = false;
232 gyroForce_R_body = false;
233 b_slipstreamEffects = false;
241 // gear_model[MAX_GEAR]
242 memset(gear_model,false,MAX_GEAR*sizeof(gear_model[0]));
244 // start with gear down if it is ultimately used
250 memset(bootTrue,false,20*sizeof(gear_model[0]));
251 eta_from_file = false;
252 eta_wing_left_input = false;
253 eta_wing_right_input = false;
254 eta_tail_input = false;
255 demo_eps_alpha_max = false;
256 demo_eps_pitch_max = false;
257 demo_eps_pitch_min = false;
258 demo_eps_roll_max = false;
259 demo_eps_thrust_min = false;
260 demo_eps_airspeed_max = false;
261 demo_eps_airspeed_min = false;
262 demo_eps_flap_max = false;
263 demo_boot_cycle_tail = false;
264 demo_boot_cycle_wing_left = false;
265 demo_boot_cycle_wing_right = false;
266 demo_eps_pitch_input = false;
267 tactilefadef = false;
268 demo_ap_pah_on = false;
269 demo_ap_alh_on = false;
270 demo_ap_Theta_ref = false;
271 demo_ap_alt_ref = false;
272 demo_tactile = false;
273 demo_ice_tail = false;
274 demo_ice_left = false;
275 demo_ice_right = false;
276 flapper_model = false;
277 ignore_unknown_keywords = false;
278 pilot_throttle_no = false;
287 void uiuc_vel_init ()
289 // Calculates the local velocity (V_north, V_east, V_down) from the body
291 // Called from uiuc_local_vel_init which is called from ls_step.
292 // Used during initialization (while Simtime=0)
293 if (U_body_init_true && V_body_init_true && W_body_init_true)
295 double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
297 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);
298 cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
299 cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
300 cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
301 cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
302 cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
303 cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
304 cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
305 cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
306 cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
308 V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
309 V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
310 V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
312 V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
316 void uiuc_init_aeromodel ()
318 // Initializes the UIUC aircraft model.
319 // Called once from uiuc_init_2_wrapper
320 SGPath path(globals->get_fg_root());
321 path.append(fgGetString("/sim/aircraft-dir"));
322 path.append("aircraft.dat");
323 cout << "We are using "<< path.str() << endl;
324 uiuc_initializemaps(); // Initialize the <string,int> maps
325 uiuc_menu(path.str()); // Read the specified aircraft file
328 void uiuc_force_moment(double dt)
330 double qS = Dynamic_pressure * Sw;
331 double qScbar = qS * cbar;
332 double qSb = qS * bw;
334 uiuc_aerodeflections(dt);
335 uiuc_coefficients(dt);
337 /* Calculate the forces */
346 // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
347 // hence Cos_beta * Cos_beta term included.
348 // Same thing is done w/ moments below.
349 // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
350 // that should not be the case. See FGFS notes 021105
351 F_X_wind = -CD * qS * Cos_beta * Cos_beta;
353 F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
354 // F_X_wind = -CD * qS * Cos_beta * Cos_beta;
355 // F_Y_wind = CY * qS * Cos_beta * Cos_beta;
356 // F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
358 // wind-axis to body-axis transformation
359 F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
360 F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
361 F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
363 // Moment calculations
364 M_l_aero = Cl * qSb ;
365 M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
366 M_n_aero = Cn * qSb ;
367 // M_l_aero = Cl * qSb * Cos_beta * Cos_beta;
368 // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
369 // M_n_aero = Cn * qSb * Cos_beta * Cos_beta;
371 // Adding in apparent mass effects
372 if (Mass_appMass_ratio)
373 F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
374 if (I_xx_appMass_ratio)
375 M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
376 if (I_yy_appMass_ratio)
377 M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
378 if (I_zz_appMass_ratio)
379 M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
381 // adding in apparent mass in body axis X direction
382 // F_X_aero += -(0.05 * Mass) * U_dot_body;
386 F_Z_aero += -Mass_appMass * W_dot_body;
388 M_l_aero += -I_xx_appMass * P_dot_body;
390 M_m_aero += -I_yy_appMass * Q_dot_body;
392 M_n_aero += -I_zz_appMass * R_dot_body;
394 // gyroscopic moments
395 // engineOmega is positive when rotation is ccw when viewed from the front
396 if (gyroForce_Q_body)
397 M_n_aero += polarInertia * engineOmega * Q_body;
398 if (gyroForce_R_body)
399 M_m_aero += -polarInertia * engineOmega * R_body;
401 // ornithopter support
404 uiuc_get_flapper(dt);
405 F_X_aero += F_X_aero_flapper;
406 F_Z_aero += F_Z_aero_flapper;
407 M_m_aero += flapper_Moment;
418 vis = fgGetDouble("/environment/visibility-m");
423 fgSetDouble("/environment/visibility-m", vis);
427 /* Send data on the network to the Glass Cockpit */
431 // input += " stick_right " + ftoa(Lat_control);
432 // input += " rudder_left " + ftoa(-Rudder_pedal);
433 // input += " stick_forward " + ftoa(Long_control);
434 // input += " stick_trim_forward " + ftoa(Long_trim);
435 // input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
436 // input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
437 // input += " vehicle_speed " + ftoa(V_rel_wind);
438 // input += " throttle_forward " + ftoa(Throttle_pct);
439 // input += " altitude " + ftoa(Altitude);
440 // input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
442 // testarray.getHello();
443 // testarray.sendData(input);
445 /* End of Networking */
449 void uiuc_wind_routine()
454 void uiuc_engine_routine()
459 void uiuc_gear_routine ()
464 void uiuc_record_routine(double dt)
466 if (trigger_last_time_step == 0 && trigger_on == 1) {
467 if (trigger_toggle == 0)
472 if (trigger_num % 2 != 0)
476 if (Simtime >= recordStartTime)
479 trigger_last_time_step = trigger_on;
482 void uiuc_network_recv_routine()
484 //if (use_uiuc_network)
488 void uiuc_network_send_routine()
490 //if (use_uiuc_network)
493 //end uiuc_wrapper.cpp