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 called from both ls_step and ls_model(uiuc model side).
138 // Apply brute force initializations, which override ls_step and ls_aux values
139 // for the first time step.
140 if (P_body_init_true)
141 P_body = P_body_init;
142 if (Q_body_init_true)
143 Q_body = Q_body_init;
144 if (R_body_init_true)
145 R_body = R_body_init;
154 if (U_body_init_true)
155 U_body = U_body_init;
156 if (V_body_init_true)
157 V_body = V_body_init;
158 if (W_body_init_true)
159 W_body = W_body_init;
162 void uiuc_defaults_inits ()
164 // set defaults and initialize (called from ls_step.c at Simtime=0)
169 fog_point_index = -1;
173 fog_current_segment = 0;
176 use_V_rel_wind_2U = 0;
177 nondim_rate_V_rel_wind = 0;
178 use_abs_U_body_2U = 0;
179 use_dyn_on_speed_curve1 = 0;
180 use_Alpha_dot_on_speed = 0;
181 use_gamma_horiz_on_speed = 0;
183 P_body_init_true = 0;
184 Q_body_init_true = 0;
185 R_body_init_true = 0;
191 U_body_init_true = 0;
192 V_body_init_true = 0;
193 W_body_init_true = 0;
195 use_uiuc_network = 0;
196 old_flap_routine = 0;
202 elevator_singlet = 0;
203 elevator_doublet = 0;
208 pilot_elev_no_check = 0;
210 pilot_ail_no_check = 0;
212 pilot_rud_no_check = 0;
216 use_elevator_sas_type1 = 0;
217 use_elevator_sas_max = 0;
218 use_elevator_sas_min = 0;
219 use_elevator_stick_gain = 0;
220 use_aileron_sas_type1 = 0;
221 use_aileron_sas_max = 0;
222 use_aileron_stick_gain = 0;
223 use_rudder_sas_type1 = 0;
224 use_rudder_sas_max = 0;
225 use_rudder_stick_gain = 0;
226 simpleSingleModel = 0;
227 Throttle_pct_input = 0;
228 gyroForce_Q_body = 0;
229 gyroForce_R_body = 0;
230 b_slipstreamEffects = 0;
235 // gear_model[MAX_GEAR] = 0;
242 eta_wing_left_input = 0;
243 eta_wing_right_input = 0;
245 demo_eps_alpha_max = 0;
246 demo_eps_pitch_max = 0;
247 demo_eps_pitch_min = 0;
248 demo_eps_roll_max = 0;
249 demo_eps_thrust_min = 0;
250 demo_eps_airspeed_max = 0;
251 demo_eps_airspeed_min = 0;
252 demo_eps_flap_max = 0;
253 demo_boot_cycle_tail = 0;
254 demo_boot_cycle_wing_left = 0;
255 demo_boot_cycle_wing_right = 0;
256 demo_eps_pitch_input = 0;
259 demo_ap_Theta_ref_deg = 0;
265 ignore_unknown_keywords = 0;
266 pilot_throttle_no = 0;
270 void uiuc_vel_init ()
272 if (U_body_init_true && V_body_init_true && W_body_init_true)
274 double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
276 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);
277 cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
278 cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
279 cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
280 cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
281 cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
282 cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
283 cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
284 cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
285 cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
287 V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
288 V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
289 V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
291 V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
295 void uiuc_init_aeromodel ()
297 SGPath path(globals->get_fg_root());
298 path.append(aircraft_dir);
299 path.append("aircraft.dat");
300 cout << "We are using "<< path.str() << endl;
301 uiuc_initializemaps(); // Initialize the <string,int> maps
302 uiuc_menu(path.str()); // Read the specified aircraft file
305 void uiuc_force_moment(double dt)
307 double qS = Dynamic_pressure * Sw;
308 double qScbar = qS * cbar;
309 double qSb = qS * bw;
311 uiuc_aerodeflections(dt);
312 uiuc_coefficients(dt);
314 /* Calculate the forces */
323 // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
324 // hence Cos_beta * Cos_beta term included.
325 // Same thing is done w/ moments below.
326 // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
327 // that should not be the case. See FGFS notes 021105
328 F_X_wind = -CD * qS * Cos_beta * Cos_beta;
330 F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
331 // F_X_wind = -CD * qS * Cos_beta * Cos_beta;
332 // F_Y_wind = CY * qS * Cos_beta * Cos_beta;
333 // F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
335 // wind-axis to body-axis transformation
336 F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
337 F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
338 F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
340 // Moment calculations
341 M_l_aero = Cl * qSb ;
342 M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
343 M_n_aero = Cn * qSb ;
344 // M_l_aero = Cl * qSb * Cos_beta * Cos_beta;
345 // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
346 // M_n_aero = Cn * qSb * Cos_beta * Cos_beta;
348 // Adding in apparent mass effects
349 if (Mass_appMass_ratio)
350 F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
351 if (I_xx_appMass_ratio)
352 M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
353 if (I_yy_appMass_ratio)
354 M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
355 if (I_zz_appMass_ratio)
356 M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
359 F_Z_aero += -Mass_appMass * W_dot_body;
361 M_l_aero += -I_xx_appMass * P_dot_body;
363 M_m_aero += -I_yy_appMass * Q_dot_body;
365 M_n_aero += -I_zz_appMass * R_dot_body;
367 // gyroscopic moments
368 // engineOmega is positive when rotation is ccw when viewed from the front
369 if (gyroForce_Q_body)
370 M_n_aero += polarInertia * engineOmega * Q_body;
371 if (gyroForce_R_body)
372 M_m_aero += -polarInertia * engineOmega * R_body;
374 // ornithopter support
377 uiuc_get_flapper(dt);
378 F_X_aero += F_X_aero_flapper;
379 F_Z_aero += F_Z_aero_flapper;
380 M_m_aero += flapper_Moment;
391 vis = fgGetDouble("/environment/visibility-m");
396 fgSetDouble("/environment/visibility-m", vis);
400 /* Send data on the network to the Glass Cockpit */
404 // input += " stick_right " + ftoa(Lat_control);
405 // input += " rudder_left " + ftoa(-Rudder_pedal);
406 // input += " stick_forward " + ftoa(Long_control);
407 // input += " stick_trim_forward " + ftoa(Long_trim);
408 // input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
409 // input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
410 // input += " vehicle_speed " + ftoa(V_rel_wind);
411 // input += " throttle_forward " + ftoa(Throttle_pct);
412 // input += " altitude " + ftoa(Altitude);
413 // input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
415 // testarray.getHello();
416 // testarray.sendData(input);
418 /* End of Networking */
422 void uiuc_wind_routine()
427 void uiuc_engine_routine()
432 void uiuc_gear_routine ()
437 void uiuc_record_routine(double dt)
439 if (trigger_last_time_step == 0 && trigger_on == 1) {
440 if (trigger_toggle == 0)
445 if (trigger_num % 2 != 0)
449 if (Simtime >= recordStartTime)
452 trigger_last_time_step = trigger_on;
455 void uiuc_network_recv_routine()
457 //if (use_uiuc_network)
461 void uiuc_network_send_routine()
463 //if (use_uiuc_network)
466 //end uiuc_wrapper.cpp