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