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;
199 outside_control = false;
200 set_Long_trim = false;
201 zero_Long_trim = false;
202 elevator_step = false;
203 elevator_singlet = false;
204 elevator_doublet = false;
205 elevator_input = false;
206 elevator_input_ntime = 0;
207 aileron_input = false;
208 aileron_input_ntime = 0;
209 rudder_input = false;
210 rudder_input_ntime = 0;
211 pilot_elev_no = false;
212 pilot_elev_no_check = false;
213 pilot_ail_no = false;
214 pilot_ail_no_check = false;
215 pilot_rud_no = false;
216 pilot_rud_no_check = false;
218 use_spoilers = false;
219 flap_pos_input = false;
220 flap_pos_input_ntime = 0;
221 use_elevator_sas_type1 = false;
222 use_elevator_sas_max = false;
223 use_elevator_sas_min = false;
224 use_elevator_stick_gain = false;
225 use_aileron_sas_type1 = false;
226 use_aileron_sas_max = false;
227 use_aileron_stick_gain = false;
228 use_rudder_sas_type1 = false;
229 use_rudder_sas_max = false;
230 use_rudder_stick_gain = false;
231 simpleSingleModel = false;
232 Throttle_pct_input = false;
233 Throttle_pct_input_ntime = 0;
234 gyroForce_Q_body = false;
235 gyroForce_R_body = false;
236 b_slipstreamEffects = false;
244 // gear_model[MAX_GEAR]
245 memset(gear_model,false,MAX_GEAR*sizeof(gear_model[0]));
247 // start with gear down if it is ultimately used
253 memset(bootTrue,false,20*sizeof(gear_model[0]));
254 eta_from_file = false;
255 eta_wing_left_input = false;
256 eta_wing_right_input = false;
257 eta_tail_input = false;
258 demo_eps_alpha_max = false;
259 demo_eps_pitch_max = false;
260 demo_eps_pitch_min = false;
261 demo_eps_roll_max = false;
262 demo_eps_thrust_min = false;
263 demo_eps_airspeed_max = false;
264 demo_eps_airspeed_min = false;
265 demo_eps_flap_max = false;
266 demo_boot_cycle_tail = false;
267 demo_boot_cycle_wing_left = false;
268 demo_boot_cycle_wing_right = false;
269 demo_eps_pitch_input = false;
270 tactilefadef = false;
271 demo_ap_pah_on = false;
272 demo_ap_alh_on = false;
273 demo_ap_Theta_ref = false;
274 demo_ap_alt_ref = false;
275 demo_tactile = false;
276 demo_ice_tail = false;
277 demo_ice_left = false;
278 demo_ice_right = false;
279 flapper_model = false;
280 ignore_unknown_keywords = false;
281 pilot_throttle_no = false;
290 void uiuc_vel_init ()
292 // Calculates the local velocity (V_north, V_east, V_down) from the body
294 // Called from uiuc_local_vel_init which is called from ls_step.
295 // Used during initialization (while Simtime=0)
296 if (U_body_init_true && V_body_init_true && W_body_init_true)
298 double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
300 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);
301 cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
302 cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
303 cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
304 cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
305 cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
306 cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
307 cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
308 cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
309 cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
311 V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
312 V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
313 V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
315 V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
319 void uiuc_init_aeromodel ()
321 // Initializes the UIUC aircraft model.
322 // Called once from uiuc_init_2_wrapper
323 SGPath path(globals->get_fg_root());
324 path.append(aircraft_dir);
325 path.append("aircraft.dat");
326 cout << "We are using "<< path.str() << endl;
327 uiuc_initializemaps(); // Initialize the <string,int> maps
328 uiuc_menu(path.str()); // Read the specified aircraft file
331 void uiuc_force_moment(double dt)
333 double qS = Dynamic_pressure * Sw;
334 double qScbar = qS * cbar;
335 double qSb = qS * bw;
337 uiuc_aerodeflections(dt);
338 uiuc_coefficients(dt);
340 /* Calculate the forces */
349 // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
350 // hence Cos_beta * Cos_beta term included.
351 // Same thing is done w/ moments below.
352 // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
353 // that should not be the case. See FGFS notes 021105
354 F_X_wind = -CD * qS * Cos_beta * Cos_beta;
356 F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
357 // F_X_wind = -CD * qS * Cos_beta * Cos_beta;
358 // F_Y_wind = CY * qS * Cos_beta * Cos_beta;
359 // F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
361 // wind-axis to body-axis transformation
362 F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
363 F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
364 F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
366 // Moment calculations
367 M_l_aero = Cl * qSb ;
368 M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
369 M_n_aero = Cn * qSb ;
370 // M_l_aero = Cl * qSb * Cos_beta * Cos_beta;
371 // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
372 // M_n_aero = Cn * qSb * Cos_beta * Cos_beta;
374 // Adding in apparent mass effects
375 if (Mass_appMass_ratio)
376 F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
377 if (I_xx_appMass_ratio)
378 M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
379 if (I_yy_appMass_ratio)
380 M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
381 if (I_zz_appMass_ratio)
382 M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
384 // adding in apparent mass in body axis X direction
385 // F_X_aero += -(0.05 * Mass) * U_dot_body;
389 F_Z_aero += -Mass_appMass * W_dot_body;
391 M_l_aero += -I_xx_appMass * P_dot_body;
393 M_m_aero += -I_yy_appMass * Q_dot_body;
395 M_n_aero += -I_zz_appMass * R_dot_body;
397 // gyroscopic moments
398 // engineOmega is positive when rotation is ccw when viewed from the front
399 if (gyroForce_Q_body)
400 M_n_aero += polarInertia * engineOmega * Q_body;
401 if (gyroForce_R_body)
402 M_m_aero += -polarInertia * engineOmega * R_body;
404 // ornithopter support
407 uiuc_get_flapper(dt);
408 F_X_aero += F_X_aero_flapper;
409 F_Z_aero += F_Z_aero_flapper;
410 M_m_aero += flapper_Moment;
421 vis = fgGetDouble("/environment/visibility-m");
426 fgSetDouble("/environment/visibility-m", vis);
430 /* Send data on the network to the Glass Cockpit */
434 // input += " stick_right " + ftoa(Lat_control);
435 // input += " rudder_left " + ftoa(-Rudder_pedal);
436 // input += " stick_forward " + ftoa(Long_control);
437 // input += " stick_trim_forward " + ftoa(Long_trim);
438 // input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
439 // input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
440 // input += " vehicle_speed " + ftoa(V_rel_wind);
441 // input += " throttle_forward " + ftoa(Throttle_pct);
442 // input += " altitude " + ftoa(Altitude);
443 // input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
445 // testarray.getHello();
446 // testarray.sendData(input);
448 /* End of Networking */
452 void uiuc_wind_routine()
457 void uiuc_engine_routine()
462 void uiuc_gear_routine ()
467 void uiuc_record_routine(double dt)
469 if (trigger_last_time_step == 0 && trigger_on == 1) {
470 if (trigger_toggle == 0)
475 if (trigger_num % 2 != 0)
479 if (Simtime >= recordStartTime)
482 trigger_last_time_step = trigger_on;
485 void uiuc_network_recv_routine()
487 //if (use_uiuc_network)
491 void uiuc_network_send_routine()
493 //if (use_uiuc_network)
496 //end uiuc_wrapper.cpp