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