src/FDM/LaRCsim/ls_aux.c

   1 /***************************************************************************
   2
   3     TITLE:              ls_aux
   4
   5 ----------------------------------------------------------------------------
   6
   7     FUNCTION:   Atmospheric and auxilary relationships for LaRCSim EOM
   8
   9 ----------------------------------------------------------------------------
  10
  11     MODULE STATUS:      developmental
  12
  13 ----------------------------------------------------------------------------
  14
  15     GENEALOGY:  Created 9208026 as part of C-castle simulation project.
  16
  17 ----------------------------------------------------------------------------
  18
  19     DESIGNED BY:        B. Jackson
  20
  21     CODED BY:           B. Jackson
  22
  23     MAINTAINED BY:      B. Jackson
  24
  25 ----------------------------------------------------------------------------
  26
  27     MODIFICATION HISTORY:
  28
  29     DATE    PURPOSE
  30
  31     931006  Moved calculations of auxiliary accelerations from here
  32             to ls_accel.c and corrected minus sign in front of A_Y_Pilot
  33             contribution from Q_body*P_body*D_X_pilot term.         EBJ
  34     931014  Changed calculation of Alpha from atan to atan2 so sign is correct.
  35                                                                     EBJ
  36     931220  Added calculations for static and total temperatures & pressures,
  37             as well as dynamic and impact pressures and calibrated airspeed.
  38                                                                     EBJ
  39     940111  Changed #included header files from old "ls_eom.h" to newer
  40             "ls_types.h", "ls_constants.h" and "ls_generic.h".      EBJ
  41
  42     950207  Changed use of "abs" to "fabs" in calculation of signU. EBJ
  43
  44     950228  Fixed bug in calculation of beta_dot.                   EBJ
  45
  46     CURRENT RCS HEADER INFO:
  47
  48 $Header$
  49 $Log$
  50 Revision 1.3  2003/05/13 18:45:06  curt
  51 Robert Deters:
  52
  53   I have attached some revisions for the UIUCModel and some LaRCsim.
  54   The only thing you should need to check is LaRCsim.cxx.  The file
  55   I attached is a revised version of 1.5 and the latest is 1.7.  Also,
  56   uiuc_getwind.c and uiuc_getwind.h are no longer in the LaRCsim
  57   directory.  They have been moved over to UIUCModel.
  58
  59 Revision 1.2  2003/03/31 03:05:39  m-selig
  60 uiuc wind changes, MSS
  61
  62 Revision 1.1.1.1  2003/02/28 01:33:39  rob
  63 uiuc version of FlightGear v0.9.0
  64
  65 Revision 1.2  2002/10/22 21:06:49  rob
  66 *** empty log message ***
  67
  68 Revision 1.1.1.1  2002/04/24 17:08:23  rob
  69 UIUC version of FlightGear-0.7.pre11
  70
  71 Revision 1.3  2001/03/24 05:03:12  curt
  72 SG-ified logstream.
  73
  74 Revision 1.2  2000/10/23 22:34:54  curt
  75 I tested:
  76 LaRCsim c172 on-ground and in-air starts, reset: all work
  77 UIUC Cessna172 on-ground and in-air starts work as expected, reset
  78 results in an aircraft that is upside down but does not crash FG.   I
  79 don't know what it was like before, so it may well be no change.
  80 JSBSim c172 and X15 in-air starts work fine, resets now work (and are
  81 trimmed), on-ground starts do not -- the c172 ends up on its back.  I
  82 suspect this is no worse than before.
  83
  84 I did not test:
  85 Balloon (the weather code returns nan's for the atmosphere data --this
  86 is in the weather module and apparently is a linux only bug)
  87 ADA (don't know how)
  88 MagicCarpet  (needs work yet)
  89 External (don't know how)
  90
  91 known to be broken:
  92 LaRCsim c172 on-ground starts with a negative terrain altitude (this
  93 happens at KPAO when the scenery is not present).   The FDM inits to
  94 about 50 feet AGL and the model falls to the ground.  It does stay
  95 upright, however, and seems to be fine once it settles out, FWIW.
  96
  97 To do:
  98 --implement set_Model on the bus
  99 --bring Christian's weather data into JSBSim
 100 -- add default method to bus for updating things like the sin and cos of
 101 latitude (for Balloon, MagicCarpet)
 102 -- lots of cleanup
 103
 104 The files:
 105 src/FDM/flight.cxx
 106 src/FDM/flight.hxx
 107 -- all data members now declared protected instead of private.
 108 -- eliminated all but a small set of 'setters', no change to getters.
 109 -- that small set is declared virtual, the default implementation
 110 provided preserves the old behavior
 111 -- all of the vector data members are now initialized.
 112 -- added busdump() method -- SG_LOG's  all the bus data when called,
 113 useful for diagnostics.
 114
 115 src/FDM/ADA.cxx
 116 -- bus data members now directly assigned to
 117
 118 src/FDM/Balloon.cxx
 119 -- bus data members now directly assigned to
 120 -- changed V_equiv_kts to V_calibrated_kts
 121
 122 src/FDM/JSBSim.cxx
 123 src/FDM/JSBSim.hxx
 124 -- bus data members now directly assigned to
 125 -- implemented the FGInterface virtual setters with JSBSim specific
 126 logic
 127 -- changed the static FDMExec to a dynamic fdmex (needed so that the
 128 JSBSim object can be deleted when a model change is called for)
 129 -- implemented constructor and destructor, moved some of the logic
 130 formerly in init() to constructor
 131 -- added logic to bring up FGEngInterface objects and set the RPM and
 132 throttle values.
 133
 134 src/FDM/LaRCsim.cxx
 135 src/FDM/LaRCsim.hxx
 136 -- bus data members now directly assigned to
 137 -- implemented the FGInterface virtual setters with LaRCsim specific
 138 logic, uses LaRCsimIC
 139 -- implemented constructor and destructor, moved some of the logic
 140 formerly in init() to constructor
 141 -- moved default inertias to here from fg_init.cxx
 142 -- eliminated the climb rate calculation.  The equivalent, climb_rate =
 143 -1*vdown, is now in copy_from_LaRCsim().
 144
 145 src/FDM/LaRCsimIC.cxx
 146 src/FDM/LaRCsimIC.hxx
 147 -- similar to FGInitialCondition, this class has all the logic needed to
 148 turn data like Vc and Mach into the more fundamental quantities LaRCsim
 149 needs to initialize.
 150 -- put it in src/FDM since it is a class
 151
 152 src/FDM/MagicCarpet.cxx
 153  -- bus data members now directly assigned to
 154
 155 src/FDM/Makefile.am
 156 -- adds LaRCsimIC.hxx and cxx
 157
 158 src/FDM/JSBSim/FGAtmosphere.h
 159 src/FDM/JSBSim/FGDefs.h
 160 src/FDM/JSBSim/FGInitialCondition.cpp
 161 src/FDM/JSBSim/FGInitialCondition.h
 162 src/FDM/JSBSim/JSBSim.cpp
 163 -- changes to accomodate the new bus
 164
 165 src/FDM/LaRCsim/atmos_62.h
 166 src/FDM/LaRCsim/ls_geodesy.h
 167 -- surrounded prototypes with #ifdef __cplusplus ... #endif , functions
 168 here are needed in LaRCsimIC
 169
 170 src/FDM/LaRCsim/c172_main.c
 171 src/FDM/LaRCsim/cherokee_aero.c
 172 src/FDM/LaRCsim/ls_aux.c
 173 src/FDM/LaRCsim/ls_constants.h
 174 src/FDM/LaRCsim/ls_geodesy.c
 175 src/FDM/LaRCsim/ls_geodesy.h
 176 src/FDM/LaRCsim/ls_step.c
 177 src/FDM/UIUCModel/uiuc_betaprobe.cpp
 178 -- changed PI to LS_PI, eliminates preprocessor naming conflict with
 179 weather module
 180
 181 src/FDM/LaRCsim/ls_interface.c
 182 src/FDM/LaRCsim/ls_interface.h
 183 -- added function ls_set_model_dt()
 184
 185 src/Main/bfi.cxx
 186 -- eliminated calls that set the NED speeds to body components.  They
 187 are no longer needed and confuse the new bus.
 188
 189 src/Main/fg_init.cxx
 190 -- eliminated calls that just brought the bus data up-to-date (e.g.
 191 set_sin_cos_latitude). or set default values.   The bus now handles the
 192 defaults and updates itself when the setters are called (for LaRCsim and
 193 JSBSim).  A default method for doing this needs to be added to the bus.
 194 -- added fgVelocityInit() to set the speed the user asked for.  Both
 195 JSBSim and LaRCsim can now be initialized using any of:
 196 vc,mach, NED components, UVW components.
 197
 198 src/Main/main.cxx
 199 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
 200 onto the bus every update()
 201
 202 src/Main/options.cxx
 203 src/Main/options.hxx
 204 -- added enum to keep track of the speed requested by the user
 205 -- eliminated calls to set NED velocity properties to body speeds, they
 206 are no longer needed.
 207 -- added options for the NED components.
 208
 209 src/Network/garmin.cxx
 210 src/Network/nmea.cxx
 211 --eliminated calls that just brought the bus data up-to-date (e.g.
 212 set_sin_cos_latitude).  The bus now updates itself when the setters are
 213 called (for LaRCsim and JSBSim).  A default method for doing this needs
 214 to be added to the bus.
 215 -- changed set_V_equiv_kts to set_V_calibrated_kts.  set_V_equiv_kts no
 216 longer exists ( get_V_equiv_kts still does, though)
 217
 218 src/WeatherCM/FGLocalWeatherDatabase.cpp
 219 -- commented out the code to put the weather data on the bus, a
 220 different scheme for this is needed.
 221
 222 Revision 1.1.1.1  1999/06/17 18:07:33  curt
 223 Start of 0.7.x branch
 224
 225 Revision 1.1.1.1  1999/04/05 21:32:45  curt
 226 Start of 0.6.x branch.
 227
 228 Revision 1.4  1998/08/24 20:09:26  curt
 229 Code optimization tweaks from Norman Vine.
 230
 231 Revision 1.3  1998/08/06 12:46:38  curt
 232 Header change.
 233
 234 Revision 1.2  1998/01/19 18:40:24  curt
 235 Tons of little changes to clean up the code and to remove fatal errors
 236 when building with the c++ compiler.
 237
 238 Revision 1.1  1997/05/29 00:09:54  curt
 239 Initial Flight Gear revision.
 240
 241  * Revision 1.12  1995/02/28  17:57:16  bjax
 242  * Corrected calculation of beta_dot. EBJ
 243  *
 244  * Revision 1.11  1995/02/07  21:09:47  bjax
 245  * Corrected calculation of "signU"; was using divide by
 246  * abs(), which returns an integer; now using fabs(), which
 247  * returns a double.  EBJ
 248  *
 249  * Revision 1.10  1994/05/10  20:09:42  bjax
 250  * Fixed a major problem with dx_pilot_from_cg, etc. not being calculated locally.
 251  *
 252  * Revision 1.9  1994/01/11  18:44:33  bjax
 253  * Changed header files to use ls_types, ls_constants, and ls_generic.
 254  *
 255  * Revision 1.8  1993/12/21  14:36:33  bjax
 256  * Added calcs of pressures, temps and calibrated airspeeds.
 257  *
 258  * Revision 1.7  1993/10/14  11:25:38  bjax
 259  * Changed calculation of Alpha to use 'atan2' instead of 'atan' so alphas
 260  * larger than +/- 90 degrees are calculated correctly.                 EBJ
 261  *
 262  * Revision 1.6  1993/10/07  18:45:56  bjax
 263  * A little cleanup; no significant changes. EBJ
 264  *
 265  * Revision 1.5  1993/10/07  18:42:22  bjax
 266  * Moved calculations of auxiliary accelerations here from ls_aux, and
 267  * corrected sign on Q_body*P_body*d_x_pilot term of A_Y_pilot calc.  EBJ
 268  *
 269  * Revision 1.4  1993/07/16  18:28:58  bjax
 270  * Changed call from atmos_62 to ls_atmos. EBJ
 271  *
 272  * Revision 1.3  1993/06/02  15:02:42  bjax
 273  * Changed call to geodesy calcs from ls_geodesy to ls_geoc_to_geod.
 274  *
 275  * Revision 1.1  92/12/30  13:17:39  bjax
 276  * Initial revision
 277  *
 278
 279
 280 ----------------------------------------------------------------------------
 281
 282     REFERENCES: [ 1] Shapiro, Ascher H.: "The Dynamics and Thermodynamics
 283                         of Compressible Fluid Flow", Volume I, The Ronald
 284                         Press, 1953.
 285
 286 ----------------------------------------------------------------------------
 287
 288                 CALLED BY:
 289
 290 ----------------------------------------------------------------------------
 291
 292                 CALLS TO:
 293
 294 ----------------------------------------------------------------------------
 295
 296                 INPUTS:
 297
 298 ----------------------------------------------------------------------------
 299
 300                 OUTPUTS:
 301
 302 --------------------------------------------------------------------------*/
 303 #include "ls_types.h"
 304 #include "ls_constants.h"
 305 #include "ls_generic.h"
 306
 307 #include "ls_aux.h"
 308
 309 #include "atmos_62.h"
 310 #include "ls_geodesy.h"
 311 #include "ls_gravity.h"
 312
 313 #include <math.h>
 314
 315 void ls_aux( void ) {
 316
 317         SCALAR  dx_pilot_from_cg, dy_pilot_from_cg, dz_pilot_from_cg;
 318         /* SCALAR inv_Mass; */
 319         SCALAR  v_XZ_plane_2, signU, v_tangential;
 320         /* SCALAR inv_radius_ratio; */
 321         SCALAR  cos_rwy_hdg, sin_rwy_hdg;
 322         SCALAR  mach2, temp_ratio, pres_ratio;
 323         SCALAR  tmp;
 324
 325     /* update geodetic position */
 326
 327         ls_geoc_to_geod( Lat_geocentric, Radius_to_vehicle,
 328                                 &Latitude, &Altitude, &Sea_level_radius );
 329         Longitude = Lon_geocentric - Earth_position_angle;
 330
 331     /* Calculate body axis velocities */
 332
 333         /* Form relative velocity vector */
 334
 335         V_north_rel_ground = V_north;
 336         V_east_rel_ground  = V_east
 337           - OMEGA_EARTH*Sea_level_radius*cos( Lat_geocentric );
 338         V_down_rel_ground  = V_down;
 339
 340         V_north_rel_airmass = V_north_rel_ground - V_north_airmass;
 341         V_east_rel_airmass  = V_east_rel_ground  - V_east_airmass;
 342         V_down_rel_airmass  = V_down_rel_ground  - V_down_airmass;
 343
 344         U_body = T_local_to_body_11*V_north_rel_airmass
 345           + T_local_to_body_12*V_east_rel_airmass
 346             + T_local_to_body_13*V_down_rel_airmass + U_gust;
 347         V_body = T_local_to_body_21*V_north_rel_airmass
 348           + T_local_to_body_22*V_east_rel_airmass
 349             + T_local_to_body_23*V_down_rel_airmass + V_gust;
 350         W_body = T_local_to_body_31*V_north_rel_airmass
 351           + T_local_to_body_32*V_east_rel_airmass
 352             + T_local_to_body_33*V_down_rel_airmass + W_gust;
 353
 354         V_rel_wind = sqrt(U_body*U_body + V_body*V_body + W_body*W_body);
 355
 356
 357     /* Calculate alpha and beta rates   */
 358
 359         v_XZ_plane_2 = (U_body*U_body + W_body*W_body);
 360
 361         if (U_body == 0)
 362                 signU = 1;
 363         else
 364                 signU = U_body/fabs(U_body);
 365
 366         if( (v_XZ_plane_2 == 0) || (V_rel_wind == 0) )
 367         {
 368                 Alpha_dot = 0;
 369                 Beta_dot = 0;
 370         }
 371         else
 372         {
 373                 Alpha_dot = (U_body*W_dot_body - W_body*U_dot_body)/
 374                   v_XZ_plane_2;
 375                 Beta_dot = (signU*v_XZ_plane_2*V_dot_body
 376                   - V_body*(U_body*U_dot_body + W_body*W_dot_body))
 377                     /(V_rel_wind*V_rel_wind*sqrt(v_XZ_plane_2));
 378         }
 379
 380     /* Calculate flight path and other flight condition values */
 381
 382         if (U_body == 0)
 383                 Alpha = 0;
 384         else
 385                 Alpha = atan2( W_body, U_body );
 386
 387         Cos_alpha = cos(Alpha);
 388         Sin_alpha = sin(Alpha);
 389
 390         if (V_rel_wind == 0)
 391                 Beta = 0;
 392         else
 393                 Beta = asin( V_body/ V_rel_wind );
 394
 395         Cos_beta = cos(Beta);
 396         Sin_beta = sin(Beta);
 397
 398         V_true_kts = V_rel_wind * V_TO_KNOTS;
 399
 400         V_ground_speed = sqrt(V_north_rel_ground*V_north_rel_ground
 401                               + V_east_rel_ground*V_east_rel_ground );
 402
 403         V_rel_ground = sqrt(V_ground_speed*V_ground_speed
 404                             + V_down_rel_ground*V_down_rel_ground );
 405
 406         v_tangential = sqrt(V_north*V_north + V_east*V_east);
 407
 408         V_inertial = sqrt(v_tangential*v_tangential + V_down*V_down);
 409
 410         if( (V_ground_speed == 0) && (V_down == 0) )
 411           Gamma_vert_rad = 0;
 412         else
 413           Gamma_vert_rad = atan2( -V_down, V_ground_speed );
 414
 415         if( (V_north_rel_ground == 0) && (V_east_rel_ground == 0) )
 416           Gamma_horiz_rad = 0;
 417         else
 418           Gamma_horiz_rad = atan2( V_east_rel_ground, V_north_rel_ground );
 419
 420         if (Gamma_horiz_rad < 0)
 421           Gamma_horiz_rad = Gamma_horiz_rad + 2*LS_PI;
 422
 423     /* Calculate local gravity  */
 424
 425         ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
 426
 427     /* call function for (smoothed) density ratio, sonic velocity, and
 428            ambient pressure */
 429
 430         ls_atmos(Altitude, &Sigma, &V_sound,
 431                  &Static_temperature, &Static_pressure);
 432
 433         Density = Sigma*SEA_LEVEL_DENSITY;
 434
 435         Mach_number = V_rel_wind / V_sound;
 436
 437         V_equiv = V_rel_wind*sqrt(Sigma);
 438
 439         V_equiv_kts = V_equiv*V_TO_KNOTS;
 440
 441     /* calculate temperature and pressure ratios (from [1]) */
 442
 443         mach2 = Mach_number*Mach_number;
 444         temp_ratio = 1.0 + 0.2*mach2;
 445         tmp = 3.5;
 446         pres_ratio = pow( temp_ratio, tmp );
 447
 448         Total_temperature = temp_ratio*Static_temperature;
 449         Total_pressure    = pres_ratio*Static_pressure;
 450
 451     /* calculate impact and dynamic pressures */
 452
 453         Impact_pressure = Total_pressure - Static_pressure;
 454
 455         Dynamic_pressure = 0.5*Density*V_rel_wind*V_rel_wind;
 456
 457     /* calculate calibrated airspeed indication */
 458
 459         V_calibrated = sqrt( 2.0*Dynamic_pressure / SEA_LEVEL_DENSITY );
 460         V_calibrated_kts = V_calibrated*V_TO_KNOTS;
 461
 462         Centrifugal_relief = 1 - v_tangential/(Radius_to_vehicle*Gravity);
 463
 464 /* Determine location in runway coordinates */
 465
 466         Radius_to_rwy = Sea_level_radius + Runway_altitude;
 467         cos_rwy_hdg = cos(Runway_heading*DEG_TO_RAD);
 468         sin_rwy_hdg = sin(Runway_heading*DEG_TO_RAD);
 469
 470         D_cg_north_of_rwy = Radius_to_rwy*(Latitude - Runway_latitude);
 471         D_cg_east_of_rwy = Radius_to_rwy*cos(Runway_latitude)
 472                 *(Longitude - Runway_longitude);
 473         D_cg_above_rwy  = Radius_to_vehicle - Radius_to_rwy;
 474
 475         X_cg_rwy = D_cg_north_of_rwy*cos_rwy_hdg
 476           + D_cg_east_of_rwy*sin_rwy_hdg;
 477         Y_cg_rwy =-D_cg_north_of_rwy*sin_rwy_hdg
 478           + D_cg_east_of_rwy*cos_rwy_hdg;
 479         H_cg_rwy = D_cg_above_rwy;
 480
 481         dx_pilot_from_cg = Dx_pilot - Dx_cg;
 482         dy_pilot_from_cg = Dy_pilot - Dy_cg;
 483         dz_pilot_from_cg = Dz_pilot - Dz_cg;
 484
 485         D_pilot_north_of_rwy = D_cg_north_of_rwy
 486           + T_local_to_body_11*dx_pilot_from_cg
 487             + T_local_to_body_21*dy_pilot_from_cg
 488               + T_local_to_body_31*dz_pilot_from_cg;
 489         D_pilot_east_of_rwy  = D_cg_east_of_rwy
 490           + T_local_to_body_12*dx_pilot_from_cg
 491             + T_local_to_body_22*dy_pilot_from_cg
 492               + T_local_to_body_32*dz_pilot_from_cg;
 493         D_pilot_above_rwy    = D_cg_above_rwy
 494           - T_local_to_body_13*dx_pilot_from_cg
 495             - T_local_to_body_23*dy_pilot_from_cg
 496               - T_local_to_body_33*dz_pilot_from_cg;
 497
 498         X_pilot_rwy =  D_pilot_north_of_rwy*cos_rwy_hdg
 499           + D_pilot_east_of_rwy*sin_rwy_hdg;
 500         Y_pilot_rwy = -D_pilot_north_of_rwy*sin_rwy_hdg
 501           + D_pilot_east_of_rwy*cos_rwy_hdg;
 502         H_pilot_rwy =  D_pilot_above_rwy;
 503
 504 /* Calculate Euler rates */
 505
 506         Sin_phi = sin(Phi);
 507         Cos_phi = cos(Phi);
 508         Sin_theta = sin(Theta);
 509         Cos_theta = cos(Theta);
 510         Sin_psi = sin(Psi);
 511         Cos_psi = cos(Psi);
 512
 513         if( Cos_theta == 0 )
 514           Psi_dot = 0;
 515         else
 516           Psi_dot = (Q_total*Sin_phi + R_total*Cos_phi)/Cos_theta;
 517
 518         Theta_dot = Q_total*Cos_phi - R_total*Sin_phi;
 519         Phi_dot = P_total + Psi_dot*Sin_theta;
 520
 521 /* end of ls_aux */
 522
 523 }
 524 /*************************************************************************/