]> git.mxchange.org Git - flightgear.git/blob - src/FDM/UIUCModel/uiuc_wrapper.cpp
Make sure sign is preserved for turbulence direction.
[flightgear.git] / src / FDM / UIUCModel / uiuc_wrapper.cpp
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., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
78  USA or view http://www.gnu.org/copyleft/gpl.html.
79  
80 **********************************************************************/
81
82 #ifdef HAVE_CONFIG_H
83 #  include <config.h>
84 #endif
85
86 #include <simgear/compiler.h>
87 #include <simgear/misc/sg_path.hxx>
88 #include <Aircraft/aircraft.hxx>
89 #include <Main/fg_props.hxx>
90
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"
106
107 SG_USING_STD(cout);
108 SG_USING_STD(endl);
109
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();
121
122 AIRCRAFT *aircraft_ = new AIRCRAFT;
123 AIRCRAFTDIR *aircraftdir_ = new AIRCRAFTDIR;
124
125 // SendArray testarray(4950);
126
127 /* Convert float to string */
128 //string ftoa(double in)
129 //{
130 //  static char temp[20];
131 //  sprintf(temp,"%g",in);
132 //  return (string)temp;
133 //}
134
135 void uiuc_initial_init ()
136 {
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;
146
147   if (Phi_init_true)
148     Phi = Phi_init;
149   if (Theta_init_true)
150     Theta = Theta_init;
151   if (Psi_init_true)
152     Psi = Psi_init;
153
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;
160 }
161
162 void uiuc_defaults_inits ()
163 {
164   // set defaults and initialize (called from ls_step.c at Simtime=0)
165
166   //fog inits 
167   fog_field = 0;
168   fog_segments = 0;
169   fog_point_index = -1;
170   fog_time = NULL;
171   fog_value = NULL;
172   fog_next_time = 0.0;
173   fog_current_segment = 0;
174   Fog = 0;
175
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;
194   trim_case_2 = false;
195   use_uiuc_network = false;
196   old_flap_routine = false;
197   icing_demo = false;
198   outside_control = false;
199   set_Long_trim = false;
200   zero_Long_trim = false;
201   elevator_step = false;
202   elevator_singlet = false;
203   elevator_doublet = false;
204   elevator_input = false;
205   elevator_input_ntime = 0;
206   aileron_input = false;
207   aileron_input_ntime = 0;
208   rudder_input = false;
209   rudder_input_ntime = 0;
210   pilot_elev_no = false;
211   pilot_elev_no_check = false;
212   pilot_ail_no = false;
213   pilot_ail_no_check = false;
214   pilot_rud_no = false;
215   pilot_rud_no_check = false;
216   use_flaps = false;
217   use_spoilers = false;
218   flap_pos_input = false;
219   flap_pos_input_ntime = 0;
220   use_elevator_sas_type1 = false;
221   use_elevator_sas_max = false;
222   use_elevator_sas_min = false;
223   use_elevator_stick_gain = false;
224   use_aileron_sas_type1 = false;
225   use_aileron_sas_max = false;
226   use_aileron_stick_gain = false;
227   use_rudder_sas_type1 = false;
228   use_rudder_sas_max = false;
229   use_rudder_stick_gain = false;
230   simpleSingleModel = false;
231   Throttle_pct_input = false;
232   Throttle_pct_input_ntime = 0;
233   gyroForce_Q_body = false;
234   gyroForce_R_body = false;
235   b_slipstreamEffects = false;
236   Xp_input = false;
237   Xp_input_ntime = 0;
238   Zp_input = false;
239   Zp_input_ntime = 0;
240   Mp_input = false;
241   Mp_input_ntime = 0;
242   b_CLK = false;
243   //  gear_model[MAX_GEAR]
244   memset(gear_model,false,MAX_GEAR*sizeof(gear_model[0])); 
245   use_gear = false;
246   // start with gear down if it is ultimately used
247   gear_pos_norm = 1;
248   ice_model = false;
249   ice_on = false;
250   beta_model = false;
251   //  bootTrue[20] = 0;
252   memset(bootTrue,false,20*sizeof(gear_model[0]));
253   eta_from_file = false;
254   eta_wing_left_input = false;
255   eta_wing_right_input = false;
256   eta_tail_input = false;
257   demo_eps_alpha_max = false;
258   demo_eps_pitch_max = false;
259   demo_eps_pitch_min = false;
260   demo_eps_roll_max = false;
261   demo_eps_thrust_min = false;
262   demo_eps_airspeed_max = false;
263   demo_eps_airspeed_min = false;
264   demo_eps_flap_max = false;
265   demo_boot_cycle_tail = false;
266   demo_boot_cycle_wing_left = false;
267   demo_boot_cycle_wing_right = false;
268   demo_eps_pitch_input = false;
269   tactilefadef = false;
270   demo_ap_pah_on = false;
271   demo_ap_Theta_ref_deg = 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;
279
280 }
281
282 void uiuc_vel_init ()
283 {
284   if (U_body_init_true && V_body_init_true && W_body_init_true)
285     {
286   double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
287
288   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);
289   cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
290   cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
291   cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
292   cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
293   cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
294   cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
295   cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
296   cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
297   cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
298
299   V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
300   V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
301   V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
302
303   V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
304     }
305 }
306
307 void uiuc_init_aeromodel ()
308 {
309   SGPath path(globals->get_fg_root());
310   path.append(aircraft_dir);
311   path.append("aircraft.dat");
312   cout << "We are using "<< path.str() << endl;
313   uiuc_initializemaps(); // Initialize the <string,int> maps
314   uiuc_menu(path.str());   // Read the specified aircraft file 
315 }
316
317 void uiuc_force_moment(double dt)
318 {
319   double qS = Dynamic_pressure * Sw;
320   double qScbar = qS * cbar;
321   double qSb = qS * bw;
322
323   uiuc_aerodeflections(dt);
324   uiuc_coefficients(dt);
325
326   /* Calculate the forces */
327   if (CX && CZ)
328     {
329       F_X_aero = CX * qS;
330       F_Y_aero = CY * qS;
331       F_Z_aero = CZ * qS;
332     }
333   else
334     {
335       // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing, 
336       // hence Cos_beta * Cos_beta term included.
337       // Same thing is done w/ moments below.
338       // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
339       // that should not be the case.  See FGFS notes 021105
340       F_X_wind = -CD * qS  * Cos_beta * Cos_beta;
341       F_Y_wind =  CY * qS;
342       F_Z_wind = -CL * qS  * Cos_beta * Cos_beta;
343       // F_X_wind = -CD * qS  * Cos_beta * Cos_beta;
344       // F_Y_wind =  CY * qS  * Cos_beta * Cos_beta;
345       // F_Z_wind = -CL * qS  * Cos_beta * Cos_beta;
346
347       // wind-axis to body-axis transformation 
348       F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
349       F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
350       F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
351     }
352   // Moment calculations
353   M_l_aero = Cl * qSb    ;
354   M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
355   M_n_aero = Cn * qSb    ;
356   // M_l_aero = Cl * qSb    * Cos_beta * Cos_beta;
357   // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
358   // M_n_aero = Cn * qSb    * Cos_beta * Cos_beta;
359
360   // Adding in apparent mass effects
361   if (Mass_appMass_ratio)
362     F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
363   if (I_xx_appMass_ratio)
364     M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
365   if (I_yy_appMass_ratio)
366     M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
367   if (I_zz_appMass_ratio)
368     M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
369
370   if (Mass_appMass)
371     F_Z_aero += -Mass_appMass * W_dot_body;
372   if (I_xx_appMass)
373     M_l_aero += -I_xx_appMass * P_dot_body;
374   if (I_yy_appMass)
375     M_m_aero += -I_yy_appMass * Q_dot_body;
376   if (I_zz_appMass)
377     M_n_aero += -I_zz_appMass * R_dot_body;
378
379   // gyroscopic moments
380   // engineOmega is positive when rotation is ccw when viewed from the front
381   if (gyroForce_Q_body)
382     M_n_aero +=  polarInertia * engineOmega * Q_body;
383   if (gyroForce_R_body)
384     M_m_aero += -polarInertia * engineOmega * R_body;
385
386   // ornithopter support
387   if (flapper_model)
388     {
389       uiuc_get_flapper(dt);
390       F_X_aero += F_X_aero_flapper;
391       F_Z_aero += F_Z_aero_flapper;
392       M_m_aero += flapper_Moment;
393     }
394
395   // fog field update
396    Fog = 0;
397    if (fog_field)
398      uiuc_fog();
399
400    double vis;
401    if (Fog != 0)
402    {
403      vis = fgGetDouble("/environment/visibility-m");
404      if (Fog > 0)
405        vis /= 1.01;
406      else
407        vis *= 1.01;
408      fgSetDouble("/environment/visibility-m", vis);
409    }
410  
411
412   /* Send data on the network to the Glass Cockpit */
413  
414    //  string input="";
415
416    //  input += " stick_right " + ftoa(Lat_control);
417    //  input += " rudder_left " + ftoa(-Rudder_pedal);
418    //  input += " stick_forward " + ftoa(Long_control);
419    //  input += " stick_trim_forward " + ftoa(Long_trim);
420    //  input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
421    //  input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
422    //  input += " vehicle_speed " + ftoa(V_rel_wind);
423    //  input += " throttle_forward " + ftoa(Throttle_pct);
424    //  input += " altitude " + ftoa(Altitude);
425    //  input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
426  
427    //  testarray.getHello();
428    //  testarray.sendData(input);
429   
430   /* End of Networking */ 
431
432 }
433
434 void uiuc_wind_routine()
435 {
436   uiuc_getwind();
437 }
438
439 void uiuc_engine_routine()
440 {
441   uiuc_engine();
442 }
443
444 void uiuc_gear_routine ()
445 {
446   uiuc_gear();
447 }
448
449 void uiuc_record_routine(double dt)
450 {
451   if (trigger_last_time_step == 0 && trigger_on == 1) {
452     if (trigger_toggle == 0)
453       trigger_toggle = 1;
454     else
455       trigger_toggle = 0;
456     trigger_num++;
457     if (trigger_num % 2 != 0)
458       trigger_counter++;
459   }
460
461   if (Simtime >= recordStartTime)
462     uiuc_recorder(dt);
463
464   trigger_last_time_step = trigger_on;
465 }
466
467 void uiuc_network_recv_routine()
468 {
469   //if (use_uiuc_network)
470     //uiuc_network(1);
471 }
472
473 void uiuc_network_send_routine()
474 {
475   //if (use_uiuc_network)
476     //uiuc_network(2);
477 }
478 //end uiuc_wrapper.cpp