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 <iostream>
  86 #include <cstring>
  87
  88 #include <simgear/compiler.h>
  89 #include <simgear/misc/sg_path.hxx>
  90 #include <Main/fg_props.hxx>
  91
  92 #include "uiuc_aircraft.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 extern "C" void uiuc_initial_init ();
 108 extern "C" void uiuc_defaults_inits ();
 109 extern "C" void uiuc_vel_init ();
 110 extern "C" void uiuc_init_aeromodel ();
 111 extern "C" void uiuc_force_moment(double dt);
 112 extern "C" void uiuc_engine_routine();
 113 extern "C" void uiuc_wind_routine();
 114 extern "C" void uiuc_gear_routine();
 115 extern "C" void uiuc_record_routine(double dt);
 116 extern "C" void uiuc_network_recv_routine();
 117 extern "C" void uiuc_network_send_routine();
 118
 119 AIRCRAFT *aircraft_ = 0;
 120
 121 // SendArray testarray(4950);
 122
 123 /* Convert float to string */
 124 //string ftoa(double in)
 125 //{
 126 //  static char temp[20];
 127 //  sprintf(temp,"%g",in);
 128 //  return (string)temp;
 129 //}
 130
 131 void uiuc_initial_init ()
 132 {
 133   // This function is called from uiuc_init_2_wrapper (uiuc_aero.c in LaRCsim)
 134   // which is called from ls_step and ls_model.
 135   // Apply brute force initializations, which override unwanted changes
 136   // performed by LaRCsim.
 137   // Used during initialization (while Simtime=0).
 138   if (P_body_init_true)
 139     P_body = P_body_init;
 140   if (Q_body_init_true)
 141     Q_body = Q_body_init;
 142   if (R_body_init_true)
 143     R_body = R_body_init;
 144
 145   if (Phi_init_true)
 146     Phi = Phi_init;
 147   if (Theta_init_true)
 148     Theta = Theta_init;
 149   if (Psi_init_true)
 150     Psi = Psi_init;
 151
 152   if (U_body_init_true)
 153     U_body = U_body_init;
 154   if (V_body_init_true)
 155     V_body = V_body_init;
 156   if (W_body_init_true)
 157     W_body = W_body_init;
 158 }
 159
 160 void uiuc_defaults_inits ()
 161 {
 162   if (aircraft_ == 0)
 163     aircraft_ = new AIRCRAFT;
 164
 165   // set defaults and initialize (called once from uiuc_init_2_wrapper)
 166
 167   //fog inits
 168   fog_field = 0;
 169   fog_segments = 0;
 170   fog_point_index = -1;
 171   fog_time = NULL;
 172   fog_value = NULL;
 173   fog_next_time = 0.0;
 174   fog_current_segment = 0;
 175   Fog = 0;
 176
 177   use_V_rel_wind_2U = false;
 178   nondim_rate_V_rel_wind = false;
 179   use_abs_U_body_2U = false;
 180   use_dyn_on_speed_curve1 = false;
 181   use_Alpha_dot_on_speed = false;
 182   use_gamma_horiz_on_speed = false;
 183   b_downwashMode = false;
 184   P_body_init_true = false;
 185   Q_body_init_true = false;
 186   R_body_init_true = false;
 187   Phi_init_true = false;
 188   Theta_init_true = false;
 189   Psi_init_true = false;
 190   Alpha_init_true = false;
 191   Beta_init_true = false;
 192   U_body_init_true = false;
 193   V_body_init_true = false;
 194   W_body_init_true = false;
 195   trim_case_2 = false;
 196   use_uiuc_network = 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_alh_on = false;
 272   demo_ap_Theta_ref = false;
 273   demo_ap_alt_ref = 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   ap_pah_on = 0;
 285   ap_alh_on = 0;
 286
 287 }
 288
 289 void uiuc_vel_init ()
 290 {
 291   // Calculates the local velocity (V_north, V_east, V_down) from the body
 292   // velocities.
 293   // Called from uiuc_local_vel_init which is called from ls_step.
 294   // Used during initialization (while Simtime=0)
 295   if (U_body_init_true && V_body_init_true && W_body_init_true)
 296     {
 297   double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
 298
 299   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);
 300   cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
 301   cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
 302   cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
 303   cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
 304   cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
 305   cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
 306   cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
 307   cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
 308   cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
 309
 310   V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
 311   V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
 312   V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
 313
 314   V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
 315     }
 316 }
 317
 318 void uiuc_init_aeromodel ()
 319 {
 320   // Initializes the UIUC aircraft model.
 321   // Called once from uiuc_init_2_wrapper
 322   SGPath path(globals->get_fg_root());
 323   path.append(fgGetString("/sim/aircraft-dir"));
 324   path.append("aircraft.dat");
 325   std::cout << "We are using "<< path.str() << std::endl;
 326   uiuc_initializemaps(); // Initialize the <string,int> maps
 327   uiuc_menu(path.str());   // Read the specified aircraft file
 328 }
 329
 330 void uiuc_force_moment(double dt)
 331 {
 332   double qS = Dynamic_pressure * Sw;
 333   double qScbar = qS * cbar;
 334   double qSb = qS * bw;
 335
 336   uiuc_aerodeflections(dt);
 337   uiuc_coefficients(dt);
 338
 339   /* Calculate the forces */
 340   if (CX && CZ)
 341     {
 342       F_X_aero = CX * qS;
 343       F_Y_aero = CY * qS;
 344       F_Z_aero = CZ * qS;
 345     }
 346   else
 347     {
 348       // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
 349       // hence Cos_beta * Cos_beta term included.
 350       // Same thing is done w/ moments below.
 351       // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
 352       // that should not be the case.  See FGFS notes 021105
 353       F_X_wind = -CD * qS  * Cos_beta * Cos_beta;
 354       F_Y_wind =  CY * qS;
 355       F_Z_wind = -CL * qS  * Cos_beta * Cos_beta;
 356       // F_X_wind = -CD * qS  * Cos_beta * Cos_beta;
 357       // F_Y_wind =  CY * qS  * Cos_beta * Cos_beta;
 358       // F_Z_wind = -CL * qS  * Cos_beta * Cos_beta;
 359
 360       // wind-axis to body-axis transformation
 361       F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
 362       F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
 363       F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
 364     }
 365   // Moment calculations
 366   M_l_aero = Cl * qSb    ;
 367   M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
 368   M_n_aero = Cn * qSb    ;
 369   // M_l_aero = Cl * qSb    * Cos_beta * Cos_beta;
 370   // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
 371   // M_n_aero = Cn * qSb    * Cos_beta * Cos_beta;
 372
 373   // Adding in apparent mass effects
 374   if (Mass_appMass_ratio)
 375     F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
 376   if (I_xx_appMass_ratio)
 377     M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
 378   if (I_yy_appMass_ratio)
 379     M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
 380   if (I_zz_appMass_ratio)
 381     M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
 382
 383   // adding in apparent mass in body axis X direction
 384   // F_X_aero += -(0.05 * Mass) * U_dot_body;
 385
 386
 387   if (Mass_appMass)
 388     F_Z_aero += -Mass_appMass * W_dot_body;
 389   if (I_xx_appMass)
 390     M_l_aero += -I_xx_appMass * P_dot_body;
 391   if (I_yy_appMass)
 392     M_m_aero += -I_yy_appMass * Q_dot_body;
 393   if (I_zz_appMass)
 394     M_n_aero += -I_zz_appMass * R_dot_body;
 395
 396   // gyroscopic moments
 397   // engineOmega is positive when rotation is ccw when viewed from the front
 398   if (gyroForce_Q_body)
 399     M_n_aero +=  polarInertia * engineOmega * Q_body;
 400   if (gyroForce_R_body)
 401     M_m_aero += -polarInertia * engineOmega * R_body;
 402
 403   // ornithopter support
 404   if (flapper_model)
 405     {
 406       uiuc_get_flapper(dt);
 407       F_X_aero += F_X_aero_flapper;
 408       F_Z_aero += F_Z_aero_flapper;
 409       M_m_aero += flapper_Moment;
 410     }
 411
 412   // fog field update
 413    Fog = 0;
 414    if (fog_field)
 415      uiuc_fog();
 416
 417    double vis;
 418    if (Fog != 0)
 419    {
 420      vis = fgGetDouble("/environment/visibility-m");
 421      if (Fog > 0)
 422        vis /= 1.01;
 423      else
 424        vis *= 1.01;
 425      fgSetDouble("/environment/visibility-m", vis);
 426    }
 427
 428
 429   /* Send data on the network to the Glass Cockpit */
 430
 431    //  string input="";
 432
 433    //  input += " stick_right " + ftoa(Lat_control);
 434    //  input += " rudder_left " + ftoa(-Rudder_pedal);
 435    //  input += " stick_forward " + ftoa(Long_control);
 436    //  input += " stick_trim_forward " + ftoa(Long_trim);
 437    //  input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
 438    //  input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
 439    //  input += " vehicle_speed " + ftoa(V_rel_wind);
 440    //  input += " throttle_forward " + ftoa(Throttle_pct);
 441    //  input += " altitude " + ftoa(Altitude);
 442    //  input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
 443
 444    //  testarray.getHello();
 445    //  testarray.sendData(input);
 446
 447   /* End of Networking */
 448
 449 }
 450
 451 void uiuc_wind_routine()
 452 {
 453   uiuc_getwind();
 454 }
 455
 456 void uiuc_engine_routine()
 457 {
 458   uiuc_engine();
 459 }
 460
 461 void uiuc_gear_routine ()
 462 {
 463   uiuc_gear();
 464 }
 465
 466 void uiuc_record_routine(double dt)
 467 {
 468   if (trigger_last_time_step == 0 && trigger_on == 1) {
 469     if (trigger_toggle == 0)
 470       trigger_toggle = 1;
 471     else
 472       trigger_toggle = 0;
 473     trigger_num++;
 474     if (trigger_num % 2 != 0)
 475       trigger_counter++;
 476   }
 477
 478   if (Simtime >= recordStartTime)
 479     uiuc_recorder(dt);
 480
 481   trigger_last_time_step = trigger_on;
 482 }
 483
 484 void uiuc_network_recv_routine()
 485 {
 486   //if (use_uiuc_network)
 487   //  uiuc_network(1);
 488 }
 489
 490 void uiuc_network_send_routine()
 491 {
 492   //if (use_uiuc_network)
 493   //  uiuc_network(2);
 494 }
 495 //end uiuc_wrapper.cpp