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