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_coefficients.h"
93 #include "uiuc_getwind.h"
94 #include "uiuc_engine.h"
95 #include "uiuc_gear.h"
96 #include "uiuc_aerodeflections.h"
97 #include "uiuc_recorder.h"
98 #include "uiuc_menu.h"
99 #include "uiuc_betaprobe.h"
100 #include <FDM/LaRCsim/ls_generic.h>
101 //#include "Main/simple_udp.h"
102 #include "uiuc_fog.h" //321654
103 //#include "uiuc_network.h"
104 #include "uiuc_get_flapper.h"
109 extern "C" void uiuc_initial_init ();
110 extern "C" void uiuc_defaults_inits ();
111 extern "C" void uiuc_vel_init ();
112 extern "C" void uiuc_init_aeromodel ();
113 extern "C" void uiuc_force_moment(double dt);
114 extern "C" void uiuc_engine_routine();
115 extern "C" void uiuc_wind_routine();
116 extern "C" void uiuc_gear_routine();
117 extern "C" void uiuc_record_routine(double dt);
118 extern "C" void uiuc_network_recv_routine();
119 extern "C" void uiuc_network_send_routine();
121 AIRCRAFT *aircraft_ = new AIRCRAFT;
123 // SendArray testarray(4950);
125 /* Convert float to string */
126 //string ftoa(double in)
128 // static char temp[20];
129 // sprintf(temp,"%g",in);
130 // return (string)temp;
133 void uiuc_initial_init ()
135 // This function is called from uiuc_init_2_wrapper (uiuc_aero.c in LaRCsim)
136 // which is called from ls_step and ls_model.
137 // Apply brute force initializations, which override unwanted changes
138 // performed by LaRCsim.
139 // Used during initialization (while Simtime=0).
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 once from uiuc_init_2_wrapper)
169 fog_point_index = -1;
173 fog_current_segment = 0;
176 use_V_rel_wind_2U = false;
177 nondim_rate_V_rel_wind = false;
178 use_abs_U_body_2U = false;
179 use_dyn_on_speed_curve1 = false;
180 use_Alpha_dot_on_speed = false;
181 use_gamma_horiz_on_speed = false;
182 b_downwashMode = false;
183 P_body_init_true = false;
184 Q_body_init_true = false;
185 R_body_init_true = false;
186 Phi_init_true = false;
187 Theta_init_true = false;
188 Psi_init_true = false;
189 Alpha_init_true = false;
190 Beta_init_true = false;
191 U_body_init_true = false;
192 V_body_init_true = false;
193 W_body_init_true = false;
195 use_uiuc_network = false;
197 outside_control = false;
198 set_Long_trim = false;
199 zero_Long_trim = false;
200 elevator_step = false;
201 elevator_singlet = false;
202 elevator_doublet = false;
203 elevator_input = false;
204 elevator_input_ntime = 0;
205 aileron_input = false;
206 aileron_input_ntime = 0;
207 rudder_input = false;
208 rudder_input_ntime = 0;
209 pilot_elev_no = false;
210 pilot_elev_no_check = false;
211 pilot_ail_no = false;
212 pilot_ail_no_check = false;
213 pilot_rud_no = false;
214 pilot_rud_no_check = false;
216 use_spoilers = false;
217 flap_pos_input = false;
218 flap_pos_input_ntime = 0;
219 use_elevator_sas_type1 = false;
220 use_elevator_sas_max = false;
221 use_elevator_sas_min = false;
222 use_elevator_stick_gain = false;
223 use_aileron_sas_type1 = false;
224 use_aileron_sas_max = false;
225 use_aileron_stick_gain = false;
226 use_rudder_sas_type1 = false;
227 use_rudder_sas_max = false;
228 use_rudder_stick_gain = false;
229 simpleSingleModel = false;
230 Throttle_pct_input = false;
231 Throttle_pct_input_ntime = 0;
232 gyroForce_Q_body = false;
233 gyroForce_R_body = false;
234 b_slipstreamEffects = false;
242 // gear_model[MAX_GEAR]
243 memset(gear_model,false,MAX_GEAR*sizeof(gear_model[0]));
245 // start with gear down if it is ultimately used
251 memset(bootTrue,false,20*sizeof(gear_model[0]));
252 eta_from_file = false;
253 eta_wing_left_input = false;
254 eta_wing_right_input = false;
255 eta_tail_input = false;
256 demo_eps_alpha_max = false;
257 demo_eps_pitch_max = false;
258 demo_eps_pitch_min = false;
259 demo_eps_roll_max = false;
260 demo_eps_thrust_min = false;
261 demo_eps_airspeed_max = false;
262 demo_eps_airspeed_min = false;
263 demo_eps_flap_max = false;
264 demo_boot_cycle_tail = false;
265 demo_boot_cycle_wing_left = false;
266 demo_boot_cycle_wing_right = false;
267 demo_eps_pitch_input = false;
268 tactilefadef = false;
269 demo_ap_pah_on = false;
270 demo_ap_alh_on = false;
271 demo_ap_Theta_ref = false;
272 demo_ap_alt_ref = false;
273 demo_tactile = false;
274 demo_ice_tail = false;
275 demo_ice_left = false;
276 demo_ice_right = false;
277 flapper_model = false;
278 ignore_unknown_keywords = false;
279 pilot_throttle_no = false;
288 void uiuc_vel_init ()
290 // Calculates the local velocity (V_north, V_east, V_down) from the body
292 // Called from uiuc_local_vel_init which is called from ls_step.
293 // Used during initialization (while Simtime=0)
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);
317 void uiuc_init_aeromodel ()
319 // Initializes the UIUC aircraft model.
320 // Called once from uiuc_init_2_wrapper
321 SGPath path(globals->get_fg_root());
322 path.append(fgGetString("/sim/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