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
  31 ----------------------------------------------------------------------
  32
  33  AUTHOR(S):    Bipin Sehgal       <bsehgal@uiuc.edu>
  34                Robert Deters      <rdeters@uiuc.edu>
  35                Glen Dimock        <dimock@uiuc.edu>
  36                David Megginson    <david@megginson.com>
  37
  38 ----------------------------------------------------------------------
  39
  40  VARIABLES:
  41
  42 ----------------------------------------------------------------------
  43
  44  INPUTS:       *
  45
  46 ----------------------------------------------------------------------
  47
  48  OUTPUTS:      *
  49
  50 ----------------------------------------------------------------------
  51
  52  CALLED BY:    *
  53
  54 ----------------------------------------------------------------------
  55
  56  CALLS TO:     *
  57
  58 ----------------------------------------------------------------------
  59
  60  COPYRIGHT:    (C) 2000 by Michael Selig
  61
  62  This program is free software; you can redistribute it and/or
  63  modify it under the terms of the GNU General Public License
  64  as published by the Free Software Foundation.
  65
  66  This program is distributed in the hope that it will be useful,
  67  but WITHOUT ANY WARRANTY; without even the implied warranty of
  68  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  69  GNU General Public License for more details.
  70
  71  You should have received a copy of the GNU General Public License
  72  along with this program; if not, write to the Free Software
  73  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
  74  USA or view http://www.gnu.org/copyleft/gpl.html.
  75
  76 **********************************************************************/
  77
  78 #include <simgear/compiler.h>
  79 #include <simgear/misc/sg_path.hxx>
  80 #include <Aircraft/aircraft.hxx>
  81 #include <Main/fg_props.hxx>
  82
  83 #include "uiuc_aircraft.h"
  84 #include "uiuc_aircraftdir.h"
  85 #include "uiuc_coefficients.h"
  86 #include "uiuc_engine.h"
  87 #include "uiuc_gear.h"
  88 #include "uiuc_aerodeflections.h"
  89 #include "uiuc_recorder.h"
  90 #include "uiuc_menu.h"
  91 #include "uiuc_betaprobe.h"
  92 #include <FDM/LaRCsim/ls_generic.h>
  93 //#include "Main/simple_udp.h"
  94 #include "uiuc_fog.h" //321654
  95 //#include "uiuc_network.h"
  96 //#include "uiuc_get_flapper.h"
  97
  98 #if !defined (SG_HAVE_NATIVE_SGI_COMPILERS)
  99 SG_USING_STD(cout);
 100 SG_USING_STD(endl);
 101 #endif
 102
 103 extern "C" void uiuc_init_aeromodel ();
 104 extern "C" void uiuc_force_moment(double dt);
 105 extern "C" void uiuc_engine_routine();
 106 extern "C" void uiuc_gear_routine();
 107 extern "C" void uiuc_record_routine(double dt);
 108 //extern "C" void uiuc_network_routine();
 109 extern "C" void uiuc_vel_init ();
 110 extern "C" void uiuc_initial_init ();
 111
 112 AIRCRAFT *aircraft_ = new AIRCRAFT;
 113 AIRCRAFTDIR *aircraftdir_ = new AIRCRAFTDIR;
 114
 115 // SendArray testarray(4950);
 116
 117 /* Convert float to string */
 118 //string ftoa(double in)
 119 //{
 120 //  static char temp[20];
 121 //  sprintf(temp,"%g",in);
 122 //  return (string)temp;
 123 //}
 124
 125 void uiuc_initial_init ()
 126 {
 127   if (P_body_init_true)
 128     P_body = P_body_init;
 129   if (Q_body_init_true)
 130     Q_body = Q_body_init;
 131   if (R_body_init_true)
 132     R_body = R_body_init;
 133
 134   if (Phi_init_true)
 135     Phi = Phi_init;
 136   if (Theta_init_true)
 137     Theta = Theta_init;
 138   if (Psi_init_true)
 139     Psi = Psi_init;
 140
 141   if (U_body_init_true)
 142     U_body = U_body_init;
 143   if (V_body_init_true)
 144     V_body = V_body_init;
 145   if (W_body_init_true)
 146     W_body = W_body_init;
 147
 148 }
 149
 150 void uiuc_vel_init ()
 151 {
 152   if (U_body_init_true && V_body_init_true && W_body_init_true)
 153     {
 154   double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
 155
 156   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);
 157   cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
 158   cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
 159   cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
 160   cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
 161   cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
 162   cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
 163   cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
 164   cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
 165   cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
 166
 167   V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
 168   V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
 169   V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
 170
 171   V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
 172     }
 173 }
 174
 175 void uiuc_init_aeromodel ()
 176 {
 177   SGPath path(globals->get_fg_root());
 178   path.append(aircraft_dir);
 179   path.append("aircraft.dat");
 180   cout << "We are using "<< path.str() << endl;
 181   uiuc_initializemaps(); // Initialize the <string,int> maps
 182   uiuc_menu(path.str());   // Read the specified aircraft file
 183 }
 184
 185 void uiuc_force_moment(double dt)
 186 {
 187   double qS = Dynamic_pressure * Sw;
 188   double qScbar = qS * cbar;
 189   double qSb = qS * bw;
 190
 191   uiuc_aerodeflections(dt);
 192   uiuc_coefficients(dt);
 193   //if (flapper_model)
 194   //  {
 195   //    uiuc_get_flapper(dt);
 196   //  }
 197
 198   /* Calculate the forces */
 199   if (CX && CZ)
 200     {
 201       F_X_aero = CX * qS;
 202       F_Y_aero = CY * qS;
 203       F_Z_aero = CZ * qS;
 204     }
 205   else
 206     {
 207       // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
 208       // hence Cos_beta * Cos_beta term included.
 209       // Same thing is done w/ moments below.
 210       // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
 211       // that should not be the case.  See FGFS notes 021105
 212       F_X_wind = -CD * qS  * Cos_beta * Cos_beta;
 213       F_Y_wind =  CY * qS;
 214       F_Z_wind = -CL * qS  * Cos_beta * Cos_beta;
 215       // F_X_wind = -CD * qS  * Cos_beta * Cos_beta;
 216       // F_Y_wind =  CY * qS  * Cos_beta * Cos_beta;
 217       // F_Z_wind = -CL * qS  * Cos_beta * Cos_beta;
 218
 219       // wind-axis to body-axis transformation
 220       F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
 221       F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
 222       F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
 223     }
 224   // Moment calculations
 225   M_l_aero = Cl * qSb    ;
 226   M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
 227   M_n_aero = Cn * qSb    ;
 228   // M_l_aero = Cl * qSb    * Cos_beta * Cos_beta;
 229   // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
 230   // M_n_aero = Cn * qSb    * Cos_beta * Cos_beta;
 231
 232   // Adding in apparent mass effects
 233   if (Mass_appMass_ratio)
 234     F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
 235   if (I_xx_appMass_ratio)
 236     M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
 237   if (I_yy_appMass_ratio)
 238     M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
 239   if (I_zz_appMass_ratio)
 240     M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
 241
 242   if (Mass_appMass)
 243     F_Z_aero += -Mass_appMass * W_dot_body;
 244   if (I_xx_appMass)
 245     M_l_aero += -I_xx_appMass * P_dot_body;
 246   if (I_yy_appMass)
 247     M_m_aero += -I_yy_appMass * Q_dot_body;
 248   if (I_zz_appMass)
 249     M_n_aero += -I_zz_appMass * R_dot_body;
 250
 251   // gyroscopic moments
 252   // engineOmega is positive when rotation is ccw when viewed from the front
 253   if (gyroForce_Q_body)
 254     M_n_aero +=  polarInertia * engineOmega * Q_body;
 255   if (gyroForce_R_body)
 256     M_m_aero += -polarInertia * engineOmega * R_body;
 257
 258   // ornithopter support
 259   //if (flapper_model)
 260   //  {
 261   //    F_X_aero += F_X_aero_flapper;
 262   //    F_Z_aero += F_Z_aero_flapper;
 263   //    M_m_aero += flapper_Moment;
 264   //  }
 265
 266   // fog field update
 267    Fog = 0;
 268    if (fog_field)
 269      uiuc_fog();
 270
 271    double vis;
 272    if (Fog != 0)
 273    {
 274      vis = fgGetDouble("/environment/visibility-m");
 275      if (Fog > 0)
 276        vis /= 1.01;
 277      else
 278        vis *= 1.01;
 279      fgSetDouble("/environment/visibility-m", vis);
 280    }
 281
 282
 283   /* Send data on the network to the Glass Cockpit */
 284
 285    //  string input="";
 286
 287    //  input += " stick_right " + ftoa(Lat_control);
 288    //  input += " rudder_left " + ftoa(-Rudder_pedal);
 289    //  input += " stick_forward " + ftoa(Long_control);
 290    //  input += " stick_trim_forward " + ftoa(Long_trim);
 291    //  input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
 292    //  input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
 293    //  input += " vehicle_speed " + ftoa(V_rel_wind);
 294    //  input += " throttle_forward " + ftoa(Throttle_pct);
 295    //  input += " altitude " + ftoa(Altitude);
 296    //  input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
 297
 298    //  testarray.getHello();
 299    //  testarray.sendData(input);
 300
 301   /* End of Networking */
 302
 303 }
 304
 305 void uiuc_engine_routine()
 306 {
 307   uiuc_engine();
 308 }
 309
 310 void uiuc_gear_routine ()
 311 {
 312   uiuc_gear();
 313 }
 314
 315 void uiuc_record_routine(double dt)
 316 {
 317   if (Simtime >= recordStartTime)
 318     uiuc_recorder(dt);
 319 }
 320
 321 //void uiuc_network_routine()
 322 //{
 323 //  if (use_uiuc_network)
 324 //    uiuc_network(2);  //send data
 325 //}
 326 //end uiuc_wrapper.cpp