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 #ifdef HAVE_CONFIG_H
  79 #  include <config.h>
  80 #endif
  81
  82 #include <simgear/compiler.h>
  83 #include <simgear/misc/sg_path.hxx>
  84 #include <Aircraft/aircraft.hxx>
  85 #include <Main/fg_props.hxx>
  86
  87 #include "uiuc_aircraft.h"
  88 #include "uiuc_aircraftdir.h"
  89 #include "uiuc_coefficients.h"
  90 #include "uiuc_engine.h"
  91 #include "uiuc_gear.h"
  92 #include "uiuc_aerodeflections.h"
  93 #include "uiuc_recorder.h"
  94 #include "uiuc_menu.h"
  95 #include "uiuc_betaprobe.h"
  96 #include <FDM/LaRCsim/ls_generic.h>
  97 //#include "Main/simple_udp.h"
  98 #include "uiuc_fog.h" //321654
  99 //#include "uiuc_network.h"
 100 //#include "uiuc_get_flapper.h"
 101
 102 #if !defined (SG_HAVE_NATIVE_SGI_COMPILERS)
 103 SG_USING_STD(cout);
 104 SG_USING_STD(endl);
 105 #endif
 106
 107 extern "C" void uiuc_init_aeromodel ();
 108 extern "C" void uiuc_force_moment(double dt);
 109 extern "C" void uiuc_engine_routine();
 110 extern "C" void uiuc_gear_routine();
 111 extern "C" void uiuc_record_routine(double dt);
 112 //extern "C" void uiuc_network_routine();
 113 extern "C" void uiuc_vel_init ();
 114 extern "C" void uiuc_initial_init ();
 115
 116 AIRCRAFT *aircraft_ = new AIRCRAFT;
 117 AIRCRAFTDIR *aircraftdir_ = new AIRCRAFTDIR;
 118
 119 // SendArray testarray(4950);
 120
 121 /* Convert float to string */
 122 //string ftoa(double in)
 123 //{
 124 //  static char temp[20];
 125 //  sprintf(temp,"%g",in);
 126 //  return (string)temp;
 127 //}
 128
 129 void uiuc_initial_init ()
 130 {
 131   if (P_body_init_true)
 132     P_body = P_body_init;
 133   if (Q_body_init_true)
 134     Q_body = Q_body_init;
 135   if (R_body_init_true)
 136     R_body = R_body_init;
 137
 138   if (Phi_init_true)
 139     Phi = Phi_init;
 140   if (Theta_init_true)
 141     Theta = Theta_init;
 142   if (Psi_init_true)
 143     Psi = Psi_init;
 144
 145   if (U_body_init_true)
 146     U_body = U_body_init;
 147   if (V_body_init_true)
 148     V_body = V_body_init;
 149   if (W_body_init_true)
 150     W_body = W_body_init;
 151
 152 }
 153
 154 void uiuc_vel_init ()
 155 {
 156   if (U_body_init_true && V_body_init_true && W_body_init_true)
 157     {
 158   double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
 159
 160   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);
 161   cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
 162   cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
 163   cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
 164   cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
 165   cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
 166   cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
 167   cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
 168   cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
 169   cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
 170
 171   V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
 172   V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
 173   V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
 174
 175   V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
 176     }
 177 }
 178
 179 void uiuc_init_aeromodel ()
 180 {
 181   SGPath path(globals->get_fg_root());
 182   path.append(aircraft_dir);
 183   path.append("aircraft.dat");
 184   cout << "We are using "<< path.str() << endl;
 185   uiuc_initializemaps(); // Initialize the <string,int> maps
 186   uiuc_menu(path.str());   // Read the specified aircraft file
 187 }
 188
 189 void uiuc_force_moment(double dt)
 190 {
 191   double qS = Dynamic_pressure * Sw;
 192   double qScbar = qS * cbar;
 193   double qSb = qS * bw;
 194
 195   uiuc_aerodeflections(dt);
 196   uiuc_coefficients(dt);
 197   //if (flapper_model)
 198   //  {
 199   //    uiuc_get_flapper(dt);
 200   //  }
 201
 202   /* Calculate the forces */
 203   if (CX && CZ)
 204     {
 205       F_X_aero = CX * qS;
 206       F_Y_aero = CY * qS;
 207       F_Z_aero = CZ * qS;
 208     }
 209   else
 210     {
 211       // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
 212       // hence Cos_beta * Cos_beta term included.
 213       // Same thing is done w/ moments below.
 214       // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
 215       // that should not be the case.  See FGFS notes 021105
 216       F_X_wind = -CD * qS  * Cos_beta * Cos_beta;
 217       F_Y_wind =  CY * qS;
 218       F_Z_wind = -CL * qS  * Cos_beta * Cos_beta;
 219       // F_X_wind = -CD * qS  * Cos_beta * Cos_beta;
 220       // F_Y_wind =  CY * qS  * Cos_beta * Cos_beta;
 221       // F_Z_wind = -CL * qS  * Cos_beta * Cos_beta;
 222
 223       // wind-axis to body-axis transformation
 224       F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
 225       F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
 226       F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
 227     }
 228   // Moment calculations
 229   M_l_aero = Cl * qSb    ;
 230   M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
 231   M_n_aero = Cn * qSb    ;
 232   // M_l_aero = Cl * qSb    * Cos_beta * Cos_beta;
 233   // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
 234   // M_n_aero = Cn * qSb    * Cos_beta * Cos_beta;
 235
 236   // Adding in apparent mass effects
 237   if (Mass_appMass_ratio)
 238     F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
 239   if (I_xx_appMass_ratio)
 240     M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
 241   if (I_yy_appMass_ratio)
 242     M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
 243   if (I_zz_appMass_ratio)
 244     M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
 245
 246   if (Mass_appMass)
 247     F_Z_aero += -Mass_appMass * W_dot_body;
 248   if (I_xx_appMass)
 249     M_l_aero += -I_xx_appMass * P_dot_body;
 250   if (I_yy_appMass)
 251     M_m_aero += -I_yy_appMass * Q_dot_body;
 252   if (I_zz_appMass)
 253     M_n_aero += -I_zz_appMass * R_dot_body;
 254
 255   // gyroscopic moments
 256   // engineOmega is positive when rotation is ccw when viewed from the front
 257   if (gyroForce_Q_body)
 258     M_n_aero +=  polarInertia * engineOmega * Q_body;
 259   if (gyroForce_R_body)
 260     M_m_aero += -polarInertia * engineOmega * R_body;
 261
 262   // ornithopter support
 263   //if (flapper_model)
 264   //  {
 265   //    F_X_aero += F_X_aero_flapper;
 266   //    F_Z_aero += F_Z_aero_flapper;
 267   //    M_m_aero += flapper_Moment;
 268   //  }
 269
 270   // fog field update
 271    Fog = 0;
 272    if (fog_field)
 273      uiuc_fog();
 274
 275    double vis;
 276    if (Fog != 0)
 277    {
 278      vis = fgGetDouble("/environment/visibility-m");
 279      if (Fog > 0)
 280        vis /= 1.01;
 281      else
 282        vis *= 1.01;
 283      fgSetDouble("/environment/visibility-m", vis);
 284    }
 285
 286
 287   /* Send data on the network to the Glass Cockpit */
 288
 289    //  string input="";
 290
 291    //  input += " stick_right " + ftoa(Lat_control);
 292    //  input += " rudder_left " + ftoa(-Rudder_pedal);
 293    //  input += " stick_forward " + ftoa(Long_control);
 294    //  input += " stick_trim_forward " + ftoa(Long_trim);
 295    //  input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
 296    //  input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
 297    //  input += " vehicle_speed " + ftoa(V_rel_wind);
 298    //  input += " throttle_forward " + ftoa(Throttle_pct);
 299    //  input += " altitude " + ftoa(Altitude);
 300    //  input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
 301
 302    //  testarray.getHello();
 303    //  testarray.sendData(input);
 304
 305   /* End of Networking */
 306
 307 }
 308
 309 void uiuc_engine_routine()
 310 {
 311   uiuc_engine();
 312 }
 313
 314 void uiuc_gear_routine ()
 315 {
 316   uiuc_gear();
 317 }
 318
 319 void uiuc_record_routine(double dt)
 320 {
 321   if (Simtime >= recordStartTime)
 322     uiuc_recorder(dt);
 323 }
 324
 325 //void uiuc_network_routine()
 326 //{
 327 //  if (use_uiuc_network)
 328 //    uiuc_network(2);  //send data
 329 //}
 330 //end uiuc_wrapper.cpp