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., 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 <simgear/compiler.h>
  86 #include <simgear/misc/sg_path.hxx>
  87 #include <Aircraft/aircraft.hxx>
  88 #include <Main/fg_props.hxx>
  89
  90 #include "uiuc_aircraft.h"
  91 #include "uiuc_coefficients.h"
  92 #include "uiuc_getwind.h"
  93 #include "uiuc_engine.h"
  94 #include "uiuc_gear.h"
  95 #include "uiuc_aerodeflections.h"
  96 #include "uiuc_recorder.h"
  97 #include "uiuc_menu.h"
  98 #include "uiuc_betaprobe.h"
  99 #include <FDM/LaRCsim/ls_generic.h>
 100 //#include "Main/simple_udp.h"
 101 #include "uiuc_fog.h" //321654
 102 //#include "uiuc_network.h"
 103 #include "uiuc_get_flapper.h"
 104
 105 using std::cout;
 106 using std::endl;
 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
 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
 161 void uiuc_defaults_inits ()
 162 {
 163   if (aircraft_ == 0)
 164     aircraft_ = new AIRCRAFT;
 165
 166   // set defaults and initialize (called once from uiuc_init_2_wrapper)
 167
 168   //fog inits
 169   fog_field = 0;
 170   fog_segments = 0;
 171   fog_point_index = -1;
 172   fog_time = NULL;
 173   fog_value = NULL;
 174   fog_next_time = 0.0;
 175   fog_current_segment = 0;
 176   Fog = 0;
 177
 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;
 196   trim_case_2 = false;
 197   use_uiuc_network = false;
 198   icing_demo = 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;
 217   use_flaps = 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;
 237   Xp_input = false;
 238   Xp_input_ntime = 0;
 239   Zp_input = false;
 240   Zp_input_ntime = 0;
 241   Mp_input = false;
 242   Mp_input_ntime = 0;
 243   b_CLK = false;
 244   //  gear_model[MAX_GEAR]
 245   memset(gear_model,false,MAX_GEAR*sizeof(gear_model[0]));
 246   use_gear = false;
 247   // start with gear down if it is ultimately used
 248   gear_pos_norm = 1;
 249   ice_model = false;
 250   ice_on = false;
 251   beta_model = false;
 252   //  bootTrue[20] = 0;
 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;
 282   Dx_cg = 0.0;
 283   Dy_cg = 0.0;
 284   Dz_cg = 0.0;
 285   ap_pah_on = 0;
 286   ap_alh_on = 0;
 287
 288 }
 289
 290 void uiuc_vel_init ()
 291 {
 292   // Calculates the local velocity (V_north, V_east, V_down) from the body
 293   // velocities.
 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)
 297     {
 298   double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
 299
 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;
 310
 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;
 314
 315   V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
 316     }
 317 }
 318
 319 void uiuc_init_aeromodel ()
 320 {
 321   // Initializes the UIUC aircraft model.
 322   // Called once from uiuc_init_2_wrapper
 323   SGPath path(globals->get_fg_root());
 324   path.append(fgGetString("/sim/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
 329 }
 330
 331 void uiuc_force_moment(double dt)
 332 {
 333   double qS = Dynamic_pressure * Sw;
 334   double qScbar = qS * cbar;
 335   double qSb = qS * bw;
 336
 337   uiuc_aerodeflections(dt);
 338   uiuc_coefficients(dt);
 339
 340   /* Calculate the forces */
 341   if (CX && CZ)
 342     {
 343       F_X_aero = CX * qS;
 344       F_Y_aero = CY * qS;
 345       F_Z_aero = CZ * qS;
 346     }
 347   else
 348     {
 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;
 355       F_Y_wind =  CY * qS;
 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;
 360
 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;
 365     }
 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;
 373
 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;
 383
 384   // adding in apparent mass in body axis X direction
 385   // F_X_aero += -(0.05 * Mass) * U_dot_body;
 386
 387
 388   if (Mass_appMass)
 389     F_Z_aero += -Mass_appMass * W_dot_body;
 390   if (I_xx_appMass)
 391     M_l_aero += -I_xx_appMass * P_dot_body;
 392   if (I_yy_appMass)
 393     M_m_aero += -I_yy_appMass * Q_dot_body;
 394   if (I_zz_appMass)
 395     M_n_aero += -I_zz_appMass * R_dot_body;
 396
 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;
 403
 404   // ornithopter support
 405   if (flapper_model)
 406     {
 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;
 411     }
 412
 413   // fog field update
 414    Fog = 0;
 415    if (fog_field)
 416      uiuc_fog();
 417
 418    double vis;
 419    if (Fog != 0)
 420    {
 421      vis = fgGetDouble("/environment/visibility-m");
 422      if (Fog > 0)
 423        vis /= 1.01;
 424      else
 425        vis *= 1.01;
 426      fgSetDouble("/environment/visibility-m", vis);
 427    }
 428
 429
 430   /* Send data on the network to the Glass Cockpit */
 431
 432    //  string input="";
 433
 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);
 444
 445    //  testarray.getHello();
 446    //  testarray.sendData(input);
 447
 448   /* End of Networking */
 449
 450 }
 451
 452 void uiuc_wind_routine()
 453 {
 454   uiuc_getwind();
 455 }
 456
 457 void uiuc_engine_routine()
 458 {
 459   uiuc_engine();
 460 }
 461
 462 void uiuc_gear_routine ()
 463 {
 464   uiuc_gear();
 465 }
 466
 467 void uiuc_record_routine(double dt)
 468 {
 469   if (trigger_last_time_step == 0 && trigger_on == 1) {
 470     if (trigger_toggle == 0)
 471       trigger_toggle = 1;
 472     else
 473       trigger_toggle = 0;
 474     trigger_num++;
 475     if (trigger_num % 2 != 0)
 476       trigger_counter++;
 477   }
 478
 479   if (Simtime >= recordStartTime)
 480     uiuc_recorder(dt);
 481
 482   trigger_last_time_step = trigger_on;
 483 }
 484
 485 void uiuc_network_recv_routine()
 486 {
 487   //if (use_uiuc_network)
 488   //  uiuc_network(1);
 489 }
 490
 491 void uiuc_network_send_routine()
 492 {
 493   //if (use_uiuc_network)
 494   //  uiuc_network(2);
 495 }
 496 //end uiuc_wrapper.cpp