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 SG_USING_STD(cout);
 106 SG_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_ = new AIRCRAFT;
 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   // set defaults and initialize (called once from uiuc_init_2_wrapper)
 164
 165   //fog inits
 166   fog_field = 0;
 167   fog_segments = 0;
 168   fog_point_index = -1;
 169   fog_time = NULL;
 170   fog_value = NULL;
 171   fog_next_time = 0.0;
 172   fog_current_segment = 0;
 173   Fog = 0;
 174
 175   use_V_rel_wind_2U = false;
 176   nondim_rate_V_rel_wind = false;
 177   use_abs_U_body_2U = false;
 178   use_dyn_on_speed_curve1 = false;
 179   use_Alpha_dot_on_speed = false;
 180   use_gamma_horiz_on_speed = false;
 181   b_downwashMode = false;
 182   P_body_init_true = false;
 183   Q_body_init_true = false;
 184   R_body_init_true = false;
 185   Phi_init_true = false;
 186   Theta_init_true = false;
 187   Psi_init_true = false;
 188   Alpha_init_true = false;
 189   Beta_init_true = false;
 190   U_body_init_true = false;
 191   V_body_init_true = false;
 192   W_body_init_true = false;
 193   trim_case_2 = false;
 194   use_uiuc_network = false;
 195   icing_demo = false;
 196   outside_control = false;
 197   set_Long_trim = false;
 198   zero_Long_trim = false;
 199   elevator_step = false;
 200   elevator_singlet = false;
 201   elevator_doublet = false;
 202   elevator_input = false;
 203   elevator_input_ntime = 0;
 204   aileron_input = false;
 205   aileron_input_ntime = 0;
 206   rudder_input = false;
 207   rudder_input_ntime = 0;
 208   pilot_elev_no = false;
 209   pilot_elev_no_check = false;
 210   pilot_ail_no = false;
 211   pilot_ail_no_check = false;
 212   pilot_rud_no = false;
 213   pilot_rud_no_check = false;
 214   use_flaps = false;
 215   use_spoilers = false;
 216   flap_pos_input = false;
 217   flap_pos_input_ntime = 0;
 218   use_elevator_sas_type1 = false;
 219   use_elevator_sas_max = false;
 220   use_elevator_sas_min = false;
 221   use_elevator_stick_gain = false;
 222   use_aileron_sas_type1 = false;
 223   use_aileron_sas_max = false;
 224   use_aileron_stick_gain = false;
 225   use_rudder_sas_type1 = false;
 226   use_rudder_sas_max = false;
 227   use_rudder_stick_gain = false;
 228   simpleSingleModel = false;
 229   Throttle_pct_input = false;
 230   Throttle_pct_input_ntime = 0;
 231   gyroForce_Q_body = false;
 232   gyroForce_R_body = false;
 233   b_slipstreamEffects = false;
 234   Xp_input = false;
 235   Xp_input_ntime = 0;
 236   Zp_input = false;
 237   Zp_input_ntime = 0;
 238   Mp_input = false;
 239   Mp_input_ntime = 0;
 240   b_CLK = false;
 241   //  gear_model[MAX_GEAR]
 242   memset(gear_model,false,MAX_GEAR*sizeof(gear_model[0]));
 243   use_gear = false;
 244   // start with gear down if it is ultimately used
 245   gear_pos_norm = 1;
 246   ice_model = false;
 247   ice_on = false;
 248   beta_model = false;
 249   //  bootTrue[20] = 0;
 250   memset(bootTrue,false,20*sizeof(gear_model[0]));
 251   eta_from_file = false;
 252   eta_wing_left_input = false;
 253   eta_wing_right_input = false;
 254   eta_tail_input = false;
 255   demo_eps_alpha_max = false;
 256   demo_eps_pitch_max = false;
 257   demo_eps_pitch_min = false;
 258   demo_eps_roll_max = false;
 259   demo_eps_thrust_min = false;
 260   demo_eps_airspeed_max = false;
 261   demo_eps_airspeed_min = false;
 262   demo_eps_flap_max = false;
 263   demo_boot_cycle_tail = false;
 264   demo_boot_cycle_wing_left = false;
 265   demo_boot_cycle_wing_right = false;
 266   demo_eps_pitch_input = false;
 267   tactilefadef = false;
 268   demo_ap_pah_on = false;
 269   demo_ap_alh_on = false;
 270   demo_ap_Theta_ref = false;
 271   demo_ap_alt_ref = 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   Dx_cg = 0.0;
 280   Dy_cg = 0.0;
 281   Dz_cg = 0.0;
 282   ap_pah_on = 0;
 283   ap_alh_on = 0;
 284
 285 }
 286
 287 void uiuc_vel_init ()
 288 {
 289   // Calculates the local velocity (V_north, V_east, V_down) from the body
 290   // velocities.
 291   // Called from uiuc_local_vel_init which is called from ls_step.
 292   // Used during initialization (while Simtime=0)
 293   if (U_body_init_true && V_body_init_true && W_body_init_true)
 294     {
 295   double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
 296
 297   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);
 298   cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
 299   cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
 300   cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
 301   cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
 302   cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
 303   cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
 304   cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
 305   cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
 306   cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
 307
 308   V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
 309   V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
 310   V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
 311
 312   V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
 313     }
 314 }
 315
 316 void uiuc_init_aeromodel ()
 317 {
 318   // Initializes the UIUC aircraft model.
 319   // Called once from uiuc_init_2_wrapper
 320   SGPath path(globals->get_fg_root());
 321   path.append(fgGetString("/sim/aircraft-dir"));
 322   path.append("aircraft.dat");
 323   cout << "We are using "<< path.str() << endl;
 324   uiuc_initializemaps(); // Initialize the <string,int> maps
 325   uiuc_menu(path.str());   // Read the specified aircraft file
 326 }
 327
 328 void uiuc_force_moment(double dt)
 329 {
 330   double qS = Dynamic_pressure * Sw;
 331   double qScbar = qS * cbar;
 332   double qSb = qS * bw;
 333
 334   uiuc_aerodeflections(dt);
 335   uiuc_coefficients(dt);
 336
 337   /* Calculate the forces */
 338   if (CX && CZ)
 339     {
 340       F_X_aero = CX * qS;
 341       F_Y_aero = CY * qS;
 342       F_Z_aero = CZ * qS;
 343     }
 344   else
 345     {
 346       // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
 347       // hence Cos_beta * Cos_beta term included.
 348       // Same thing is done w/ moments below.
 349       // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
 350       // that should not be the case.  See FGFS notes 021105
 351       F_X_wind = -CD * qS  * Cos_beta * Cos_beta;
 352       F_Y_wind =  CY * qS;
 353       F_Z_wind = -CL * qS  * Cos_beta * Cos_beta;
 354       // F_X_wind = -CD * qS  * Cos_beta * Cos_beta;
 355       // F_Y_wind =  CY * qS  * Cos_beta * Cos_beta;
 356       // F_Z_wind = -CL * qS  * Cos_beta * Cos_beta;
 357
 358       // wind-axis to body-axis transformation
 359       F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
 360       F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
 361       F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
 362     }
 363   // Moment calculations
 364   M_l_aero = Cl * qSb    ;
 365   M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
 366   M_n_aero = Cn * qSb    ;
 367   // M_l_aero = Cl * qSb    * Cos_beta * Cos_beta;
 368   // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
 369   // M_n_aero = Cn * qSb    * Cos_beta * Cos_beta;
 370
 371   // Adding in apparent mass effects
 372   if (Mass_appMass_ratio)
 373     F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
 374   if (I_xx_appMass_ratio)
 375     M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
 376   if (I_yy_appMass_ratio)
 377     M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
 378   if (I_zz_appMass_ratio)
 379     M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
 380
 381   // adding in apparent mass in body axis X direction
 382   // F_X_aero += -(0.05 * Mass) * U_dot_body;
 383
 384
 385   if (Mass_appMass)
 386     F_Z_aero += -Mass_appMass * W_dot_body;
 387   if (I_xx_appMass)
 388     M_l_aero += -I_xx_appMass * P_dot_body;
 389   if (I_yy_appMass)
 390     M_m_aero += -I_yy_appMass * Q_dot_body;
 391   if (I_zz_appMass)
 392     M_n_aero += -I_zz_appMass * R_dot_body;
 393
 394   // gyroscopic moments
 395   // engineOmega is positive when rotation is ccw when viewed from the front
 396   if (gyroForce_Q_body)
 397     M_n_aero +=  polarInertia * engineOmega * Q_body;
 398   if (gyroForce_R_body)
 399     M_m_aero += -polarInertia * engineOmega * R_body;
 400
 401   // ornithopter support
 402   if (flapper_model)
 403     {
 404       uiuc_get_flapper(dt);
 405       F_X_aero += F_X_aero_flapper;
 406       F_Z_aero += F_Z_aero_flapper;
 407       M_m_aero += flapper_Moment;
 408     }
 409
 410   // fog field update
 411    Fog = 0;
 412    if (fog_field)
 413      uiuc_fog();
 414
 415    double vis;
 416    if (Fog != 0)
 417    {
 418      vis = fgGetDouble("/environment/visibility-m");
 419      if (Fog > 0)
 420        vis /= 1.01;
 421      else
 422        vis *= 1.01;
 423      fgSetDouble("/environment/visibility-m", vis);
 424    }
 425
 426
 427   /* Send data on the network to the Glass Cockpit */
 428
 429    //  string input="";
 430
 431    //  input += " stick_right " + ftoa(Lat_control);
 432    //  input += " rudder_left " + ftoa(-Rudder_pedal);
 433    //  input += " stick_forward " + ftoa(Long_control);
 434    //  input += " stick_trim_forward " + ftoa(Long_trim);
 435    //  input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
 436    //  input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
 437    //  input += " vehicle_speed " + ftoa(V_rel_wind);
 438    //  input += " throttle_forward " + ftoa(Throttle_pct);
 439    //  input += " altitude " + ftoa(Altitude);
 440    //  input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
 441
 442    //  testarray.getHello();
 443    //  testarray.sendData(input);
 444
 445   /* End of Networking */
 446
 447 }
 448
 449 void uiuc_wind_routine()
 450 {
 451   uiuc_getwind();
 452 }
 453
 454 void uiuc_engine_routine()
 455 {
 456   uiuc_engine();
 457 }
 458
 459 void uiuc_gear_routine ()
 460 {
 461   uiuc_gear();
 462 }
 463
 464 void uiuc_record_routine(double dt)
 465 {
 466   if (trigger_last_time_step == 0 && trigger_on == 1) {
 467     if (trigger_toggle == 0)
 468       trigger_toggle = 1;
 469     else
 470       trigger_toggle = 0;
 471     trigger_num++;
 472     if (trigger_num % 2 != 0)
 473       trigger_counter++;
 474   }
 475
 476   if (Simtime >= recordStartTime)
 477     uiuc_recorder(dt);
 478
 479   trigger_last_time_step = trigger_on;
 480 }
 481
 482 void uiuc_network_recv_routine()
 483 {
 484   //if (use_uiuc_network)
 485   //  uiuc_network(1);
 486 }
 487
 488 void uiuc_network_send_routine()
 489 {
 490   //if (use_uiuc_network)
 491   //  uiuc_network(2);
 492 }
 493 //end uiuc_wrapper.cpp