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