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  1999/06/17 18:07:33  curt
  51 Initial revision
  52
  53 Revision 1.1.1.1  1999/04/05 21:32:45  curt
  54 Start of 0.6.x branch.
  55
  56 Revision 1.4  1998/08/24 20:09:26  curt
  57 Code optimization tweaks from Norman Vine.
  58
  59 Revision 1.3  1998/08/06 12:46:38  curt
  60 Header change.
  61
  62 Revision 1.2  1998/01/19 18:40:24  curt
  63 Tons of little changes to clean up the code and to remove fatal errors
  64 when building with the c++ compiler.
  65
  66 Revision 1.1  1997/05/29 00:09:54  curt
  67 Initial Flight Gear revision.
  68
  69  * Revision 1.12  1995/02/28  17:57:16  bjax
  70  * Corrected calculation of beta_dot. EBJ
  71  *
  72  * Revision 1.11  1995/02/07  21:09:47  bjax
  73  * Corrected calculation of "signU"; was using divide by
  74  * abs(), which returns an integer; now using fabs(), which
  75  * returns a double.  EBJ
  76  *
  77  * Revision 1.10  1994/05/10  20:09:42  bjax
  78  * Fixed a major problem with dx_pilot_from_cg, etc. not being calculated locally.
  79  *
  80  * Revision 1.9  1994/01/11  18:44:33  bjax
  81  * Changed header files to use ls_types, ls_constants, and ls_generic.
  82  *
  83  * Revision 1.8  1993/12/21  14:36:33  bjax
  84  * Added calcs of pressures, temps and calibrated airspeeds.
  85  *
  86  * Revision 1.7  1993/10/14  11:25:38  bjax
  87  * Changed calculation of Alpha to use 'atan2' instead of 'atan' so alphas
  88  * larger than +/- 90 degrees are calculated correctly.                 EBJ
  89  *
  90  * Revision 1.6  1993/10/07  18:45:56  bjax
  91  * A little cleanup; no significant changes. EBJ
  92  *
  93  * Revision 1.5  1993/10/07  18:42:22  bjax
  94  * Moved calculations of auxiliary accelerations here from ls_aux, and
  95  * corrected sign on Q_body*P_body*d_x_pilot term of A_Y_pilot calc.  EBJ
  96  *
  97  * Revision 1.4  1993/07/16  18:28:58  bjax
  98  * Changed call from atmos_62 to ls_atmos. EBJ
  99  *
 100  * Revision 1.3  1993/06/02  15:02:42  bjax
 101  * Changed call to geodesy calcs from ls_geodesy to ls_geoc_to_geod.
 102  *
 103  * Revision 1.1  92/12/30  13:17:39  bjax
 104  * Initial revision
 105  *
 106
 107
 108 ----------------------------------------------------------------------------
 109
 110     REFERENCES: [ 1] Shapiro, Ascher H.: "The Dynamics and Thermodynamics
 111                         of Compressible Fluid Flow", Volume I, The Ronald
 112                         Press, 1953.
 113
 114 ----------------------------------------------------------------------------
 115
 116                 CALLED BY:
 117
 118 ----------------------------------------------------------------------------
 119
 120                 CALLS TO:
 121
 122 ----------------------------------------------------------------------------
 123
 124                 INPUTS:
 125
 126 ----------------------------------------------------------------------------
 127
 128                 OUTPUTS:
 129
 130 --------------------------------------------------------------------------*/
 131 #include "ls_types.h"
 132 #include "ls_constants.h"
 133 #include "ls_generic.h"
 134
 135 #include "ls_aux.h"
 136
 137 #include "atmos_62.h"
 138 #include "ls_geodesy.h"
 139 #include "ls_gravity.h"
 140
 141 #include <math.h>
 142
 143
 144 void ls_aux( void ) {
 145
 146         SCALAR  dx_pilot_from_cg, dy_pilot_from_cg, dz_pilot_from_cg;
 147         /* SCALAR inv_Mass; */
 148         SCALAR  v_XZ_plane_2, signU, v_tangential;
 149         /* SCALAR inv_radius_ratio; */
 150         SCALAR  cos_rwy_hdg, sin_rwy_hdg;
 151         SCALAR  mach2, temp_ratio, pres_ratio;
 152         SCALAR  tmp;
 153
 154     /* update geodetic position */
 155
 156         ls_geoc_to_geod( Lat_geocentric, Radius_to_vehicle,
 157                                 &Latitude, &Altitude, &Sea_level_radius );
 158         Longitude = Lon_geocentric - Earth_position_angle;
 159
 160     /* Calculate body axis velocities */
 161
 162         /* Form relative velocity vector */
 163
 164         V_north_rel_ground = V_north;
 165         V_east_rel_ground  = V_east
 166           - OMEGA_EARTH*Sea_level_radius*cos( Lat_geocentric );
 167         V_down_rel_ground  = V_down;
 168
 169         V_north_rel_airmass = V_north_rel_ground - V_north_airmass;
 170         V_east_rel_airmass  = V_east_rel_ground  - V_east_airmass;
 171         V_down_rel_airmass  = V_down_rel_ground  - V_down_airmass;
 172
 173         U_body = T_local_to_body_11*V_north_rel_airmass
 174           + T_local_to_body_12*V_east_rel_airmass
 175             + T_local_to_body_13*V_down_rel_airmass + U_gust;
 176         V_body = T_local_to_body_21*V_north_rel_airmass
 177           + T_local_to_body_22*V_east_rel_airmass
 178             + T_local_to_body_23*V_down_rel_airmass + V_gust;
 179         W_body = T_local_to_body_31*V_north_rel_airmass
 180           + T_local_to_body_32*V_east_rel_airmass
 181             + T_local_to_body_33*V_down_rel_airmass + W_gust;
 182
 183         V_rel_wind = sqrt(U_body*U_body + V_body*V_body + W_body*W_body);
 184
 185
 186     /* Calculate alpha and beta rates   */
 187
 188         v_XZ_plane_2 = (U_body*U_body + W_body*W_body);
 189
 190         if (U_body == 0)
 191                 signU = 1;
 192         else
 193                 signU = U_body/fabs(U_body);
 194
 195         if( (v_XZ_plane_2 == 0) || (V_rel_wind == 0) )
 196         {
 197                 Alpha_dot = 0;
 198                 Beta_dot = 0;
 199         }
 200         else
 201         {
 202                 Alpha_dot = (U_body*W_dot_body - W_body*U_dot_body)/
 203                   v_XZ_plane_2;
 204                 Beta_dot = (signU*v_XZ_plane_2*V_dot_body
 205                   - V_body*(U_body*U_dot_body + W_body*W_dot_body))
 206                     /(V_rel_wind*V_rel_wind*sqrt(v_XZ_plane_2));
 207         }
 208
 209     /* Calculate flight path and other flight condition values */
 210
 211         if (U_body == 0)
 212                 Alpha = 0;
 213         else
 214                 Alpha = atan2( W_body, U_body );
 215
 216         Cos_alpha = cos(Alpha);
 217         Sin_alpha = sin(Alpha);
 218
 219         if (V_rel_wind == 0)
 220                 Beta = 0;
 221         else
 222                 Beta = asin( V_body/ V_rel_wind );
 223
 224         Cos_beta = cos(Beta);
 225         Sin_beta = sin(Beta);
 226
 227         V_true_kts = V_rel_wind * V_TO_KNOTS;
 228
 229         V_ground_speed = sqrt(V_north_rel_ground*V_north_rel_ground
 230                               + V_east_rel_ground*V_east_rel_ground );
 231
 232         V_rel_ground = sqrt(V_ground_speed*V_ground_speed
 233                             + V_down_rel_ground*V_down_rel_ground );
 234
 235         v_tangential = sqrt(V_north*V_north + V_east*V_east);
 236
 237         V_inertial = sqrt(v_tangential*v_tangential + V_down*V_down);
 238
 239         if( (V_ground_speed == 0) && (V_down == 0) )
 240           Gamma_vert_rad = 0;
 241         else
 242           Gamma_vert_rad = atan2( -V_down, V_ground_speed );
 243
 244         if( (V_north_rel_ground == 0) && (V_east_rel_ground == 0) )
 245           Gamma_horiz_rad = 0;
 246         else
 247           Gamma_horiz_rad = atan2( V_east_rel_ground, V_north_rel_ground );
 248
 249         if (Gamma_horiz_rad < 0)
 250           Gamma_horiz_rad = Gamma_horiz_rad + 2*PI;
 251
 252     /* Calculate local gravity  */
 253
 254         ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
 255
 256     /* call function for (smoothed) density ratio, sonic velocity, and
 257            ambient pressure */
 258
 259         ls_atmos(Altitude, &Sigma, &V_sound,
 260                  &Static_temperature, &Static_pressure);
 261
 262         Density = Sigma*SEA_LEVEL_DENSITY;
 263
 264         Mach_number = V_rel_wind / V_sound;
 265
 266         V_equiv = V_rel_wind*sqrt(Sigma);
 267
 268         V_equiv_kts = V_equiv*V_TO_KNOTS;
 269
 270     /* calculate temperature and pressure ratios (from [1]) */
 271
 272         mach2 = Mach_number*Mach_number;
 273         temp_ratio = 1.0 + 0.2*mach2;
 274         tmp = 3.5;
 275         pres_ratio = pow( temp_ratio, tmp );
 276
 277         Total_temperature = temp_ratio*Static_temperature;
 278         Total_pressure    = pres_ratio*Static_pressure;
 279
 280     /* calculate impact and dynamic pressures */
 281
 282         Impact_pressure = Total_pressure - Static_pressure;
 283
 284         Dynamic_pressure = 0.5*Density*V_rel_wind*V_rel_wind;
 285
 286     /* calculate calibrated airspeed indication */
 287
 288         V_calibrated = sqrt( 2.0*Dynamic_pressure / SEA_LEVEL_DENSITY );
 289         V_calibrated_kts = V_calibrated*V_TO_KNOTS;
 290
 291         Centrifugal_relief = 1 - v_tangential/(Radius_to_vehicle*Gravity);
 292
 293 /* Determine location in runway coordinates */
 294
 295         Radius_to_rwy = Sea_level_radius + Runway_altitude;
 296         cos_rwy_hdg = cos(Runway_heading*DEG_TO_RAD);
 297         sin_rwy_hdg = sin(Runway_heading*DEG_TO_RAD);
 298
 299         D_cg_north_of_rwy = Radius_to_rwy*(Latitude - Runway_latitude);
 300         D_cg_east_of_rwy = Radius_to_rwy*cos(Runway_latitude)
 301                 *(Longitude - Runway_longitude);
 302         D_cg_above_rwy  = Radius_to_vehicle - Radius_to_rwy;
 303
 304         X_cg_rwy = D_cg_north_of_rwy*cos_rwy_hdg
 305           + D_cg_east_of_rwy*sin_rwy_hdg;
 306         Y_cg_rwy =-D_cg_north_of_rwy*sin_rwy_hdg
 307           + D_cg_east_of_rwy*cos_rwy_hdg;
 308         H_cg_rwy = D_cg_above_rwy;
 309
 310         dx_pilot_from_cg = Dx_pilot - Dx_cg;
 311         dy_pilot_from_cg = Dy_pilot - Dy_cg;
 312         dz_pilot_from_cg = Dz_pilot - Dz_cg;
 313
 314         D_pilot_north_of_rwy = D_cg_north_of_rwy
 315           + T_local_to_body_11*dx_pilot_from_cg
 316             + T_local_to_body_21*dy_pilot_from_cg
 317               + T_local_to_body_31*dz_pilot_from_cg;
 318         D_pilot_east_of_rwy  = D_cg_east_of_rwy
 319           + T_local_to_body_12*dx_pilot_from_cg
 320             + T_local_to_body_22*dy_pilot_from_cg
 321               + T_local_to_body_32*dz_pilot_from_cg;
 322         D_pilot_above_rwy    = D_cg_above_rwy
 323           - T_local_to_body_13*dx_pilot_from_cg
 324             - T_local_to_body_23*dy_pilot_from_cg
 325               - T_local_to_body_33*dz_pilot_from_cg;
 326
 327         X_pilot_rwy =  D_pilot_north_of_rwy*cos_rwy_hdg
 328           + D_pilot_east_of_rwy*sin_rwy_hdg;
 329         Y_pilot_rwy = -D_pilot_north_of_rwy*sin_rwy_hdg
 330           + D_pilot_east_of_rwy*cos_rwy_hdg;
 331         H_pilot_rwy =  D_pilot_above_rwy;
 332
 333 /* Calculate Euler rates */
 334
 335         Sin_phi = sin(Phi);
 336         Cos_phi = cos(Phi);
 337         Sin_theta = sin(Theta);
 338         Cos_theta = cos(Theta);
 339         Sin_psi = sin(Psi);
 340         Cos_psi = cos(Psi);
 341
 342         if( Cos_theta == 0 )
 343           Psi_dot = 0;
 344         else
 345           Psi_dot = (Q_total*Sin_phi + R_total*Cos_phi)/Cos_theta;
 346
 347         Theta_dot = Q_total*Cos_phi - R_total*Sin_phi;
 348         Phi_dot = P_total + Psi_dot*Sin_theta;
 349
 350 /* end of ls_aux */
 351
 352 }
 353 /*************************************************************************/