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