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