1 /**********************************************************************
2
3 FILENAME: uiuc_wrapper.cpp
4
5 ----------------------------------------------------------------------
6
7 DESCRIPTION: A wrapper(interface) between the UIUC Aeromodel (C++ files)
8 and the LaRCsim FDM (C files)
9
10 ----------------------------------------------------------------------
11
12 STATUS: alpha version
13
14 ----------------------------------------------------------------------
15
16 REFERENCES:
17
18 ----------------------------------------------------------------------
19
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
24 the velocities.
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
29 body-axis is used
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
34
35 ----------------------------------------------------------------------
36
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>
41
42 ----------------------------------------------------------------------
43
44 VARIABLES:
45
46 ----------------------------------------------------------------------
47
48 INPUTS: *
49
50 ----------------------------------------------------------------------
51
52 OUTPUTS: *
53
54 ----------------------------------------------------------------------
55
56 CALLED BY: *
57
58 ----------------------------------------------------------------------
59
60 CALLS TO: *
61
62 ----------------------------------------------------------------------
63
64 COPYRIGHT: (C) 2000 by Michael Selig
65
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.
69
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.
74
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.
78
79 **********************************************************************/
80
81 #ifdef HAVE_CONFIG_H
82 # include <config.h>
83 #endif
84
85 #include <iostream>
86 #include <cstring>
87
88 #include <Main/fg_props.hxx>
89 #include <Main/sentryIntegration.hxx>
90 #include <simgear/compiler.h>
91 #include <simgear/misc/sg_path.hxx>
92
93 #include "uiuc_aircraft.h"
94 #include "uiuc_coefficients.h"
95 #include "uiuc_getwind.h"
96 #include "uiuc_engine.h"
97 #include "uiuc_gear.h"
98 #include "uiuc_aerodeflections.h"
99 #include "uiuc_recorder.h"
100 #include "uiuc_menu.h"
101 #include "uiuc_betaprobe.h"
102 #include <FDM/LaRCsim/ls_generic.h>
103 //#include "Main/simple_udp.h"
104 #include "uiuc_fog.h" //321654
105 //#include "uiuc_network.h"
106 #include "uiuc_get_flapper.h"
107
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();
119
120 AIRCRAFT *aircraft_ = 0;
121
122 // SendArray testarray(4950);
123
124 /* Convert float to string */
125 //string ftoa(double in)
126 //{
127 // static char temp[20];
128 // sprintf(temp,"%g",in);
129 // return (string)temp;
130 //}
131
uiuc_initial_init()132 void uiuc_initial_init ()
133 {
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;
145
146 if (Phi_init_true)
147 Phi = Phi_init;
148 if (Theta_init_true)
149 Theta = Theta_init;
150 if (Psi_init_true)
151 Psi = Psi_init;
152
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;
159 }
160
uiuc_defaults_inits()161 void uiuc_defaults_inits ()
162 {
163 if (aircraft_ == 0) {
164 // becuase we're getting crashes about aircraft_ being null, add
165 // an explicit breadcrumb at this point
166 flightgear::addSentryBreadcrumb("UIUC aircraf created", "info");
167 aircraft_ = new AIRCRAFT;
168 }
169
170 // set defaults and initialize (called once from uiuc_init_2_wrapper)
171
172 //fog inits
173 fog_field = 0;
174 fog_segments = 0;
175 fog_point_index = -1;
176 fog_time = NULL;
177 fog_value = NULL;
178 fog_next_time = 0.0;
179 fog_current_segment = 0;
180 Fog = 0;
181
182 use_V_rel_wind_2U = false;
183 nondim_rate_V_rel_wind = false;
184 use_abs_U_body_2U = false;
185 use_dyn_on_speed_curve1 = false;
186 use_Alpha_dot_on_speed = false;
187 use_gamma_horiz_on_speed = false;
188 b_downwashMode = false;
189 P_body_init_true = false;
190 Q_body_init_true = false;
191 R_body_init_true = false;
192 Phi_init_true = false;
193 Theta_init_true = false;
194 Psi_init_true = false;
195 Alpha_init_true = false;
196 Beta_init_true = false;
197 U_body_init_true = false;
198 V_body_init_true = false;
199 W_body_init_true = false;
200 trim_case_2 = false;
201 use_uiuc_network = false;
202 icing_demo = false;
203 outside_control = false;
204 set_Long_trim = false;
205 zero_Long_trim = false;
206 elevator_step = false;
207 elevator_singlet = false;
208 elevator_doublet = false;
209 elevator_input = false;
210 elevator_input_ntime = 0;
211 aileron_input = false;
212 aileron_input_ntime = 0;
213 rudder_input = false;
214 rudder_input_ntime = 0;
215 pilot_elev_no = false;
216 pilot_elev_no_check = false;
217 pilot_ail_no = false;
218 pilot_ail_no_check = false;
219 pilot_rud_no = false;
220 pilot_rud_no_check = false;
221 use_flaps = false;
222 use_spoilers = false;
223 flap_pos_input = false;
224 flap_pos_input_ntime = 0;
225 use_elevator_sas_type1 = false;
226 use_elevator_sas_max = false;
227 use_elevator_sas_min = false;
228 use_elevator_stick_gain = false;
229 use_aileron_sas_type1 = false;
230 use_aileron_sas_max = false;
231 use_aileron_stick_gain = false;
232 use_rudder_sas_type1 = false;
233 use_rudder_sas_max = false;
234 use_rudder_stick_gain = false;
235 simpleSingleModel = false;
236 Throttle_pct_input = false;
237 Throttle_pct_input_ntime = 0;
238 gyroForce_Q_body = false;
239 gyroForce_R_body = false;
240 b_slipstreamEffects = false;
241 Xp_input = false;
242 Xp_input_ntime = 0;
243 Zp_input = false;
244 Zp_input_ntime = 0;
245 Mp_input = false;
246 Mp_input_ntime = 0;
247 b_CLK = false;
248 // gear_model[MAX_GEAR]
249 memset(gear_model,false,MAX_GEAR*sizeof(gear_model[0]));
250 use_gear = false;
251 // start with gear down if it is ultimately used
252 gear_pos_norm = 1;
253 ice_model = false;
254 ice_on = false;
255 beta_model = false;
256 // bootTrue[20] = 0;
257 memset(bootTrue,false,20*sizeof(gear_model[0]));
258 eta_from_file = false;
259 eta_wing_left_input = false;
260 eta_wing_right_input = false;
261 eta_tail_input = false;
262 demo_eps_alpha_max = false;
263 demo_eps_pitch_max = false;
264 demo_eps_pitch_min = false;
265 demo_eps_roll_max = false;
266 demo_eps_thrust_min = false;
267 demo_eps_airspeed_max = false;
268 demo_eps_airspeed_min = false;
269 demo_eps_flap_max = false;
270 demo_boot_cycle_tail = false;
271 demo_boot_cycle_wing_left = false;
272 demo_boot_cycle_wing_right = false;
273 demo_eps_pitch_input = false;
274 tactilefadef = false;
275 demo_ap_pah_on = false;
276 demo_ap_alh_on = false;
277 demo_ap_Theta_ref = false;
278 demo_ap_alt_ref = false;
279 demo_tactile = false;
280 demo_ice_tail = false;
281 demo_ice_left = false;
282 demo_ice_right = false;
283 flapper_model = false;
284 ignore_unknown_keywords = false;
285 pilot_throttle_no = false;
286 Dx_cg = 0.0;
287 Dy_cg = 0.0;
288 Dz_cg = 0.0;
289 ap_pah_on = 0;
290 ap_alh_on = 0;
291
292 }
293
uiuc_vel_init()294 void uiuc_vel_init ()
295 {
296 // Calculates the local velocity (V_north, V_east, V_down) from the body
297 // velocities.
298 // Called from uiuc_local_vel_init which is called from ls_step.
299 // Used during initialization (while Simtime=0)
300 if (U_body_init_true && V_body_init_true && W_body_init_true)
301 {
302 double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
303
304 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);
305 cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
306 cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
307 cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
308 cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
309 cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
310 cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
311 cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
312 cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
313 cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
314
315 V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
316 V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
317 V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
318
319 V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
320 }
321 }
322
uiuc_init_aeromodel()323 void uiuc_init_aeromodel ()
324 {
325 // Initializes the UIUC aircraft model.
326 // Called once from uiuc_init_2_wrapper
327 SGPath path(fgGetString("/sim/aircraft-dir"));
328 path.append("aircraft.dat");
329 std::cout << "We are using "<< path << std::endl;
330 uiuc_initializemaps(); // Initialize the <string,int> maps
331 uiuc_menu(path.local8BitStr()); // Read the specified aircraft file
332 }
333
uiuc_force_moment(double dt)334 void uiuc_force_moment(double dt)
335 {
336 double qS = Dynamic_pressure * Sw;
337 double qScbar = qS * cbar;
338 double qSb = qS * bw;
339
340 uiuc_aerodeflections(dt);
341 uiuc_coefficients(dt);
342
343 /* Calculate the forces */
344 if (CX && CZ)
345 {
346 F_X_aero = CX * qS;
347 F_Y_aero = CY * qS;
348 F_Z_aero = CZ * qS;
349 }
350 else
351 {
352 // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
353 // hence Cos_beta * Cos_beta term included.
354 // Same thing is done w/ moments below.
355 // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
356 // that should not be the case. See FGFS notes 021105
357 F_X_wind = -CD * qS * Cos_beta * Cos_beta;
358 F_Y_wind = CY * qS;
359 F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
360 // F_X_wind = -CD * qS * Cos_beta * Cos_beta;
361 // F_Y_wind = CY * qS * Cos_beta * Cos_beta;
362 // F_Z_wind = -CL * qS * Cos_beta * Cos_beta;
363
364 // wind-axis to body-axis transformation
365 F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
366 F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
367 F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
368 }
369 // Moment calculations
370 M_l_aero = Cl * qSb ;
371 M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
372 M_n_aero = Cn * qSb ;
373 // M_l_aero = Cl * qSb * Cos_beta * Cos_beta;
374 // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
375 // M_n_aero = Cn * qSb * Cos_beta * Cos_beta;
376
377 // Adding in apparent mass effects
378 if (Mass_appMass_ratio)
379 F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
380 if (I_xx_appMass_ratio)
381 M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
382 if (I_yy_appMass_ratio)
383 M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
384 if (I_zz_appMass_ratio)
385 M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
386
387 // adding in apparent mass in body axis X direction
388 // F_X_aero += -(0.05 * Mass) * U_dot_body;
389
390
391 if (Mass_appMass)
392 F_Z_aero += -Mass_appMass * W_dot_body;
393 if (I_xx_appMass)
394 M_l_aero += -I_xx_appMass * P_dot_body;
395 if (I_yy_appMass)
396 M_m_aero += -I_yy_appMass * Q_dot_body;
397 if (I_zz_appMass)
398 M_n_aero += -I_zz_appMass * R_dot_body;
399
400 // gyroscopic moments
401 // engineOmega is positive when rotation is ccw when viewed from the front
402 if (gyroForce_Q_body)
403 M_n_aero += polarInertia * engineOmega * Q_body;
404 if (gyroForce_R_body)
405 M_m_aero += -polarInertia * engineOmega * R_body;
406
407 // ornithopter support
408 if (flapper_model)
409 {
410 uiuc_get_flapper(dt);
411 F_X_aero += F_X_aero_flapper;
412 F_Z_aero += F_Z_aero_flapper;
413 M_m_aero += flapper_Moment;
414 }
415
416 // fog field update
417 Fog = 0;
418 if (fog_field)
419 uiuc_fog();
420
421 double vis;
422 if (Fog != 0)
423 {
424 vis = fgGetDouble("/environment/visibility-m");
425 if (Fog > 0)
426 vis /= 1.01;
427 else
428 vis *= 1.01;
429 fgSetDouble("/environment/visibility-m", vis);
430 }
431
432
433 /* Send data on the network to the Glass Cockpit */
434
435 // string input="";
436
437 // input += " stick_right " + ftoa(Lat_control);
438 // input += " rudder_left " + ftoa(-Rudder_pedal);
439 // input += " stick_forward " + ftoa(Long_control);
440 // input += " stick_trim_forward " + ftoa(Long_trim);
441 // input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
442 // input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
443 // input += " vehicle_speed " + ftoa(V_rel_wind);
444 // input += " throttle_forward " + ftoa(Throttle_pct);
445 // input += " altitude " + ftoa(Altitude);
446 // input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
447
448 // testarray.getHello();
449 // testarray.sendData(input);
450
451 /* End of Networking */
452
453 }
454
uiuc_wind_routine()455 void uiuc_wind_routine()
456 {
457 uiuc_getwind();
458 }
459
uiuc_engine_routine()460 void uiuc_engine_routine()
461 {
462 uiuc_engine();
463 }
464
uiuc_gear_routine()465 void uiuc_gear_routine ()
466 {
467 uiuc_gear();
468 }
469
uiuc_record_routine(double dt)470 void uiuc_record_routine(double dt)
471 {
472 if (trigger_last_time_step == 0 && trigger_on == 1) {
473 if (trigger_toggle == 0)
474 trigger_toggle = 1;
475 else
476 trigger_toggle = 0;
477 trigger_num++;
478 if (trigger_num % 2 != 0)
479 trigger_counter++;
480 }
481
482 if (Simtime >= recordStartTime)
483 uiuc_recorder(dt);
484
485 trigger_last_time_step = trigger_on;
486 }
487
uiuc_network_recv_routine()488 void uiuc_network_recv_routine()
489 {
490 //if (use_uiuc_network)
491 // uiuc_network(1);
492 }
493
uiuc_network_send_routine()494 void uiuc_network_send_routine()
495 {
496 //if (use_uiuc_network)
497 // uiuc_network(2);
498 }
499 //end uiuc_wrapper.cpp
500