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