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