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