1 /***************************************************************************
5 ----------------------------------------------------------------------------
7 FUNCTION: Atmospheric and auxilary relationships for LaRCSim EOM
9 ----------------------------------------------------------------------------
11 MODULE STATUS: developmental
13 ----------------------------------------------------------------------------
15 GENEALOGY: Created 9208026 as part of C-castle simulation project.
17 ----------------------------------------------------------------------------
19 DESIGNED BY: B. Jackson
23 MAINTAINED BY: B. Jackson
25 ----------------------------------------------------------------------------
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.
36 931220 Added calculations for static and total temperatures & pressures,
37 as well as dynamic and impact pressures and calibrated airspeed.
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
42 950207 Changed use of "abs" to "fabs" in calculation of signU. EBJ
44 950228 Fixed bug in calculation of beta_dot. EBJ
46 CURRENT RCS HEADER INFO:
50 Revision 1.1 2002/09/10 01:14:01 curt
53 Revision 1.3 2001/03/24 05:03:12 curt
56 Revision 1.2 2000/10/23 22:34:54 curt
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.
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)
70 MagicCarpet (needs work yet)
71 External (don't know how)
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.
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)
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.
98 -- bus data members now directly assigned to
101 -- bus data members now directly assigned to
102 -- changed V_equiv_kts to V_calibrated_kts
106 -- bus data members now directly assigned to
107 -- implemented the FGInterface virtual setters with JSBSim specific
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
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().
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
132 -- put it in src/FDM since it is a class
134 src/FDM/MagicCarpet.cxx
135 -- bus data members now directly assigned to
138 -- adds LaRCsimIC.hxx and cxx
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
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
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
163 src/FDM/LaRCsim/ls_interface.c
164 src/FDM/LaRCsim/ls_interface.h
165 -- added function ls_set_model_dt()
168 -- eliminated calls that set the NED speeds to body components. They
169 are no longer needed and confuse the new bus.
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.
181 --eliminated call to fgFDMSetGroundElevation, this data is now 'pulled'
182 onto the bus every update()
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.
191 src/Network/garmin.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)
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.
204 Revision 1.1.1.1 1999/06/17 18:07:33 curt
205 Start of 0.7.x branch
207 Revision 1.1.1.1 1999/04/05 21:32:45 curt
208 Start of 0.6.x branch.
210 Revision 1.4 1998/08/24 20:09:26 curt
211 Code optimization tweaks from Norman Vine.
213 Revision 1.3 1998/08/06 12:46:38 curt
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.
220 Revision 1.1 1997/05/29 00:09:54 curt
221 Initial Flight Gear revision.
223 * Revision 1.12 1995/02/28 17:57:16 bjax
224 * Corrected calculation of beta_dot. EBJ
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
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.
234 * Revision 1.9 1994/01/11 18:44:33 bjax
235 * Changed header files to use ls_types, ls_constants, and ls_generic.
237 * Revision 1.8 1993/12/21 14:36:33 bjax
238 * Added calcs of pressures, temps and calibrated airspeeds.
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
244 * Revision 1.6 1993/10/07 18:45:56 bjax
245 * A little cleanup; no significant changes. EBJ
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
251 * Revision 1.4 1993/07/16 18:28:58 bjax
252 * Changed call from atmos_62 to ls_atmos. EBJ
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.
257 * Revision 1.1 92/12/30 13:17:39 bjax
262 ----------------------------------------------------------------------------
264 REFERENCES: [ 1] Shapiro, Ascher H.: "The Dynamics and Thermodynamics
265 of Compressible Fluid Flow", Volume I, The Ronald
268 ----------------------------------------------------------------------------
272 ----------------------------------------------------------------------------
276 ----------------------------------------------------------------------------
280 ----------------------------------------------------------------------------
284 --------------------------------------------------------------------------*/
285 #include "ls_types.h"
286 #include "ls_constants.h"
287 #include "ls_generic.h"
291 #include "atmos_62.h"
292 #include "ls_geodesy.h"
293 #include "ls_gravity.h"
298 void ls_aux( void ) {
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;
308 /* update geodetic position */
310 ls_geoc_to_geod( Lat_geocentric, Radius_to_vehicle,
311 &Latitude, &Altitude, &Sea_level_radius );
312 Longitude = Lon_geocentric - Earth_position_angle;
314 /* Calculate body axis velocities */
316 /* Form relative velocity vector */
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;
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;
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;
337 V_rel_wind = sqrt(U_body*U_body + V_body*V_body + W_body*W_body);
340 /* Calculate alpha and beta rates */
342 v_XZ_plane_2 = (U_body*U_body + W_body*W_body);
347 signU = U_body/fabs(U_body);
349 if( (v_XZ_plane_2 == 0) || (V_rel_wind == 0) )
356 Alpha_dot = (U_body*W_dot_body - W_body*U_dot_body)/
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));
363 /* Calculate flight path and other flight condition values */
368 Alpha = atan2( W_body, U_body );
370 Cos_alpha = cos(Alpha);
371 Sin_alpha = sin(Alpha);
376 Beta = asin( V_body/ V_rel_wind );
378 Cos_beta = cos(Beta);
379 Sin_beta = sin(Beta);
381 V_true_kts = V_rel_wind * V_TO_KNOTS;
383 V_ground_speed = sqrt(V_north_rel_ground*V_north_rel_ground
384 + V_east_rel_ground*V_east_rel_ground );
386 V_rel_ground = sqrt(V_ground_speed*V_ground_speed
387 + V_down_rel_ground*V_down_rel_ground );
389 v_tangential = sqrt(V_north*V_north + V_east*V_east);
391 V_inertial = sqrt(v_tangential*v_tangential + V_down*V_down);
393 if( (V_ground_speed == 0) && (V_down == 0) )
396 Gamma_vert_rad = atan2( -V_down, V_ground_speed );
398 if( (V_north_rel_ground == 0) && (V_east_rel_ground == 0) )
401 Gamma_horiz_rad = atan2( V_east_rel_ground, V_north_rel_ground );
403 if (Gamma_horiz_rad < 0)
404 Gamma_horiz_rad = Gamma_horiz_rad + 2*LS_PI;
406 /* Calculate local gravity */
408 ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
410 /* call function for (smoothed) density ratio, sonic velocity, and
413 ls_atmos(Altitude, &Sigma, &V_sound,
414 &Static_temperature, &Static_pressure);
416 Density = Sigma*SEA_LEVEL_DENSITY;
418 Mach_number = V_rel_wind / V_sound;
420 V_equiv = V_rel_wind*sqrt(Sigma);
422 V_equiv_kts = V_equiv*V_TO_KNOTS;
424 /* calculate temperature and pressure ratios (from [1]) */
426 mach2 = Mach_number*Mach_number;
427 temp_ratio = 1.0 + 0.2*mach2;
429 pres_ratio = pow( temp_ratio, tmp );
431 Total_temperature = temp_ratio*Static_temperature;
432 Total_pressure = pres_ratio*Static_pressure;
434 /* calculate impact and dynamic pressures */
436 Impact_pressure = Total_pressure - Static_pressure;
438 Dynamic_pressure = 0.5*Density*V_rel_wind*V_rel_wind;
440 /* calculate calibrated airspeed indication */
442 V_calibrated = sqrt( 2.0*Dynamic_pressure / SEA_LEVEL_DENSITY );
443 V_calibrated_kts = V_calibrated*V_TO_KNOTS;
445 Centrifugal_relief = 1 - v_tangential/(Radius_to_vehicle*Gravity);
447 /* Determine location in runway coordinates */
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);
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;
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;
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;
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;
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;
487 /* Calculate Euler rates */
491 Sin_theta = sin(Theta);
492 Cos_theta = cos(Theta);
499 Psi_dot = (Q_total*Sin_phi + R_total*Cos_phi)/Cos_theta;
501 Theta_dot = Q_total*Cos_phi - R_total*Sin_phi;
502 Phi_dot = P_total + Psi_dot*Sin_theta;
507 /*************************************************************************/