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 is called from uiuc_init_2_wrapper (uiuc_aero.c in LaRCsim)
 138   // which is called from ls_step and ls_model.
 139   // Apply brute force initializations, which override unwanted changes
 140   // performed by LaRCsim.
 141   // Used during initialization (while Simtime=0).
 142   if (P_body_init_true)
 143     P_body = P_body_init;
 144   if (Q_body_init_true)
 145     Q_body = Q_body_init;
 146   if (R_body_init_true)
 147     R_body = R_body_init;
 148
 149   if (Phi_init_true)
 150     Phi = Phi_init;
 151   if (Theta_init_true)
 152     Theta = Theta_init;
 153   if (Psi_init_true)
 154     Psi = Psi_init;
 155
 156   if (U_body_init_true)
 157     U_body = U_body_init;
 158   if (V_body_init_true)
 159     V_body = V_body_init;
 160   if (W_body_init_true)
 161     W_body = W_body_init;
 162 }
 163
 164 void uiuc_defaults_inits ()
 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   old_flap_routine = false;
 199   icing_demo = false;
 200   outside_control = false;
 201   set_Long_trim = false;
 202   zero_Long_trim = false;
 203   elevator_step = false;
 204   elevator_singlet = false;
 205   elevator_doublet = false;
 206   elevator_input = false;
 207   elevator_input_ntime = 0;
 208   aileron_input = false;
 209   aileron_input_ntime = 0;
 210   rudder_input = false;
 211   rudder_input_ntime = 0;
 212   pilot_elev_no = false;
 213   pilot_elev_no_check = false;
 214   pilot_ail_no = false;
 215   pilot_ail_no_check = false;
 216   pilot_rud_no = false;
 217   pilot_rud_no_check = false;
 218   use_flaps = false;
 219   use_spoilers = false;
 220   flap_pos_input = false;
 221   flap_pos_input_ntime = 0;
 222   use_elevator_sas_type1 = false;
 223   use_elevator_sas_max = false;
 224   use_elevator_sas_min = false;
 225   use_elevator_stick_gain = false;
 226   use_aileron_sas_type1 = false;
 227   use_aileron_sas_max = false;
 228   use_aileron_stick_gain = false;
 229   use_rudder_sas_type1 = false;
 230   use_rudder_sas_max = false;
 231   use_rudder_stick_gain = false;
 232   simpleSingleModel = false;
 233   Throttle_pct_input = false;
 234   Throttle_pct_input_ntime = 0;
 235   gyroForce_Q_body = false;
 236   gyroForce_R_body = false;
 237   b_slipstreamEffects = false;
 238   Xp_input = false;
 239   Xp_input_ntime = 0;
 240   Zp_input = false;
 241   Zp_input_ntime = 0;
 242   Mp_input = false;
 243   Mp_input_ntime = 0;
 244   b_CLK = false;
 245   //  gear_model[MAX_GEAR]
 246   memset(gear_model,false,MAX_GEAR*sizeof(gear_model[0]));
 247   use_gear = false;
 248   // start with gear down if it is ultimately used
 249   gear_pos_norm = 1;
 250   ice_model = false;
 251   ice_on = false;
 252   beta_model = false;
 253   //  bootTrue[20] = 0;
 254   memset(bootTrue,false,20*sizeof(gear_model[0]));
 255   eta_from_file = false;
 256   eta_wing_left_input = false;
 257   eta_wing_right_input = false;
 258   eta_tail_input = false;
 259   demo_eps_alpha_max = false;
 260   demo_eps_pitch_max = false;
 261   demo_eps_pitch_min = false;
 262   demo_eps_roll_max = false;
 263   demo_eps_thrust_min = false;
 264   demo_eps_airspeed_max = false;
 265   demo_eps_airspeed_min = false;
 266   demo_eps_flap_max = false;
 267   demo_boot_cycle_tail = false;
 268   demo_boot_cycle_wing_left = false;
 269   demo_boot_cycle_wing_right = false;
 270   demo_eps_pitch_input = false;
 271   tactilefadef = false;
 272   demo_ap_pah_on = false;
 273   demo_ap_Theta_ref_deg = false;
 274   demo_tactile = false;
 275   demo_ice_tail = false;
 276   demo_ice_left = false;
 277   demo_ice_right = false;
 278   flapper_model = false;
 279   ignore_unknown_keywords = false;
 280   pilot_throttle_no = false;
 281   Dx_cg = 0.0;
 282   Dy_cg = 0.0;
 283   Dz_cg = 0.0;
 284
 285
 286   // Calculates the local velocity (V_north, V_east, V_down) from the body
 287   // velocities.
 288   // Called from uiuc_local_vel_init which is called from ls_step.
 289   // Used during initialization (while Simtime=0)
 290 }
 291
 292 void uiuc_vel_init ()
 293 {
 294   if (U_body_init_true && V_body_init_true && W_body_init_true)
 295     {
 296   double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
 297
 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;
 308
 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;
 312
 313   V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
 314     }
 315   // Initializes the UIUC aircraft model.
 316   // Called once from uiuc_init_2_wrapper
 317 }
 318
 319 void uiuc_init_aeromodel ()
 320 {
 321   SGPath path(globals->get_fg_root());
 322   path.append(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
 327 }
 328
 329 void uiuc_force_moment(double dt)
 330 {
 331   double qS = Dynamic_pressure * Sw;
 332   double qScbar = qS * cbar;
 333   double qSb = qS * bw;
 334
 335   uiuc_aerodeflections(dt);
 336   uiuc_coefficients(dt);
 337
 338   /* Calculate the forces */
 339   if (CX && CZ)
 340     {
 341       F_X_aero = CX * qS;
 342       F_Y_aero = CY * qS;
 343       F_Z_aero = CZ * qS;
 344     }
 345   else
 346     {
 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;
 353       F_Y_wind =  CY * qS;
 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;
 358
 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;
 363     }
 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;
 371
 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;
 381
 382   // adding in apparent mass in body axis X direction
 383   // F_X_aero += -(0.05 * Mass) * U_dot_body;
 384
 385
 386   if (Mass_appMass)
 387     F_Z_aero += -Mass_appMass * W_dot_body;
 388   if (I_xx_appMass)
 389     M_l_aero += -I_xx_appMass * P_dot_body;
 390   if (I_yy_appMass)
 391     M_m_aero += -I_yy_appMass * Q_dot_body;
 392   if (I_zz_appMass)
 393     M_n_aero += -I_zz_appMass * R_dot_body;
 394
 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;
 401
 402   // ornithopter support
 403   if (flapper_model)
 404     {
 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;
 409     }
 410
 411   // fog field update
 412    Fog = 0;
 413    if (fog_field)
 414      uiuc_fog();
 415
 416    double vis;
 417    if (Fog != 0)
 418    {
 419      vis = fgGetDouble("/environment/visibility-m");
 420      if (Fog > 0)
 421        vis /= 1.01;
 422      else
 423        vis *= 1.01;
 424      fgSetDouble("/environment/visibility-m", vis);
 425    }
 426
 427
 428   /* Send data on the network to the Glass Cockpit */
 429
 430    //  string input="";
 431
 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);
 442
 443    //  testarray.getHello();
 444    //  testarray.sendData(input);
 445
 446   /* End of Networking */
 447
 448 }
 449
 450 void uiuc_wind_routine()
 451 {
 452   uiuc_getwind();
 453 }
 454
 455 void uiuc_engine_routine()
 456 {
 457   uiuc_engine();
 458 }
 459
 460 void uiuc_gear_routine ()
 461 {
 462   uiuc_gear();
 463 }
 464
 465 void uiuc_record_routine(double dt)
 466 {
 467   if (trigger_last_time_step == 0 && trigger_on == 1) {
 468     if (trigger_toggle == 0)
 469       trigger_toggle = 1;
 470     else
 471       trigger_toggle = 0;
 472     trigger_num++;
 473     if (trigger_num % 2 != 0)
 474       trigger_counter++;
 475   }
 476
 477   if (Simtime >= recordStartTime)
 478     uiuc_recorder(dt);
 479
 480   trigger_last_time_step = trigger_on;
 481 }
 482
 483 void uiuc_network_recv_routine()
 484 {
 485   //if (use_uiuc_network)
 486     //uiuc_network(1);
 487 }
 488
 489 void uiuc_network_send_routine()
 490 {
 491   //if (use_uiuc_network)
 492     //uiuc_network(2);
 493 }
 494 //end uiuc_wrapper.cpp