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
  78 #ifndef FG_OLD_WEATHER
  79 #include <WeatherCM/FGLocalWeatherDatabase.h>
  80 #else
  81 #include <Weather/weather.hxx>
  82 #endif
  83
  84 #include "uiuc_aircraft.h"
  85 #include "uiuc_aircraftdir.h"
  86 #include "uiuc_coefficients.h"
  87 #include "uiuc_engine.h"
  88 #include "uiuc_gear.h"
  89 #include "uiuc_aerodeflections.h"
  90 #include "uiuc_recorder.h"
  91 #include "uiuc_menu.h"
  92 #include "uiuc_betaprobe.h"
  93 #include <FDM/LaRCsim/ls_generic.h>
  94 // #include "Main/simple_udp.h"
  95 #include "uiuc_fog.h" //321654
  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_vel_init ();
 108 extern "C" void uiuc_initial_init ();
 109
 110 AIRCRAFT *aircraft_ = new AIRCRAFT;
 111 AIRCRAFTDIR *aircraftdir_ = new AIRCRAFTDIR;
 112
 113 // SendArray testarray(4950);
 114
 115 /* Convert float to string */
 116 //string ftoa(double in)
 117 //{
 118 //  static char temp[20];
 119 //  sprintf(temp,"%g",in);
 120 //  return (string)temp;
 121 //}
 122
 123 void uiuc_initial_init ()
 124 {
 125   if (P_body_init_true)
 126     P_body = P_body_init;
 127   if (Q_body_init_true)
 128     Q_body = Q_body_init;
 129   if (R_body_init_true)
 130     R_body = R_body_init;
 131
 132   if (Phi_init_true)
 133     Phi = Phi_init;
 134   if (Theta_init_true)
 135     Theta = Theta_init;
 136   if (Psi_init_true)
 137     Psi = Psi_init;
 138
 139   if (U_body_init_true)
 140     U_body = U_body_init;
 141   if (V_body_init_true)
 142     V_body = V_body_init;
 143   if (W_body_init_true)
 144     W_body = W_body_init;
 145
 146 }
 147
 148 void uiuc_vel_init ()
 149 {
 150   if (U_body_init_true && V_body_init_true && W_body_init_true)
 151     {
 152   double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
 153
 154   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);
 155   cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
 156   cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
 157   cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
 158   cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
 159   cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
 160   cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
 161   cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
 162   cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
 163   cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
 164
 165   V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
 166   V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
 167   V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
 168
 169   V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
 170     }
 171 }
 172
 173 void uiuc_init_aeromodel ()
 174 {
 175   SGPath path(globals->get_fg_root());
 176   path.append(aircraft_dir);
 177   path.append("aircraft.dat");
 178   cout << "We are using "<< path.str() << endl;
 179   uiuc_initializemaps(); // Initialize the <string,int> maps
 180   uiuc_menu(path.str());   // Read the specified aircraft file
 181 }
 182
 183 void uiuc_force_moment(double dt)
 184 {
 185   double qS = Dynamic_pressure * Sw;
 186   double qScbar = qS * cbar;
 187   double qSb = qS * bw;
 188
 189   uiuc_aerodeflections(dt);
 190   uiuc_coefficients();
 191
 192   /* Calculate the wind axis forces */
 193   if (CX && CZ)
 194     {
 195       CD = -CX * cos(Alpha) - CZ * sin(Alpha);
 196       CL =  CX * sin(Alpha) - CZ * cos(Alpha);
 197     }
 198   F_X_wind = -1 * CD * qS;
 199   F_Y_wind = CY * qS;
 200   F_Z_wind = -1 * CL * qS;
 201
 202   /* wind-axis to body-axis transformation */
 203   F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
 204   F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
 205   F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
 206
 207   /* Moment calculations */
 208   M_l_aero = Cl * qSb;
 209   M_m_aero = Cm * qScbar;
 210   M_n_aero = Cn * qSb;
 211
 212   /* Call flight data recorder */
 213   //  if (Simtime >= recordStartTime)
 214   //      uiuc_recorder(dt);
 215
 216
 217   // fog field update
 218    Fog = 0;
 219    if (fog_field)
 220      uiuc_fog();
 221
 222    double vis;
 223    if (Fog != 0)
 224    {
 225  #ifndef FG_OLD_WEATHER
 226      vis = WeatherDatabase->getWeatherVisibility();
 227      if (Fog > 0)
 228        vis /= 1.01;
 229      else
 230        vis *= 1.01;
 231      WeatherDatabase->setWeatherVisibility( vis );
 232  #else
 233      vis = current_weather->get_visibility();
 234      if (Fog > 0)
 235        vis /= 1.01;
 236      else
 237        vis *= 1.01;
 238      current_weather->set_visibility( vis );
 239  #endif
 240    }
 241
 242
 243   /* Send data on the network to the Glass Cockpit */
 244
 245    //  string input="";
 246
 247    //  input += " stick_right " + ftoa(Lat_control);
 248    //  input += " rudder_left " + ftoa(-Rudder_pedal);
 249    //  input += " stick_forward " + ftoa(Long_control);
 250    //  input += " stick_trim_forward " + ftoa(Long_trim);
 251    //  input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
 252    //  input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
 253    //  input += " vehicle_speed " + ftoa(V_rel_wind);
 254    //  input += " throttle_forward " + ftoa(Throttle_pct);
 255    //  input += " altitude " + ftoa(Altitude);
 256    //  input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
 257
 258    //  testarray.getHello();
 259    //  testarray.sendData(input);
 260
 261   /* End of Networking */
 262
 263 }
 264
 265 void uiuc_engine_routine()
 266 {
 267   uiuc_engine();
 268 }
 269
 270 void uiuc_gear_routine ()
 271 {
 272   uiuc_gear();
 273 }
 274
 275 void uiuc_record_routine(double dt)
 276 {
 277   if (Simtime >= recordStartTime)
 278     uiuc_recorder(dt);
 279 }
 280 //end uiuc_wrapper.cpp