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
  97 #if !defined (SG_HAVE_NATIVE_SGI_COMPILERS)
  98 SG_USING_STD(cout);
  99 SG_USING_STD(endl);
 100 #endif
 101
 102 extern "C" void uiuc_init_aeromodel ();
 103 extern "C" void uiuc_force_moment(double dt);
 104 extern "C" void uiuc_engine_routine();
 105 extern "C" void uiuc_gear_routine();
 106 extern "C" void uiuc_record_routine(double dt);
 107 //extern "C" void uiuc_network_routine();
 108 extern "C" void uiuc_vel_init ();
 109 extern "C" void uiuc_initial_init ();
 110
 111 AIRCRAFT *aircraft_ = new AIRCRAFT;
 112 AIRCRAFTDIR *aircraftdir_ = new AIRCRAFTDIR;
 113
 114 // SendArray testarray(4950);
 115
 116 /* Convert float to string */
 117 //string ftoa(double in)
 118 //{
 119 //  static char temp[20];
 120 //  sprintf(temp,"%g",in);
 121 //  return (string)temp;
 122 //}
 123
 124 void uiuc_initial_init ()
 125 {
 126   if (P_body_init_true)
 127     P_body = P_body_init;
 128   if (Q_body_init_true)
 129     Q_body = Q_body_init;
 130   if (R_body_init_true)
 131     R_body = R_body_init;
 132
 133   if (Phi_init_true)
 134     Phi = Phi_init;
 135   if (Theta_init_true)
 136     Theta = Theta_init;
 137   if (Psi_init_true)
 138     Psi = Psi_init;
 139
 140   if (U_body_init_true)
 141     U_body = U_body_init;
 142   if (V_body_init_true)
 143     V_body = V_body_init;
 144   if (W_body_init_true)
 145     W_body = W_body_init;
 146
 147 }
 148
 149 void uiuc_vel_init ()
 150 {
 151   if (U_body_init_true && V_body_init_true && W_body_init_true)
 152     {
 153   double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
 154
 155   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);
 156   cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
 157   cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
 158   cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
 159   cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
 160   cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
 161   cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
 162   cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
 163   cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
 164   cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
 165
 166   V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
 167   V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
 168   V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
 169
 170   V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
 171     }
 172 }
 173
 174 void uiuc_init_aeromodel ()
 175 {
 176   SGPath path(globals->get_fg_root());
 177   path.append(aircraft_dir);
 178   path.append("aircraft.dat");
 179   cout << "We are using "<< path.str() << endl;
 180   uiuc_initializemaps(); // Initialize the <string,int> maps
 181   uiuc_menu(path.str());   // Read the specified aircraft file
 182 }
 183
 184 void uiuc_force_moment(double dt)
 185 {
 186   double qS = Dynamic_pressure * Sw;
 187   double qScbar = qS * cbar;
 188   double qSb = qS * bw;
 189
 190   uiuc_aerodeflections(dt);
 191   uiuc_coefficients();
 192
 193   /* Calculate the forces */
 194   if (CX && CZ)
 195     {
 196       F_X_aero = CX * qS;
 197       F_Y_aero = CY * qS;
 198       F_Z_aero = CZ * qS;
 199     }
 200   else
 201     {
 202       F_X_wind = -CD * qS  * Cos_beta * Cos_beta;
 203       F_Y_wind =  CY * qS  * Cos_beta * Cos_beta;
 204       F_Z_wind = -CL * qS  * Cos_beta * Cos_beta;
 205
 206       /* wind-axis to body-axis transformation */
 207       F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
 208       F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
 209       F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
 210     }
 211   /* Moment calculations */
 212   M_l_aero = Cl * qSb    * Cos_beta * Cos_beta;
 213   M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
 214   M_n_aero = Cn * qSb    * Cos_beta * Cos_beta;
 215
 216   /* Adding in apparent mass effects */
 217
 218   if (Mass_appMass_ratio)
 219     F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
 220   if (I_xx_appMass_ratio)
 221     M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
 222   if (I_yy_appMass_ratio)
 223     M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
 224   if (I_zz_appMass_ratio)
 225     M_m_aero += -(I_zz_appMass_ratio * I_yy) * R_dot_body;
 226
 227   if (Mass_appMass)
 228     F_Z_aero += -Mass_appMass * W_dot_body;
 229   if (I_xx_appMass)
 230     M_l_aero += -I_xx_appMass * P_dot_body;
 231   if (I_yy_appMass)
 232     M_m_aero += -I_yy_appMass * Q_dot_body;
 233   if (I_zz_appMass)
 234     M_m_aero += -I_zz_appMass * R_dot_body;
 235
 236   // fog field update
 237    Fog = 0;
 238    if (fog_field)
 239      uiuc_fog();
 240
 241    double vis;
 242    if (Fog != 0)
 243    {
 244      vis = fgGetDouble("/environment/visibility-m");
 245      if (Fog > 0)
 246        vis /= 1.01;
 247      else
 248        vis *= 1.01;
 249      fgSetDouble("/environment/visibility-m", vis);
 250    }
 251
 252
 253   /* Send data on the network to the Glass Cockpit */
 254
 255    //  string input="";
 256
 257    //  input += " stick_right " + ftoa(Lat_control);
 258    //  input += " rudder_left " + ftoa(-Rudder_pedal);
 259    //  input += " stick_forward " + ftoa(Long_control);
 260    //  input += " stick_trim_forward " + ftoa(Long_trim);
 261    //  input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
 262    //  input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
 263    //  input += " vehicle_speed " + ftoa(V_rel_wind);
 264    //  input += " throttle_forward " + ftoa(Throttle_pct);
 265    //  input += " altitude " + ftoa(Altitude);
 266    //  input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
 267
 268    //  testarray.getHello();
 269    //  testarray.sendData(input);
 270
 271   /* End of Networking */
 272
 273 }
 274
 275 void uiuc_engine_routine()
 276 {
 277   uiuc_engine();
 278 }
 279
 280 void uiuc_gear_routine ()
 281 {
 282   uiuc_gear();
 283 }
 284
 285 void uiuc_record_routine(double dt)
 286 {
 287   if (Simtime >= recordStartTime)
 288     uiuc_recorder(dt);
 289 }
 290
 291 //void uiuc_network_routine ()
 292 //{
 293 //  uiuc_network();
 294 //}
 295 //end uiuc_wrapper.cpp