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                12/11/2002   (RD) Divided uiuc_network_routine into
  31                             uiuc_network_recv_routine and
  32                             uiuc_network_send_routine
  33                03/16/2003   (RD) Added trigger lines in recorder area
  34
  35 ----------------------------------------------------------------------
  36
  37  AUTHOR(S):    Bipin Sehgal       <bsehgal@uiuc.edu>
  38                Robert Deters      <rdeters@uiuc.edu>
  39                Glen Dimock        <dimock@uiuc.edu>
  40                David Megginson    <david@megginson.com>
  41
  42 ----------------------------------------------------------------------
  43
  44  VARIABLES:
  45
  46 ----------------------------------------------------------------------
  47
  48  INPUTS:       *
  49
  50 ----------------------------------------------------------------------
  51
  52  OUTPUTS:      *
  53
  54 ----------------------------------------------------------------------
  55
  56  CALLED BY:    *
  57
  58 ----------------------------------------------------------------------
  59
  60  CALLS TO:     *
  61
  62 ----------------------------------------------------------------------
  63
  64  COPYRIGHT:    (C) 2000 by Michael Selig
  65
  66  This program is free software; you can redistribute it and/or
  67  modify it under the terms of the GNU General Public License
  68  as published by the Free Software Foundation.
  69
  70  This program is distributed in the hope that it will be useful,
  71  but WITHOUT ANY WARRANTY; without even the implied warranty of
  72  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  73  GNU General Public License for more details.
  74
  75  You should have received a copy of the GNU General Public License
  76  along with this program; if not, write to the Free Software
  77  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307,
  78  USA or view http://www.gnu.org/copyleft/gpl.html.
  79
  80 **********************************************************************/
  81
  82 #ifdef HAVE_CONFIG_H
  83 #  include <config.h>
  84 #endif
  85
  86 #include <simgear/compiler.h>
  87 #include <simgear/misc/sg_path.hxx>
  88 #include <Aircraft/aircraft.hxx>
  89 #include <Main/fg_props.hxx>
  90
  91 #include "uiuc_aircraft.h"
  92 #include "uiuc_aircraftdir.h"
  93 #include "uiuc_coefficients.h"
  94 #include "uiuc_getwind.h"
  95 #include "uiuc_engine.h"
  96 #include "uiuc_gear.h"
  97 #include "uiuc_aerodeflections.h"
  98 #include "uiuc_recorder.h"
  99 #include "uiuc_menu.h"
 100 #include "uiuc_betaprobe.h"
 101 #include <FDM/LaRCsim/ls_generic.h>
 102 //#include "Main/simple_udp.h"
 103 #include "uiuc_fog.h" //321654
 104 //#include "uiuc_network.h"
 105 #include "uiuc_get_flapper.h"
 106
 107 SG_USING_STD(cout);
 108 SG_USING_STD(endl);
 109
 110 extern "C" void uiuc_initial_init ();
 111 extern "C" void uiuc_vel_init ();
 112 extern "C" void uiuc_init_aeromodel ();
 113 extern "C" void uiuc_force_moment(double dt);
 114 extern "C" void uiuc_engine_routine();
 115 extern "C" void uiuc_wind_routine();
 116 extern "C" void uiuc_gear_routine();
 117 extern "C" void uiuc_record_routine(double dt);
 118 extern "C" void uiuc_network_recv_routine();
 119 extern "C" void uiuc_network_send_routine();
 120
 121 AIRCRAFT *aircraft_ = new AIRCRAFT;
 122 AIRCRAFTDIR *aircraftdir_ = new AIRCRAFTDIR;
 123
 124 // SendArray testarray(4950);
 125
 126 /* Convert float to string */
 127 //string ftoa(double in)
 128 //{
 129 //  static char temp[20];
 130 //  sprintf(temp,"%g",in);
 131 //  return (string)temp;
 132 //}
 133
 134 void uiuc_initial_init ()
 135 {
 136   if (P_body_init_true)
 137     P_body = P_body_init;
 138   if (Q_body_init_true)
 139     Q_body = Q_body_init;
 140   if (R_body_init_true)
 141     R_body = R_body_init;
 142
 143   if (Phi_init_true)
 144     Phi = Phi_init;
 145   if (Theta_init_true)
 146     Theta = Theta_init;
 147   if (Psi_init_true)
 148     Psi = Psi_init;
 149
 150   if (U_body_init_true)
 151     U_body = U_body_init;
 152   if (V_body_init_true)
 153     V_body = V_body_init;
 154   if (W_body_init_true)
 155     W_body = W_body_init;
 156
 157 }
 158
 159 void uiuc_vel_init ()
 160 {
 161   if (U_body_init_true && V_body_init_true && W_body_init_true)
 162     {
 163   double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33;
 164
 165   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);
 166   cof11 = T_local_to_body_22 * T_local_to_body_33 - T_local_to_body_23 * T_local_to_body_32;
 167   cof12 = T_local_to_body_23 * T_local_to_body_31 - T_local_to_body_21 * T_local_to_body_33;
 168   cof13 = T_local_to_body_21 * T_local_to_body_32 - T_local_to_body_22 * T_local_to_body_31;
 169   cof21 = T_local_to_body_13 * T_local_to_body_32 - T_local_to_body_12 * T_local_to_body_33;
 170   cof22 = T_local_to_body_11 * T_local_to_body_33 - T_local_to_body_13 * T_local_to_body_31;
 171   cof23 = T_local_to_body_12 * T_local_to_body_31 - T_local_to_body_11 * T_local_to_body_32;
 172   cof31 = T_local_to_body_12 * T_local_to_body_23 - T_local_to_body_13 * T_local_to_body_22;
 173   cof32 = T_local_to_body_13 * T_local_to_body_21 - T_local_to_body_11 * T_local_to_body_23;
 174   cof33 = T_local_to_body_11 * T_local_to_body_22 - T_local_to_body_12 * T_local_to_body_21;
 175
 176   V_north = (cof11*U_body+cof21*V_body+cof31*W_body)/det_T_l_to_b;
 177   V_east_rel_ground = (cof12*U_body+cof22*V_body+cof32*W_body)/det_T_l_to_b;
 178   V_down = (cof13*U_body+cof23*V_body+cof33*W_body)/det_T_l_to_b;
 179
 180   V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
 181     }
 182 }
 183
 184 void uiuc_init_aeromodel ()
 185 {
 186   SGPath path(globals->get_fg_root());
 187   path.append(aircraft_dir);
 188   path.append("aircraft.dat");
 189   cout << "We are using "<< path.str() << endl;
 190   uiuc_initializemaps(); // Initialize the <string,int> maps
 191   uiuc_menu(path.str());   // Read the specified aircraft file
 192 }
 193
 194 void uiuc_force_moment(double dt)
 195 {
 196   double qS = Dynamic_pressure * Sw;
 197   double qScbar = qS * cbar;
 198   double qSb = qS * bw;
 199
 200   uiuc_aerodeflections(dt);
 201   uiuc_coefficients(dt);
 202
 203   /* Calculate the forces */
 204   if (CX && CZ)
 205     {
 206       F_X_aero = CX * qS;
 207       F_Y_aero = CY * qS;
 208       F_Z_aero = CZ * qS;
 209     }
 210   else
 211     {
 212       // Cos_beta * Cos_beta corrects V_rel_wind to get normal q onto wing,
 213       // hence Cos_beta * Cos_beta term included.
 214       // Same thing is done w/ moments below.
 215       // Without this "die-off" function, lift would be produced in a 90 deg sideslip, when
 216       // that should not be the case.  See FGFS notes 021105
 217       F_X_wind = -CD * qS  * Cos_beta * Cos_beta;
 218       F_Y_wind =  CY * qS;
 219       F_Z_wind = -CL * qS  * Cos_beta * Cos_beta;
 220       // F_X_wind = -CD * qS  * Cos_beta * Cos_beta;
 221       // F_Y_wind =  CY * qS  * Cos_beta * Cos_beta;
 222       // F_Z_wind = -CL * qS  * Cos_beta * Cos_beta;
 223
 224       // wind-axis to body-axis transformation
 225       F_X_aero = F_X_wind * Cos_alpha * Cos_beta - F_Y_wind * Cos_alpha * Sin_beta - F_Z_wind * Sin_alpha;
 226       F_Y_aero = F_X_wind * Sin_beta + F_Y_wind * Cos_beta;
 227       F_Z_aero = F_X_wind * Sin_alpha * Cos_beta - F_Y_wind * Sin_alpha * Sin_beta + F_Z_wind * Cos_alpha;
 228     }
 229   // Moment calculations
 230   M_l_aero = Cl * qSb    ;
 231   M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
 232   M_n_aero = Cn * qSb    ;
 233   // M_l_aero = Cl * qSb    * Cos_beta * Cos_beta;
 234   // M_m_aero = Cm * qScbar * Cos_beta * Cos_beta;
 235   // M_n_aero = Cn * qSb    * Cos_beta * Cos_beta;
 236
 237   // Adding in apparent mass effects
 238   if (Mass_appMass_ratio)
 239     F_Z_aero += -(Mass_appMass_ratio * Mass) * W_dot_body;
 240   if (I_xx_appMass_ratio)
 241     M_l_aero += -(I_xx_appMass_ratio * I_xx) * P_dot_body;
 242   if (I_yy_appMass_ratio)
 243     M_m_aero += -(I_yy_appMass_ratio * I_yy) * Q_dot_body;
 244   if (I_zz_appMass_ratio)
 245     M_n_aero += -(I_zz_appMass_ratio * I_zz) * R_dot_body;
 246
 247   if (Mass_appMass)
 248     F_Z_aero += -Mass_appMass * W_dot_body;
 249   if (I_xx_appMass)
 250     M_l_aero += -I_xx_appMass * P_dot_body;
 251   if (I_yy_appMass)
 252     M_m_aero += -I_yy_appMass * Q_dot_body;
 253   if (I_zz_appMass)
 254     M_n_aero += -I_zz_appMass * R_dot_body;
 255
 256   // gyroscopic moments
 257   // engineOmega is positive when rotation is ccw when viewed from the front
 258   if (gyroForce_Q_body)
 259     M_n_aero +=  polarInertia * engineOmega * Q_body;
 260   if (gyroForce_R_body)
 261     M_m_aero += -polarInertia * engineOmega * R_body;
 262
 263   // ornithopter support
 264   if (flapper_model)
 265     {
 266       uiuc_get_flapper(dt);
 267       F_X_aero += F_X_aero_flapper;
 268       F_Z_aero += F_Z_aero_flapper;
 269       M_m_aero += flapper_Moment;
 270     }
 271
 272   // fog field update
 273    Fog = 0;
 274    if (fog_field)
 275      uiuc_fog();
 276
 277    double vis;
 278    if (Fog != 0)
 279    {
 280      vis = fgGetDouble("/environment/visibility-m");
 281      if (Fog > 0)
 282        vis /= 1.01;
 283      else
 284        vis *= 1.01;
 285      fgSetDouble("/environment/visibility-m", vis);
 286    }
 287
 288
 289   /* Send data on the network to the Glass Cockpit */
 290
 291    //  string input="";
 292
 293    //  input += " stick_right " + ftoa(Lat_control);
 294    //  input += " rudder_left " + ftoa(-Rudder_pedal);
 295    //  input += " stick_forward " + ftoa(Long_control);
 296    //  input += " stick_trim_forward " + ftoa(Long_trim);
 297    //  input += " vehicle_pitch " + ftoa(Theta * 180.0 / 3.14);
 298    //  input += " vehicle_roll " + ftoa(Phi * 180.0 / 3.14);
 299    //  input += " vehicle_speed " + ftoa(V_rel_wind);
 300    //  input += " throttle_forward " + ftoa(Throttle_pct);
 301    //  input += " altitude " + ftoa(Altitude);
 302    //  input += " climb_rate " + ftoa(-1.0*V_down_rel_ground);
 303
 304    //  testarray.getHello();
 305    //  testarray.sendData(input);
 306
 307   /* End of Networking */
 308
 309 }
 310
 311 void uiuc_wind_routine()
 312 {
 313   uiuc_getwind();
 314 }
 315
 316 void uiuc_engine_routine()
 317 {
 318   uiuc_engine();
 319 }
 320
 321 void uiuc_gear_routine ()
 322 {
 323   uiuc_gear();
 324 }
 325
 326 void uiuc_record_routine(double dt)
 327 {
 328   if (trigger_last_time_step == 0 && trigger_on == 1) {
 329     if (trigger_toggle == 0)
 330       trigger_toggle = 1;
 331     else
 332       trigger_toggle = 0;
 333     trigger_num++;
 334     if (trigger_num % 2 != 0)
 335       trigger_counter++;
 336   }
 337
 338   if (Simtime >= recordStartTime)
 339     uiuc_recorder(dt);
 340
 341   trigger_last_time_step = trigger_on;
 342 }
 343
 344 void uiuc_network_recv_routine()
 345 {
 346   //if (use_uiuc_network)
 347     //uiuc_network(1);
 348 }
 349
 350 void uiuc_network_send_routine()
 351 {
 352   //if (use_uiuc_network)
 353     //uiuc_network(2);
 354 }
 355 //end uiuc_wrapper.cpp